diff --git a/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp b/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp index 97cb0ea492..3dd1b1fd7b 100644 --- a/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp +++ b/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp @@ -137,7 +137,13 @@ void SensorsData::init(servConfigImu_t &servCfg, string error_string) { sensorInfo_t newSensor; newSensor.name = servCfg.id[i]; - newSensor.framename = newSensor.name; + if (servCfg.sensorName.empty()) { + newSensor.sensorName = newSensor.name; + } + else { + newSensor.sensorName = servCfg.sensorName; + } + newSensor.framename = newSensor.sensorName; if(des->typeofsensor == eoas_imu_qua) newSensor.values.resize(4); else @@ -182,7 +188,7 @@ bool SensorsData::getSensorName(size_t sens_index, eOas_sensor_t type, std::stri try { std::lock_guard lck (mutex); - name = mysens[type].at(sens_index).name; + name = mysens[type].at(sens_index).sensorName; return true; } catch (const std::out_of_range& oor) diff --git a/src/libraries/icubmod/embObjIMU/eo_imu_privData.h b/src/libraries/icubmod/embObjIMU/eo_imu_privData.h index 53640cd2cf..3e435b5287 100644 --- a/src/libraries/icubmod/embObjIMU/eo_imu_privData.h +++ b/src/libraries/icubmod/embObjIMU/eo_imu_privData.h @@ -49,6 +49,7 @@ typedef struct yarp::sig::Vector values; uint8_t state; double timestamp; + std::string sensorName; } sensorInfo_t; class yarp::dev::SensorsData diff --git a/src/libraries/icubmod/embObjLib/serviceParser.cpp b/src/libraries/icubmod/embObjLib/serviceParser.cpp index e58886f9c8..3a87e0aeca 100755 --- a/src/libraries/icubmod/embObjLib/serviceParser.cpp +++ b/src/libraries/icubmod/embObjLib/serviceParser.cpp @@ -469,7 +469,7 @@ bool ServiceParser::convert(std::string const &fromstring, eObrd_location_t &loc { //Add here parsing for local port (both PWM port (P7, P8, etc and "index_proximal"for hand)) //in xml file we have: LOC:P7 or LOC:index_proximal - + } else { @@ -744,7 +744,7 @@ bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type) Bottle b_PROPERTIES_SENSORS_boardType; if((eomn_serv_AS_inertials3 == type) || (eomn_serv_AS_pos == type)) { - + b_PROPERTIES_SENSORS_boardType = Bottle(b_PROPERTIES_SENSORS.findGroup("boardType")); if(b_PROPERTIES_SENSORS_boardType.isNull()) { @@ -874,7 +874,7 @@ bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type) { // then we need to parse four values: type, rotation, offset, invertDirection - // type + // type if(false == b_PROPERTIES_SENSORS_pos_CALIBRATION_type.isNull()) { parse_POS_CALIB_type(b_PROPERTIES_SENSORS_pos_CALIBRATION_type.get(i+1).asString(), item.pos.calibration.type, formaterror); @@ -939,6 +939,15 @@ bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type) yError() << "ServiceParser::check_analog() cannot find SETTINGS.enabledSensors"; return false; } + Bottle b_SETTINGS_sensorName = Bottle(b_SETTINGS.findGroup("sensorName")); + if(b_SETTINGS_sensorName.isNull()) + { + yWarning() << "ServiceParser::check_analog() cannot find SETTINGS.sensorName"; + } + else + { + convert(b_SETTINGS_sensorName.get(1).asString(), as_service.settings.sensorName, formaterror); + } size_t s = b_SETTINGS_enabledSensors.size(); size_t numenabledsensors = (0 == s) ? (0) : (s - 1) ; // first position of bottle contains the tag "enabledSensors" @@ -1465,7 +1474,6 @@ bool ServiceParser::parseService(Searchable &config, servConfigMais_t &maisconfi maisconfig.ethservice.configuration.data.as.mais.canloc.addr = themais_sensor.location.can.addr; maisconfig.ethservice.configuration.data.as.mais.canloc.insideindex = eobrd_caninsideindex_none; - return true; } @@ -1522,30 +1530,30 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon yError() << "ServiceParser::parseService() has received an invalid SERVICE group for strain"; return false; } - + // now we extract values ... so far we dont make many checks ... we just assume the vector<> are of size 1. servCanBoard_t thestrain_props = as_service.properties.canboards.at(0); servAnalogSensor_t thestrain_sensor = as_service.settings.enabledsensors.at(0); - + // first check we do is about thestrain_props.type if((eobrd_cantype_strain2 != thestrain_props.type) && (eobrd_cantype_strain2c != thestrain_props.type)) { yError() << "ServiceParser::parseService() for embObjFTsensor has detected an invalid type of board. it should be a eobrd_strain2 or a eobrd_strain2c but is a:" << eoboards_type2string2(eoboards_cantype2type(thestrain_props.type), eobool_false); return false; } - + ftconfig.acquisitionrate = as_service.settings.acquisitionrate; ftconfig.useCalibration = as_strain_settings.useCalibration; ftconfig.nameOfStrain = thestrain_sensor.id; ftconfig.frameName = thestrain_sensor.frameName; - + memset(&ftconfig.ethservice.configuration, 0, sizeof(ftconfig.ethservice.configuration)); - + ftconfig.ethservice.configuration.type = eomn_serv_AS_strain; ftconfig.ethservice.configuration.data.as.strain.boardtype.type = thestrain_props.type; memcpy(&ftconfig.ethservice.configuration.data.as.strain.boardtype.protocol, &thestrain_props.protocol, sizeof(eObrd_protocolversion_t)); memcpy(&ftconfig.ethservice.configuration.data.as.strain.boardtype.firmware, &thestrain_props.firmware, sizeof(eObrd_firmwareversion_t)); - + // second check we do is about thestrain_sensor.location if(eobrd_place_can != thestrain_sensor.location.any.place) { @@ -1557,7 +1565,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon ftconfig.ethservice.configuration.data.as.strain.canloc.insideindex = eobrd_caninsideindex_none; - + Bottle b_SERVICE(config.findGroup("SERVICE")); //b_SERVICE and b_SETTINGS could not be null, otherwise parseService function would have returned false Bottle b_SETTINGS = Bottle(b_SERVICE.findGroup("SETTINGS")); Bottle b_SETTINGS_temp = Bottle(b_SETTINGS.findGroup("temperature-acquisitionRate")); @@ -1571,7 +1579,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon ftconfig.temperatureAcquisitionrate = b_SETTINGS_temp.get(1).asInt32(); //TODO: chek that the acquisition rate is inside a reasonable range } - + return true; } @@ -1642,10 +1650,10 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig) yError() << "ServiceParser::parseService(IMU) has received an invalid SERVICE group for IMU"; return false; } - - + + //check the num of type of boards. At max we have 4 board type (see eOas_inertials3_boardinfos_maxnumber) - + if(as_service.properties.canboards.size() > eOas_inertials3_boardinfos_maxnumber) { yError() << "ServiceParser::parseService(IMU): too many type board info are configured. The max num is " << eOas_inertials3_boardinfos_maxnumber; @@ -1654,18 +1662,20 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig) //reset configuration service memset(&imuconfig.ethservice.configuration, 0, sizeof(imuconfig.ethservice.configuration)); - + //set type of service imuconfig.ethservice.configuration.type = eomn_serv_AS_inertials3; - - - //get acquisition rate + + + //get acquisition rate imuconfig.acquisitionrate = as_service.settings.acquisitionrate; - + //get sensor name + imuconfig.sensorName = as_service.settings.sensorName; + //get enabled sensor and fill canboard array. Note that we get only the enabled sensor, not all configured sensors !!! - + imuconfig.inertials.resize(0); - + eOas_inertial3_setof_boardinfos_t * boardInfoSet_ptr = &imuconfig.ethservice.configuration.data.as.inertial3.setofboardinfos; eOresult_t res = eoas_inertial3_setof_boardinfos_clear(boardInfoSet_ptr); if(res != eores_OK) @@ -1673,7 +1683,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig) yError() << "ServiceParser::parseService(IMU). Error in eoas_inertial3_setof_boardinfos_clear()"; return false; } - + EOarray* array = eo_array_New(eOas_inertials3_descriptors_maxnumber, sizeof(eOas_inertial3_descriptor_t), &imuconfig.ethservice.configuration.data.as.inertial3.arrayofdescriptor); for(size_t i=0; i inertials; //TODO to remove because information is already stored! std::vector id; imuConvFactors_t convFactors; + std::string sensorName{""}; } servConfigImu_t; @@ -172,8 +173,9 @@ typedef struct typedef struct { - uint16_t acquisitionrate; + uint16_t acquisitionrate; std::vector enabledsensors; + std::string sensorName{""}; } servASsettings_t; @@ -379,7 +381,7 @@ class ServiceParser bool copyjomocouplingInfo(eOmc_4jomo_coupling_t *jc_dest); - + // suggestion: split check_motion() in sub-methods which parse the groups ... };