17#include <yarp/os/Time.h>
18#include <yarp/os/Log.h>
19#include <yarp/os/LogStream.h>
22#include <yarp/dev/PolyDriver.h>
23#include <ace/config.h>
24#include <ace/Log_Msg.h>
30#include "EoAnalogSensors.h"
33#include "EoProtocol.h"
34#include "EoProtocolMN.h"
35#include "EoProtocolAS.h"
37#include <yarp/os/NetType.h>
38#include <yarp/conf/environment.h>
41#pragma warning(once:4355)
47using namespace yarp::os;
53 yWarning() << std::string(txt) <<
" not yet implemented for embObjMais\n";
59bool embObjMais::extractGroup(Bottle &input, Bottle &
out,
const std::string &key1,
const std::string &txt,
int size)
62 Bottle &
tmp=input.findGroup(key1.c_str(), txt.c_str());
65 yError (
"%s not found\n", key1.c_str());
71 yError(
"%s incorrect number of entries\n", key1.c_str());
81bool embObjMais::fromConfig(yarp::os::Searchable &_config)
83#if defined(EMBOBJMAIS_USESERVICEPARSER)
86 if(
false == parser->
parseService(_config, serviceConfig))
100 Bottle config = _config.findGroup(
"GENERAL");
101 if (!extractGroup(config, xtmp,
"Period",
"transmitting period of the sensors", 1))
103 yError() <<
"embObjMais Using default value = 0 (disabled)";
109 _period = xtmp.get(1).asInt32();
110 yDebug() <<
"embObjMais::fromConfig() detects embObjMais Using value of" << _period;
131 status = MAS_status::MAS_OK;
136 analogdata.resize(0);
139 std::string tmp = yarp::conf::environment::get_string(
"ETH_VERBOSEWHENOK");
142 verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
146 verbosewhenok =
false;
156 analogdata.resize(0);
180 if(NULL == ethManager)
182 yFatal() <<
"embObjMais::open() fails to instantiate ethManager";
187 if(
false == ethManager->
verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
189 yError() <<
"embObjMais::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
212 if(!fromConfig(config))
214 yError() <<
"embObjMais missing some configuration parameter. Check logs and your config file.";
231 yError() <<
"embObjMais::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring <<
" ... unable to continue";
235 printServiceConfig();
244#if defined(TEST_MAIS_PLUS_MC)
248 const eOmn_serv_parameter_t* mcservparam = NULL;
278#if defined(EMBOBJMAIS_USESERVICEPARSER)
279 const eOmn_serv_parameter_t* servparam = &serviceConfig.
ethservice;
281 const eOmn_serv_parameter_t* servparam = NULL;
288 SystemClock::delaySystem(1);
305 if(
false == sendConfig2Mais())
312 if(
false == initRegulars())
339bool embObjMais::sendConfig2Mais(
void)
343 eOprotID32_t id32 = eo_prot_ID32dummy;
348 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_datarate);
365 eOenum08_t maismode = eoas_maismode_txdatacontinuously;
366 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_mode);
387bool embObjMais::initRegulars()
391 vector<eOprotID32_t> id32v(0);
392 eOprotID32_t id32 = eo_prot_ID32dummy;
396 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_status_the15values);;
400 id32v.push_back(id32);
406 yError() <<
"embObjMais::initRegulars() fails to add its variables to regulars: cannot proceed any further";
415 for (
size_t r = 0; r<id32v.size(); r++)
417 uint32_t item = id32v.at(r);
418 eoprot_ID2information(item, nvinfo,
sizeof(nvinfo));
419 yDebug() <<
"\t it added regular rop for" << nvinfo;
428void embObjMais::resetCounters()
436void embObjMais::getCounters(
unsigned int &
sat,
unsigned int &err,
unsigned int &to)
448if (sens_index >= 1)
return yarp::dev::MAS_UNKNOWN;
449 return yarp::dev::MAS_OK;
453 if (sens_index >= 1)
return false;
459 if (sens_index >= 1)
return false;
460 timestamp = this->timeStamp;
461 out.resize(analogdata.size());
467 if (sens_index >= 1)
return 0;
468 return analogdata.size();
481 this->timeStamp = timestamp;
494 EOarray *array = (EOarray*)rxdata;
495 uint8_t size = eo_array_Size(array);
496 uint8_t itemsize = eo_array_ItemSize(array);
497 if((0 == size) || (1 != itemsize))
502 std::lock_guard<std::mutex> lck(mtx);
504 for (
size_t k = 0; k<analogdata.size(); k++)
506 uint8_t* tmp = (uint8_t*) eo_array_At(array, k);
511 analogdata[k] = (double)val;
530void embObjMais::printServiceConfig(
void)
539 parser->
convert(serviceConfig.
ethservice.configuration.data.as.mais.canloc, loc,
sizeof(loc));
540 parser->
convert(serviceConfig.
ethservice.configuration.data.as.mais.version.firmware, fir,
sizeof(fir));
541 parser->
convert(serviceConfig.
ethservice.configuration.data.as.mais.version.protocol, pro,
sizeof(pro));
543 yInfo() <<
"The embObjMais device using BOARD" << boardname <<
"w/ IP" << ipv4 <<
"has the following service config:";
545 yInfo() <<
"- MAIS named" << serviceConfig.
nameOfMais <<
"@" << loc <<
"with required protocol version =" << pro <<
"and required firmware version =" << fir;
549void embObjMais::cleanup(
void)
551 if(ethManager == NULL)
return;
bool convert(std::string const &fromstring, eOmc_actuator_t &toactuatortype, bool &formaterror)
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
virtual bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)=0
virtual bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)=0
virtual const Properties & getProperties()=0
virtual bool verifyEPprotocol(eOprot_endpoint_t ep)=0
virtual bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
virtual bool serviceStart(eOmn_serv_category_t category, double timeout=0.500)=0
int releaseResource2(eth::AbstractEthResource *ethresource, IethResource *interface)
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
static bool killYourself()
static TheEthManager * instance()
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
virtual bool initialised()
virtual bool getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
virtual bool getEncoderArrayName(size_t sens_index, std::string &name) const override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual size_t getNrOfEncoderArrays() const override
bool open(yarp::os::Searchable &config)
virtual yarp::dev::MAS_status getEncoderArrayStatus(size_t sens_index) const override
virtual eth::iethresType_t type()
virtual size_t getEncoderArraySize(size_t sens_index) const override
bool NOT_YET_IMPLEMENTED(const char *txt)
double sat(const double val, const double min, const double max)
Copyright (C) 2008 RobotCub Consortium.
eOmn_serv_parameter_t ethservice