From 4b207bb1f92de54851c8fe8ebdd7763ffae30a07 Mon Sep 17 00:00:00 2001 From: "marco.accame" Date: Wed, 16 Dec 2020 22:45:40 +0100 Subject: [PATCH 1/2] embObjMotionControl: added support for the parsing of eomn_serv_MC_mc4plusfaps --- .../icubmod/embObjLib/serviceParser.cpp | 242 +++++++++++++++++- .../icubmod/embObjLib/serviceParser.h | 18 +- .../embObjMotionControl.cpp | 4 +- 3 files changed, 248 insertions(+), 16 deletions(-) diff --git a/src/libraries/icubmod/embObjLib/serviceParser.cpp b/src/libraries/icubmod/embObjLib/serviceParser.cpp index a3dbce68da..a8e621ac82 100755 --- a/src/libraries/icubmod/embObjLib/serviceParser.cpp +++ b/src/libraries/icubmod/embObjLib/serviceParser.cpp @@ -385,7 +385,7 @@ bool ServiceParser::convert(std::string const &fromstring, eObrd_location_t &loc if(len > 15) { - yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eObrd_location_t because it is too long with size =" << len; + yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eObrd_location_t because it is too long with size = " << len; formaterror = true; return false; } @@ -1767,8 +1767,8 @@ bool ServiceParser::parseService(Searchable &config, servConfigPOS_t &posconfig) return true; } -#if defined(SERVICE_PARSER_USE_MC) +#if defined(SERVICE_PARSER_USE_MC) bool ServiceParser::parse_encoder_port(std::string const &fromstring, eObrd_ethtype_t const ethboard, eOmc_encoder_t type, uint8_t &toport, bool &formaterror) @@ -1835,6 +1835,7 @@ bool ServiceParser::parse_encoder_port(std::string const &fromstring, eObrd_etht } } break; + case eomc_enc_psc: { uint8_t toport1 = eobrd_port_unknown; @@ -1853,6 +1854,26 @@ bool ServiceParser::parse_encoder_port(std::string const &fromstring, eObrd_etht } } break; + + case eomc_enc_pos: + { + uint8_t toport1 = eobrd_port_unknown; + bool result = parse_port_pos(fromstring, toport1, formaterror); + + if(false == result) + { + yWarning() << "ServiceParser::parse_encoder_port():" << t << "is not a legal string for an encoder connector port"; + formaterror = true; + ret = false; + } + else + { + toport = toport1; + ret = true; + } + + } break; + default: { int len = strlen(t); @@ -1968,6 +1989,32 @@ bool ServiceParser::parse_port_psc(std::string const &fromstring, uint8_t &topor return ret; } + +bool ServiceParser::parse_port_pos(std::string const &fromstring, uint8_t &toport, bool &formaterror) +{ + const char *t = fromstring.c_str(); + bool ret = false; + + // format of string is POS:hand_thumb or :hand_index or :hand_medium or :hand_pinky + eObrd_portpos_t ppos = eobrd_portpos_unknown; + bool result = parse_pos(fromstring, ppos, formaterror); + + if(false == result) + { + yWarning() << "ServiceParser::parse_port_pos():" << t << "is not a legal string for a port pos"; + formaterror = true; + ret = false; + } + else + { + toport = ppos; + ret = true; + } + + return ret; +} + + //bool ServiceParser::parse_mais(std::string const &fromstring, eObrd_portmais_t &pmais, bool &formaterror) //{ // // parses MAIS:eobrd_portmais_thumbproximal or MAIS:thumbproximal @@ -2276,9 +2323,9 @@ bool ServiceParser::parse_psc(const std::string &fromstring, eObrd_portpsc_t &to const char *t=nullptr; if(0 != strcmp(prefix, "PSC:")) - t = &tt[0]; //port of type finger0 + t = &tt[0]; // port of type finger0 else - t = &tt[4]; //port of type PSC:finger0 + t = &tt[4]; // port of type PSC:finger0 eObool_t usecompactstring = eobool_false; toportpsc = eoboards_string2portpsc(t, usecompactstring); @@ -2291,7 +2338,42 @@ bool ServiceParser::parse_psc(const std::string &fromstring, eObrd_portpsc_t &to if(eobrd_portpsc_unknown == toportpsc) { - yWarning() << "ServiceParser::parse_psc():" << t << "is not a legal string for eObrd_portpsc_t"; + yWarning() << "ServiceParser::parse_psc(): " << t << " is not a legal string for eObrd_portpsc_t"; + formaterror = true; + return false; + } + + return true; +} + + +bool ServiceParser::parse_pos(const std::string &fromstring, eObrd_portpos_t &toportpos, bool &formaterror) +{ + // parses POS:hand_thumb or :hand_index or :hand_medium or :hand_pinky + + const char *tt = fromstring.c_str(); + char prefix[16] = {0}; + sscanf(tt, "%4c", prefix); + + const char *t=nullptr; + + if(0 != strcmp(prefix, "POS:")) + t = &tt[0]; // port of type hand_thumb + else + t = &tt[4]; // port of type POS:hand_thumb + + eObool_t usecompactstring = eobool_false; + toportpos = eoboards_string2portpos(t, usecompactstring); + + if(eobrd_portpos_unknown == toportpos) + { // attempting to retrieve the compact form + usecompactstring = eobool_true; + toportpos = eoboards_string2portpos(t, usecompactstring); + } + + if(eobrd_portpos_unknown == toportpos) + { + yWarning() << "ServiceParser::parse_pos(): " << t << " is not a legal string for eObrd_portpos_t"; formaterror = true; return false; } @@ -2299,6 +2381,7 @@ bool ServiceParser::parse_psc(const std::string &fromstring, eObrd_portpsc_t &to return true; } + bool ServiceParser::convert(std::string const &fromstring, eOmc_encoder_t &toencodertype, bool &formaterror) { const char *t = fromstring.c_str(); @@ -2405,6 +2488,9 @@ bool ServiceParser::check_motion(Searchable &config) Bottle b_PROPERTIES_PSC = Bottle(b_PROPERTIES.findGroup("PSC")); bool has_PROPERTIES_PSC = !b_PROPERTIES_PSC.isNull(); + Bottle b_PROPERTIES_POS = Bottle(b_PROPERTIES.findGroup("POS")); + bool has_PROPERTIES_POS = !b_PROPERTIES_POS.isNull(); + Bottle b_PROPERTIES_MC4 = Bottle(b_PROPERTIES.findGroup("MC4")); bool has_PROPERTIES_MC4 = !b_PROPERTIES_MC4.isNull(); @@ -2551,7 +2637,41 @@ bool ServiceParser::check_motion(Searchable &config) if(false == has_PROPERTIES_PSC) { - yError() << "ServiceParser::check_motion() cannot find PROPERTIES.MAIS for type" << eomn_servicetype2string(mc_service.type); + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.PSC for type" << eomn_servicetype2string(mc_service.type); + itisoksofar = false; + } + + if(false == has_PROPERTIES_JOINTMAPPING) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.JOINTMAPPING for type" << eomn_servicetype2string(mc_service.type); + itisoksofar = false; + } + + + } break; + + + case eomn_serv_MC_mc4plusfaps: + { + // must have: ETHBOARD, CANBOARDS, POS, JOINTMAPPING + + itisoksofar = true; + + if(false == has_PROPERTIES_ETHBOARD) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.ETHBOARD for type" << eomn_servicetype2string(mc_service.type); + itisoksofar = false; + } + + if(false == has_PROPERTIES_CANBOARDS) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.CANBOARDS for type" << eomn_servicetype2string(mc_service.type); + itisoksofar = false; + } + + if(false == has_PROPERTIES_POS) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.POS for type" << eomn_servicetype2string(mc_service.type); itisoksofar = false; } @@ -2995,6 +3115,62 @@ bool ServiceParser::check_motion(Searchable &config) } // has_PROPERTIES_PSC + if(true == has_PROPERTIES_POS) + { + // i get .location and nothing else + + Bottle b_PROPERTIES_POS_location = b_PROPERTIES_POS.findGroup("location"); + if(b_PROPERTIES_POS_location.isNull()) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.POS.location"; + return false; + } + + int tmp = b_PROPERTIES_POS_location.size(); + int numboards = tmp - 1; // first position of bottle contains the tag "location" + + // check if numboards is 1. + constexpr uint8_t wantedboards = 1; + if(wantedboards != numboards) + { + yError() << "ServiceParser::check_motion() in PROPERTIES.POS.location must contain " << wantedboards << " items and it has:" << numboards; + return false; + } + + mc_service.properties.poslocations.resize(wantedboards); + for(int i=0; ipos + eOmn_serv_config_data_as_pos_t *pos = &data_mc->pos; + + + for(size_t i=0; iboardInfo.canloc[i].port = mc_service.properties.poslocations[i].port; + pos->boardInfo.canloc[i].addr = mc_service.properties.poslocations[i].addr; + pos->boardInfo.canloc[i].insideindex = mc_service.properties.poslocations[i].insideindex; + } + + + pos->version.firmware.major = mc_service.properties.canboards.at(0).firmware.major; + pos->version.firmware.minor = mc_service.properties.canboards.at(0).firmware.minor; + pos->version.firmware.build = mc_service.properties.canboards.at(0).firmware.build; + pos->version.protocol.major = mc_service.properties.canboards.at(0).protocol.major; + pos->version.protocol.minor = mc_service.properties.canboards.at(0).protocol.minor; + + // 2. ->arrayofjomodescriptors + EOarray *arrayofjomos = eo_array_New(4, sizeof(eOmc_jomo_descriptor_t), &data_mc->arrayofjomodescriptors); + size_t numofjomos = mc_service.properties.numofjoints; + + for(size_t i=0; i canboards; + std::vector canboards; eObrd_canlocation_t maislocation; std::vector psclocations; + std::vector poslocations; eOmc_mc4shifts_t mc4shifts; - std::vector mc4broadcasts; - std::vector mc4joints; + std::vector mc4broadcasts; + std::vector mc4joints; //servMC_controller_t controller; - std::vector actuators; - std::vector encoder1s; - std::vector encoder2s; + std::vector actuators; + std::vector encoder1s; + std::vector encoder2s; - //std::vector joint2set; + //std::vector joint2set; //int numofjointsets; //std::vector jointset_cfgs; } servMCproperties_t; @@ -286,6 +287,9 @@ class ServiceParser bool parse_psc(std::string const &fromstring, eObrd_portpsc_t &ppsc, bool &formaterror); bool parse_port_psc(std::string const &fromstring, uint8_t &toport, bool &formaterror); + bool parse_pos(std::string const &fromstring, eObrd_portpos_t &ppos, bool &formaterror); + bool parse_port_pos(std::string const &fromstring, uint8_t &toport, bool &formaterror); + #endif bool convert(std::string const &fromstring, eOmn_serv_type_t &toservicetype, bool &formaterror); diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index 41cad6dc18..e950b3e348 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -16,7 +16,7 @@ #include "embObjMotionControl.h" #include #include - +#include #include @@ -253,7 +253,7 @@ embObjMotionControl::embObjMotionControl() : behFlags.useRawEncoderData = false; behFlags.pwmIsLimited = false; - std::string tmp = NetworkBase::getEnvironment("ETH_VERBOSEWHENOK"); + std::string tmp = yarp::conf::environment::getEnvironment("ETH_VERBOSEWHENOK"); if (tmp != "") { behFlags.verbosewhenok = (bool)NetType::toInt(tmp); From 17f6726f33dff2bd5c901c2bc5c7ad163b144c52 Mon Sep 17 00:00:00 2001 From: "marco.accame" Date: Wed, 23 Dec 2020 13:19:38 +0100 Subject: [PATCH 2/2] fixed in case of use of eomn_serv_MC_mc4plusfaps --- .../embObjMotionControl/embObjMotionControl.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index e950b3e348..f0e255c1dc 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -603,7 +603,12 @@ bool embObjMotionControl::saveCouplingsData(void) jc_dest = &(serviceConfig.ethservice.configuration.data.mc.mc2pluspsc.jomocoupling); } break; - case eomn_serv_MC_mc4: + case eomn_serv_MC_mc4plusfaps: + { + jc_dest = &(serviceConfig.ethservice.configuration.data.mc.mc4plusfaps.jomocoupling); + + } break; + case eomn_serv_MC_mc4: { return true; } break; @@ -4726,7 +4731,8 @@ bool embObjMotionControl::iNeedCouplingsInfo(void) if( (mc_serv_type == eomn_serv_MC_foc) || (mc_serv_type == eomn_serv_MC_mc4plus) || (mc_serv_type == eomn_serv_MC_mc4plusmais) || - (mc_serv_type == eomn_serv_MC_mc2pluspsc) + (mc_serv_type == eomn_serv_MC_mc2pluspsc) || + (mc_serv_type == eomn_serv_MC_mc4plusfaps) ) return true; else