From 8d0e0057566e4fba1f29b786a17b7d00212383db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2=20Genesio=20=28=E4=BA=8C=E3=82=B3=E3=82=B2?= =?UTF-8?q?=E3=83=8D=29?= Date: Tue, 31 Jan 2023 18:27:24 +0100 Subject: [PATCH] embObjIMU: add sensorName param (#853) --- .../icubmod/embObjIMU/eo_imu_privData.cpp | 7 +- .../icubmod/embObjLib/serviceParser.cpp | 84 +++++++++++-------- .../icubmod/embObjLib/serviceParser.h | 6 +- 3 files changed, 57 insertions(+), 40 deletions(-) diff --git a/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp b/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp index 97cb0ea492..2ed436525e 100644 --- a/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp +++ b/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp @@ -136,7 +136,12 @@ void SensorsData::init(servConfigImu_t &servCfg, string error_string) if(des->typeofsensor < eoas_sensors_numberof) { sensorInfo_t newSensor; - newSensor.name = servCfg.id[i]; + if (servCfg.sensorName[i].empty()) { + newSensor.name = servCfg.id[i]; + } + else { + newSensor.name = servCfg.sensorName[i]; + } newSensor.framename = newSensor.name; if(des->typeofsensor == eoas_imu_qua) newSensor.values.resize(4); diff --git a/src/libraries/icubmod/embObjLib/serviceParser.cpp b/src/libraries/icubmod/embObjLib/serviceParser.cpp index e58886f9c8..ecb996f01b 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 { @@ -741,10 +741,15 @@ bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type) { yWarning() << "ServiceParser::check_analog() cannot find PROPERTIES.SENSORS.framename"; } + Bottle b_PROPERTIES_SENSORS_sensorName = Bottle(b_PROPERTIES_SENSORS.findGroup("sensorName")); + if(b_PROPERTIES_SENSORS_sensorName.isNull()) + { + yWarning() << "ServiceParser::check_analog() cannot find PROPERTIES.SENSORS.sensorName"; + } 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()) { @@ -854,6 +859,10 @@ bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type) { convert(b_PROPERTIES_SENSORS_frameName.get(i+1).asString(), item.frameName, formaterror); } + if(!b_PROPERTIES_SENSORS_sensorName.isNull()) + { + convert(b_PROPERTIES_SENSORS_sensorName.get(i+1).asString(), item.sensorName, formaterror); + } if((eomn_serv_AS_inertials3 == type) || (eomn_serv_AS_pos == type)) { convert(b_PROPERTIES_SENSORS_boardType.get(i+1).asString(), item.boardtype, formaterror); @@ -874,7 +883,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); @@ -1522,30 +1531,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 +1566,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 +1580,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 +1651,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 +1663,18 @@ 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 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 +1682,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; + std::vector sensorName; imuConvFactors_t convFactors; } servConfigImu_t; @@ -154,11 +155,12 @@ struct servAnalogSensor_t eObrd_location_t location {{eobrd_place_none, 0}}; eObrd_type_t boardtype {eobrd_none}; std::string frameName {""}; + std::string sensorName {""}; servAnalogPOSspecific_t pos {}; void clear() { id.clear(); type = eoas_none; location.any.place = eobrd_place_none; - boardtype = eobrd_none; frameName.clear(); pos.clear(); + boardtype = eobrd_none; frameName.clear(); sensorName.clear(); pos.clear(); } }; @@ -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 ... };