Skip to content

Commit

Permalink
embObjIMU: add sensorName param (#853)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nicogene
Nicogene authored Jan 31, 2023
1 parent 299a481 commit 8d0e005
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 40 deletions.
7 changes: 6 additions & 1 deletion src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
84 changes: 47 additions & 37 deletions src/libraries/icubmod/embObjLib/serviceParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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())
{
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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)
{
Expand All @@ -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"));
Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
Expand All @@ -1654,45 +1663,45 @@ 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)
{
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<as_service.settings.enabledsensors.size(); i++)
{
servAnalogSensor_t sensor = as_service.settings.enabledsensors.at(i);
eOas_sensor_t type = sensor.type;

//TODO: temperature???
if( (eoas_imu_acc != type) && (eoas_imu_mag != type) && (eoas_imu_gyr != type) && (eoas_imu_eul != type) &&
if( (eoas_imu_acc != type) && (eoas_imu_mag != type) && (eoas_imu_gyr != type) && (eoas_imu_eul != type) &&
(eoas_imu_qua != type) && (eoas_imu_lia != type) && (eoas_imu_grv != type) && (eoas_imu_status != type) )
{
yWarning() << "ServiceParser::parseService() has detected a wrong inertial sensor:" << eoas_sensor2string(type) << " ... we drop it";
continue;
}
// if ok, i copy it inside ...

eOas_inertial3_descriptor_t des = {};
des.typeofsensor = type;
memcpy(&des.on, &sensor.location, sizeof(eObrd_location_t));

const eObrd_info_t *boardInfo_ptr = eoas_inertial3_setof_boardinfos_find(boardInfoSet_ptr, eoboards_type2cantype(sensor.boardtype));
if(nullptr == boardInfo_ptr)//if I did not already insert the borad info with type == sensor.boardtype, now I insert it
{
Expand Down Expand Up @@ -1724,13 +1733,14 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig)
}
}
des.typeofboard = sensor.boardtype;

eo_array_PushBack(array, &des);
imuconfig.inertials.push_back(des);
imuconfig.id.push_back(sensor.id);
imuconfig.sensorName.push_back(sensor.sensorName);
}


return true;
}

Expand Down Expand Up @@ -2325,12 +2335,12 @@ bool ServiceParser::parse_actuator_port(std::string const &fromstring, eObrd_eth
ret = false;
}
}

if(false == ret)
{
return ret;
}

if(eomc_act_foc == type)
{
// copy into todes.foc
Expand Down Expand Up @@ -2687,7 +2697,7 @@ bool ServiceParser::convert(std::string const &fromstring, eOmc_encoder_t &toenc
bool ServiceParser::check_motion(Searchable &config)
{
bool formaterror = false;

// complete format is SERVICE{ type, PROPERTIES{ ETHBOARD, CANBOARDS, MC4, MAIS, CONTROLLER, JOINTMAPPING, JOINTSETS } }
// so far, there is no SETTINGS{}.
// then, some groups are not present for some kind of SERVICE.type
Expand Down Expand Up @@ -3946,7 +3956,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
mcconfig.ethservice.configuration.type = mc_service.type;
mcconfig.ethservice.configuration.diagnosticsmode = mc_service.diagconfig.mode;
mcconfig.ethservice.configuration.diagnosticsparam = mc_service.diagconfig.par16;

switch(mc_service.type)
{
case eomn_serv_MC_foc:
Expand Down Expand Up @@ -3975,7 +3985,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
jomodes.actuator.foc.canloc.port = mc_service.properties.actuators[i].desc.foc.canloc.port;
jomodes.actuator.foc.canloc.addr = mc_service.properties.actuators[i].desc.foc.canloc.addr;
jomodes.actuator.foc.canloc.insideindex = eobrd_caninsideindex_first;

// 2. encoder1 is ...
jomodes.encoder1.type = mc_service.properties.encoder1s[i].desc.type;
jomodes.encoder1.port = mc_service.properties.encoder1s[i].desc.port;
Expand All @@ -4000,7 +4010,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
ret = true;

} break;

case eomn_serv_MC_mc4:
{
eOmn_serv_config_data_mc_mc4_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4_based);
Expand Down Expand Up @@ -4063,7 +4073,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
ret = true;

} break;

case eomn_serv_MC_mc4plus:
{
eOmn_serv_config_data_mc_mc4plus_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4plus_based);
Expand Down Expand Up @@ -4102,7 +4112,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
ret = true;

} break;

case eomn_serv_MC_mc4plusmais:
{
eOmn_serv_config_data_mc_mc4plusmais_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4plusmais_based);
Expand Down Expand Up @@ -4362,7 +4372,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
ret = false;
} break;
}


return ret;
}
Expand Down
6 changes: 4 additions & 2 deletions src/libraries/icubmod/embObjLib/serviceParser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@ typedef struct
int acquisitionrate;
std::vector<eOas_inertial3_descriptor_t> inertials; //TODO to remove because information is already stored!
std::vector<std::string> id;
std::vector<std::string> sensorName;
imuConvFactors_t convFactors;
} servConfigImu_t;

Expand Down Expand Up @@ -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();
}
};

Expand Down Expand Up @@ -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 ...
};

Expand Down

0 comments on commit 8d0e005

Please sign in to comment.