17#include <yarp/os/Time.h>
18#include <yarp/os/Log.h>
19#include <yarp/os/LogStream.h>
20#include <yarp/dev/PolyDriver.h>
21#include <yarp/os/NetType.h>
24#include <ace/config.h>
25#include <ace/Log_Msg.h>
31#include "EoAnalogSensors.h"
33#include "EoProtocol.h"
34#include "EoProtocolMN.h"
35#include "EoProtocolAS.h"
39#pragma warning(once:4355)
43 YARP_LOG_COMPONENT(EMBOBJPOS,
"yarp.dev.embObjPOS")
47using namespace yarp::os;
53bool embObjPOS::fromConfig(yarp::os::Searchable &config,
servConfigPOS_t &serviceConfig)
58 yError() << m_PDdevice.
getBoardInfo() <<
": missing some configuration parameter. Check logs and your config file.";
79 return m_PDdevice.
isOpen();
87 yCInfo(EMBOBJPOS) <<
"embObjPOS::open(): preparing ETH resource";
92 yCInfo(EMBOBJPOS) <<
"embObjPOS::open(): browsing xml files which describe the service";
96 if(!fromConfig(config, serviceConfig))
98 yCError(EMBOBJPOS) <<
"embObjPOS missing some configuration parameter. Check logs and your config file.";
107 m_data.resize(eOas_pos_data_maxnumber, 0.0);
111 yCInfo(EMBOBJPOS) <<
"embObjPOS::open(): verify the presence of the board and if its protocol version is correct";
121 yCInfo(EMBOBJPOS) <<
"embObjPOS::open(): verify and activate the POS service";
123 const eOmn_serv_parameter_t* servparam = &serviceConfig.
ethservice;
127 yCError(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"open() has an error in call of ethResources::serviceVerifyActivate()";
133 yCInfo(EMBOBJPOS) <<
"embObjPOS::open(): configure the POS service";
135 if(
false == sendConfig2boards(serviceConfig))
141 yCInfo(EMBOBJPOS) <<
"embObjPOS::open(): impose the network variable which the ETH bord must stream up";
144 if(
false == initRegulars())
151 yCInfo(EMBOBJPOS) <<
"embObjPOS::open(): start the POS service";
155 yCError(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"open() fails to start as service.... cannot continue";
163 yCDebug(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"open() correctly starts service";
167 yCInfo(EMBOBJPOS) <<
"embObjPOS::open(): start streaming of POS data";
179 eOprotID32_t id32 = eo_prot_ID32dummy;
181 eOas_pos_config_t cfg;
183 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_config);
187 yCError(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"FATAL error in sendConfig2boards() while try to configure datarate=" << cfg.datarate;
193 yCDebug(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
": sendConfig2boards() correctly configured boards with datarate=" << cfg.datarate;
200bool embObjPOS::sendStart2boards(
void)
202 eOprotID32_t id32 = eo_prot_ID32dummy;
206 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_cmmnds_enable);
210 yCError(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"FATAL error in sendStart2boards() while try to enable the boards transmission";
216 yCDebug(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
": sendStart2boards() correctly enabled the boards transmission";
224bool embObjPOS::initRegulars(
void)
228 vector<eOprotID32_t> id32v(0);
229 eOprotID32_t id32 = eo_prot_ID32dummy;
233 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_status);;
237 id32v.push_back(id32);
243 yCError(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"initRegulars() fails to add its variables to regulars: cannot proceed any further";
249 yCDebug(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"initRegulars() added" << id32v.size() <<
"regular rops ";
250 char nvinfo[128] = {};
251 for (
size_t r = 0; r<id32v.size(); r++)
253 uint32_t item = id32v.at(r);
254 eoprot_ID2information(item, nvinfo,
sizeof(nvinfo));
255 yCDebug(EMBOBJPOS) <<
"\t it added regular rop for" << nvinfo;
262void embObjPOS::helper_remapperFromSensorToDataIndex(
const uint32_t sensorId, uint32_t &dataIndex)
const
305 eObrd_portpos_t portPosIndex = eobrd_portpos_unknown;
311 portPosIndex = eobrd_portpos_hand_thumb_oc;
314 portPosIndex = eobrd_portpos_hand_index_oc;
317 portPosIndex = eobrd_portpos_hand_middle_oc;
320 portPosIndex = eobrd_portpos_hand_ring_pinky_oc;
323 portPosIndex = eobrd_portpos_unknown;
332 portPosIndex = eobrd_portpos_hand_thumb_add;
335 portPosIndex = eobrd_portpos_hand_index_add;
338 portPosIndex = eobrd_portpos_unknown;
344 portPosIndex = eobrd_portpos_unknown;
347 dataIndex =
static_cast<uint32_t
>(portPosIndex);
358 yCDebug(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"embObjPOS::getNrOfEncoderArrays called at timestamp=" << yarp::os::Time::now() <<
"returning" << serviceConfig.
idList.size();
359 return serviceConfig.
idList.size();
364 if (sens_index >= serviceConfig.
idList.size())
return yarp::dev::MAS_UNKNOWN;
365 return yarp::dev::MAS_OK;
370 if (sens_index >= serviceConfig.
idList.size())
return false;
371 name = serviceConfig.
idList[sens_index];
378 if (sens_index >= serviceConfig.
idList.size())
return false;
379 std::lock_guard<std::mutex> lock(m_mutex);
380 timestamp = yarp::os::Time::now();
382 uint32_t dataIndex = 0;
383 helper_remapperFromSensorToDataIndex(
static_cast<uint32_t
>(sens_index), dataIndex);
385 out[0] = m_data[dataIndex];
391 if (sens_index >= serviceConfig.
idList.size())
return 0;
408 eOas_pos_status_t *pos_st_ptr = (eOas_pos_status_t*)rxdata;
409 uint32_t sizeOfData = pos_st_ptr->arrayofdata.head.size;
411 if(sizeOfData>m_data.size())
412 yCWarning(EMBOBJPOS) << m_PDdevice.
getBoardInfo() <<
"In update function I received more data than I had been configured to store.My size is " << m_data.size() <<
"while I received " << sizeOfData <<
"values!";
414 std::lock_guard<std::mutex> lock(m_mutex);
416 for(uint32_t i=0; i<sizeOfData; i++)
418 eOas_pos_data_t posdata = pos_st_ptr->arrayofdata.data[i];
420 m_data[i]= posdata.value*0.1;
432void embObjPOS::cleanup(
void)
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 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
virtual bool isOpen() const
bool prerareEthService(yarp::os::Searchable &config, eth::IethResource *interface)
void cleanup(eth::IethResource *interface)
std::string getBoardInfo(void) const
eth::AbstractEthResource * res
virtual bool getEncoderArrayName(size_t sens_index, std::string &name) const override
virtual size_t getEncoderArraySize(size_t sens_index) const override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual eth::iethresType_t type()
virtual bool initialised()
virtual bool getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
virtual yarp::dev::MAS_status getEncoderArrayStatus(size_t sens_index) const override
virtual size_t getNrOfEncoderArrays() const override
bool open(yarp::os::Searchable &config)
constexpr int POS_OPEN_CLOSE_SENSORS_COUNT
constexpr int POS_ABDUCTION_SENSORS_COUNT
Copyright (C) 2008 RobotCub Consortium.
std::vector< std::string > idList
eOmn_serv_parameter_t ethservice