iCub-main
Loading...
Searching...
No Matches
embObjPOS.cpp
Go to the documentation of this file.
1
2/*
3 * Copyright (C) 2020 iCub Tech - Istituto Italiano di Tecnologia
4 * Author: Marco Accame
5 * email: marco.accame@iit.it
6*/
7
8
9// general purpose stuff.
10#include <string>
11#include <iostream>
12#include <string.h>
13#include <stdarg.h>
14#include <stdio.h>
15
16// Yarp Includes
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>
22
23// external libraries includes
24#include <ace/config.h>
25#include <ace/Log_Msg.h>
26
27
28// specific to this device driver.
29#include <embObjPOS.h>
30#include <ethManager.h>
31#include "EoAnalogSensors.h"
32#include "EOnv_hid.h"
33#include "EoProtocol.h"
34#include "EoProtocolMN.h"
35#include "EoProtocolAS.h"
36
37
38#ifdef WIN32
39#pragma warning(once:4355)
40#endif
41
42namespace {
43 YARP_LOG_COMPONENT(EMBOBJPOS, "yarp.dev.embObjPOS")
44}
45
46using namespace yarp;
47using namespace yarp::os;
48using namespace yarp::dev;
49
52
53bool embObjPOS::fromConfig(yarp::os::Searchable &config, servConfigPOS_t &serviceConfig)
54{
55 ServiceParser parser;
56 if(false == parser.parseService(config, serviceConfig))
57 {
58 yError() << m_PDdevice.getBoardInfo() << ": missing some configuration parameter. Check logs and your config file.";
59 return false;
60 }
61
62 return true;
63}
64
65
66embObjPOS::embObjPOS(): m_PDdevice("embObjPOS")
67{
68}
69
70
75
76
78{
79 return m_PDdevice.isOpen();
80}
81
82
83bool embObjPOS::open(yarp::os::Searchable &config)
84{
85 // 1) prepare eth service verifing if the eth manager is available and parsing info about the eth board.
86
87 yCInfo(EMBOBJPOS) << "embObjPOS::open(): preparing ETH resource";
88
89 if(! m_PDdevice.prerareEthService(config, this))
90 return false;
91
92 yCInfo(EMBOBJPOS) << "embObjPOS::open(): browsing xml files which describe the service";
93
94 // 2) read stuff from config file
95 serviceConfig = {};
96 if(!fromConfig(config, serviceConfig))
97 {
98 yCError(EMBOBJPOS) << "embObjPOS missing some configuration parameter. Check logs and your config file.";
99 return false;
100 }
101
102 // 3) prepare data vector
103 {
104 // for now resize to max number of pos sensors
105 //--> then we will resize to the number of enabled sensors
106 //--> we can eventually use here reserve to max number and resize to enabled number
107 m_data.resize(eOas_pos_data_maxnumber, 0.0);
108 }
109
110
111 yCInfo(EMBOBJPOS) << "embObjPOS::open(): verify the presence of the board and if its protocol version is correct";
112
113 // 4) verify analog sensor protocol and then verify-Activate the POS service
114 if(!m_PDdevice.res->verifyEPprotocol(eoprot_endpoint_analogsensors))
115 {
116 cleanup();
117 return false;
118 }
119
120
121 yCInfo(EMBOBJPOS) << "embObjPOS::open(): verify and activate the POS service";
122
123 const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
124
125 if(!m_PDdevice.res->serviceVerifyActivate(eomn_serv_category_pos, servparam, 5.0))
126 {
127 yCError(EMBOBJPOS) << m_PDdevice.getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate()";
128 cleanup();
129 return false;
130 }
131
132
133 yCInfo(EMBOBJPOS) << "embObjPOS::open(): configure the POS service";
134
135 if(false == sendConfig2boards(serviceConfig))
136 {
137 cleanup();
138 return false;
139 }
140
141 yCInfo(EMBOBJPOS) << "embObjPOS::open(): impose the network variable which the ETH bord must stream up";
142
143 // Set variable to be signaled
144 if(false == initRegulars())
145 {
146 cleanup();
147 return false;
148 }
149
150
151 yCInfo(EMBOBJPOS) << "embObjPOS::open(): start the POS service";
152
153 if(!m_PDdevice.res->serviceStart(eomn_serv_category_pos))
154 {
155 yCError(EMBOBJPOS) << m_PDdevice.getBoardInfo() << "open() fails to start as service.... cannot continue";
156 cleanup();
157 return false;
158 }
159 else
160 {
161 if(m_PDdevice.isVerbose())
162 {
163 yCDebug(EMBOBJPOS) << m_PDdevice.getBoardInfo() << "open() correctly starts service";
164 }
165 }
166
167 yCInfo(EMBOBJPOS) << "embObjPOS::open(): start streaming of POS data";
168
169 sendStart2boards();
170
171 m_PDdevice.setOpen(true);
172
173 return true;
174}
175
176
177bool embObjPOS::sendConfig2boards(servConfigPOS_t &serviceConfig)
178{
179 eOprotID32_t id32 = eo_prot_ID32dummy;
180
181 eOas_pos_config_t cfg;
182 cfg.datarate = serviceConfig.acquisitionrate;
183 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_config);
184
185 if(false == m_PDdevice.res->setcheckRemoteValue(id32, &cfg, 10, 0.010, 0.050))
186 {
187 yCError(EMBOBJPOS) << m_PDdevice.getBoardInfo() << "FATAL error in sendConfig2boards() while try to configure datarate=" << cfg.datarate;
188 return false;
189 }
190
191 if(m_PDdevice.isVerbose())
192 {
193 yCDebug(EMBOBJPOS) << m_PDdevice.getBoardInfo() << ": sendConfig2boards() correctly configured boards with datarate=" << cfg.datarate;
194 }
195
196 return true;
197
198}
199
200bool embObjPOS::sendStart2boards(void)
201{
202 eOprotID32_t id32 = eo_prot_ID32dummy;
203
204 uint8_t enable=1;
205
206 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_cmmnds_enable);
207
208 if(false == m_PDdevice.res->setcheckRemoteValue(id32, &enable, 10, 0.010, 0.050))
209 {
210 yCError(EMBOBJPOS) << m_PDdevice.getBoardInfo() << "FATAL error in sendStart2boards() while try to enable the boards transmission";
211 return false;
212 }
213
214 if(m_PDdevice.isVerbose())
215 {
216 yCDebug(EMBOBJPOS) << m_PDdevice.getBoardInfo() << ": sendStart2boards() correctly enabled the boards transmission";
217 }
218
219 return true;
220
221}
222
223
224bool embObjPOS::initRegulars(void)
225{
226 // configure regular rops
227
228 vector<eOprotID32_t> id32v(0);
229 eOprotID32_t id32 = eo_prot_ID32dummy;
230
231 // we need to choose the id32 to put inside the vector
232
233 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_status);;
234
235 // and put it inside vector
236
237 id32v.push_back(id32);
238
239 // now we send the vector
240
241 if(false == m_PDdevice.res->serviceSetRegulars(eomn_serv_category_pos, id32v))
242 {
243 yCError(EMBOBJPOS) << m_PDdevice.getBoardInfo() << "initRegulars() fails to add its variables to regulars: cannot proceed any further";
244 return false;
245 }
246
247 if(m_PDdevice.isVerbose())
248 {
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++)
252 {
253 uint32_t item = id32v.at(r);
254 eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
255 yCDebug(EMBOBJPOS) << "\t it added regular rop for" << nvinfo;
256 }
257 }
258
259 return true;
260}
261
262void embObjPOS::helper_remapperFromSensorToDataIndex(const uint32_t sensorId, uint32_t &dataIndex) const
263{
300 // First we need to understand if the sensorId is related to open-close or abduction.
301 // We can do this by checking the size of idList[] vector, which is 4 for open-close and 2 for abduction.
302 // We need to use a temporary variable to store the dataIndex, because it is passed by reference.
303 // and the enum has a different type than uint32_t.
304
305 eObrd_portpos_t portPosIndex = eobrd_portpos_unknown; // default value in case of error
306 if (serviceConfig.idList.size() == POS_OPEN_CLOSE_SENSORS_COUNT) // open-close
307 {
308 switch (sensorId)
309 {
310 case 0:
311 portPosIndex = eobrd_portpos_hand_thumb_oc;
312 break;
313 case 1:
314 portPosIndex = eobrd_portpos_hand_index_oc;
315 break;
316 case 2:
317 portPosIndex = eobrd_portpos_hand_middle_oc;
318 break;
319 case 3:
320 portPosIndex = eobrd_portpos_hand_ring_pinky_oc;
321 break;
322 default:
323 portPosIndex = eobrd_portpos_unknown; // error
324 break;
325 }
326 }
327 else if (serviceConfig.idList.size() == POS_ABDUCTION_SENSORS_COUNT) // abduction
328 {
329 switch (sensorId)
330 {
331 case 0:
332 portPosIndex = eobrd_portpos_hand_thumb_add;
333 break;
334 case 1:
335 portPosIndex = eobrd_portpos_hand_index_add;
336 break;
337 default:
338 portPosIndex = eobrd_portpos_unknown; // error
339 break;
340 }
341 }
342 else
343 {
344 portPosIndex = eobrd_portpos_unknown; // error
345 }
346
347 dataIndex = static_cast<uint32_t>(portPosIndex);
348}
349
351{
352 // this should return the number of sensors we wanna have per control boards
353 // e.g. currently POS service can manage up to 4 position sensors (2 for open/close and 2 for abduction)
354 // so we are returning the number of enabled sensors, i.e. 2 or 4, depending on the device
355 // differently, we can choose another approach, i.e. return 1, as the encoder array is a single entity,
356 // which is actually the control board associated with the opened serivice
357 // and then getEncoderArraySize() should return 2 or 4, i.es the number of enabled sensors
358 yCDebug(EMBOBJPOS) << m_PDdevice.getBoardInfo() << "embObjPOS::getNrOfEncoderArrays called at timestamp=" << yarp::os::Time::now() << "returning" << serviceConfig.idList.size();
359 return serviceConfig.idList.size();
360}
361
362yarp::dev::MAS_status embObjPOS::getEncoderArrayStatus(size_t sens_index) const
363{
364 if (sens_index >= serviceConfig.idList.size()) return yarp::dev::MAS_UNKNOWN;
365 return yarp::dev::MAS_OK;
366}
367
368bool embObjPOS::getEncoderArrayName(size_t sens_index, std::string &name) const
369{
370 if (sens_index >= serviceConfig.idList.size()) return false;
371 name = serviceConfig.idList[sens_index];
372 return true;
373}
374
375bool embObjPOS::getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
376{
377 // we cannot receice an index higher that the number of enabled sensors
378 if (sens_index >= serviceConfig.idList.size()) return false;
379 std::lock_guard<std::mutex> lock(m_mutex);
380 timestamp = yarp::os::Time::now();
381 // we need to remap the sens_index, which is the index in idList[], to the corresponding index in m_data[] vector
382 uint32_t dataIndex = 0;
383 helper_remapperFromSensorToDataIndex(static_cast<uint32_t>(sens_index), dataIndex);
384 out.resize(1);
385 out[0] = m_data[dataIndex];
386 return true;
387}
388
389size_t embObjPOS::getEncoderArraySize(size_t sens_index) const
390{
391 if (sens_index >= serviceConfig.idList.size()) return 0;
392 return 1; //this should return the size of data at index sens_index, which should actually be 1.
393}
394
399
400
401bool embObjPOS::update(eOprotID32_t id32, double timestamp, void* rxdata)
402{
403 // called by feat_manage_analogsensors_data() which is called by:
404 // eoprot_fun_UPDT_as_pos_status()
405 if(!m_PDdevice.isOpen())
406 return false;
407
408 eOas_pos_status_t *pos_st_ptr = (eOas_pos_status_t*)rxdata;
409 uint32_t sizeOfData = pos_st_ptr->arrayofdata.head.size;
410
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!";
413
414 std::lock_guard<std::mutex> lock(m_mutex);
415
416 for(uint32_t i=0; i<sizeOfData; i++)
417 {
418 eOas_pos_data_t posdata = pos_st_ptr->arrayofdata.data[i];
419 //pos value is in deci-degree
420 m_data[i]= posdata.value*0.1;
421 }
422
423 return true;
424}
425
427{
428 cleanup();
429 return true;
430}
431
432void embObjPOS::cleanup(void)
433{
434 m_PDdevice.cleanup(static_cast<eth::IethResource*> (this));
435}
436
437// eof
438
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
bool prerareEthService(yarp::os::Searchable &config, eth::IethResource *interface)
void cleanup(eth::IethResource *interface)
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()
Definition embObjPOS.cpp:77
virtual bool getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) 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)
Definition embObjPOS.cpp:83
constexpr int POS_OPEN_CLOSE_SENSORS_COUNT
Definition embObjPOS.cpp:50
constexpr int POS_ABDUCTION_SENSORS_COUNT
Definition embObjPOS.cpp:51
iethresType_t
@ iethres_analogpos
Copyright (C) 2008 RobotCub Consortium.
out
Definition sine.m:8
std::vector< std::string > idList
eOmn_serv_parameter_t ethservice