iCub-main
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 
14 // Yarp Includes
15 #include <yarp/os/Time.h>
16 #include <yarp/os/Log.h>
17 #include <yarp/os/LogStream.h>
18 #include <stdarg.h>
19 #include <stdio.h>
20 #include <yarp/dev/PolyDriver.h>
21 #include <ace/config.h>
22 #include <ace/Log_Msg.h>
23 
24 
25 // specific to this device driver.
26 #include <embObjPOS.h>
27 #include <ethManager.h>
28 #include <yarp/os/LogStream.h>
29 #include "EoAnalogSensors.h"
30 #include "EOnv_hid.h"
31 
32 #include "EoProtocol.h"
33 #include "EoProtocolMN.h"
34 #include "EoProtocolAS.h"
35 
36 #include <yarp/os/NetType.h>
37 
38 #ifdef WIN32
39 #pragma warning(once:4355)
40 #endif
41 
42 
43 
44 using namespace yarp;
45 using namespace yarp::os;
46 using namespace yarp::dev;
47 
48 
49 
50 bool embObjPOS::fromConfig(yarp::os::Searchable &config, servConfigPOS_t &serviceConfig)
51 {
52  ServiceParser parser;
53  if(false == parser.parseService(config, serviceConfig))
54  {
55  yError() << m_PDdevice.getBoardInfo() << ": missing some configuration parameter. Check logs and your config file.";
56  return false;
57  }
58 
59  return true;
60 }
61 
62 
63 embObjPOS::embObjPOS(): m_PDdevice("embObjPOS")
64 {
65 }
66 
67 
69 {
70  close();
71 }
72 
73 
75 {
76  return m_PDdevice.isOpen();
77 }
78 
79 
80 bool embObjPOS::open(yarp::os::Searchable &config)
81 {
82  // 1) prepare eth service verifing if the eth manager is available and parsing info about the eth board.
83 
84  yInfo() << "embObjPOS::open(): preparing ETH resource";
85 
86  if(! m_PDdevice.prerareEthService(config, this))
87  return false;
88 
89  yInfo() << "embObjPOS::open(): browsing xml files which describe the service";
90 
91  // 2) read stuff from config file
92  servConfigPOS_t serviceConfig;
93  if(!fromConfig(config, serviceConfig))
94  {
95  yError() << "embObjPOS missing some configuration parameter. Check logs and your config file.";
96  return false;
97  }
98 
99  // 3) prepare data vector
100  {
101  m_data.resize(eOas_pos_data_maxnumber, 0.0);
102  }
103 
104 
105  yInfo() << "embObjPOS::open(): verify the presence of the board and if its protocol version is correct";
106 
107  // 4) verify analog sensor protocol and then verify-Activate the POS service
108  if(!m_PDdevice.res->verifyEPprotocol(eoprot_endpoint_analogsensors))
109  {
110  cleanup();
111  return false;
112  }
113 
114 
115  yInfo() << "embObjPOS::open(): verify and activate the POS service";
116 
117  const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
118 
119  if(!m_PDdevice.res->serviceVerifyActivate(eomn_serv_category_pos, servparam, 5.0))
120  {
121  yError() << m_PDdevice.getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate() ";
122  cleanup();
123  return false;
124  }
125 
126 
127  //printServiceConfig();
128 
129 
130  yInfo() << "embObjPOS::open(): configure the POS service";
131 
132  if(false == sendConfig2boards(serviceConfig))
133  {
134  cleanup();
135  return false;
136  }
137 
138  yInfo() << "embObjPOS::open(): impose the network variable which the ETH bord must stream up";
139 
140  // Set variable to be signaled
141  if(false == initRegulars())
142  {
143  cleanup();
144  return false;
145  }
146 
147 
148  yInfo() << "embObjPOS::open(): start the POS service";
149 
150  if(!m_PDdevice.res->serviceStart(eomn_serv_category_pos))
151  {
152  yError() << m_PDdevice.getBoardInfo() << "open() fails to start as service.... cannot continue";
153  cleanup();
154  return false;
155  }
156  else
157  {
158  if(m_PDdevice.isVerbose())
159  {
160  yDebug() << m_PDdevice.getBoardInfo() << "open() correctly starts service";
161  }
162  }
163 
164  yInfo() << "embObjPOS::open(): start streaming of POS data";
165 
166  sendStart2boards();
167 
168  m_PDdevice.setOpen(true);
169 
170  return true;
171 }
172 
173 
174 bool embObjPOS::sendConfig2boards(servConfigPOS_t &serviceConfig)
175 {
176  eOprotID32_t id32 = eo_prot_ID32dummy;
177 
178  eOas_pos_config_t cfg;
179  cfg.datarate = serviceConfig.acquisitionrate;
180  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_config);
181 
182  if(false == m_PDdevice.res->setcheckRemoteValue(id32, &cfg, 10, 0.010, 0.050))
183  {
184  yError() << m_PDdevice.getBoardInfo() << "FATAL error in sendConfig2boards() while try to configure datarate=" << cfg.datarate;
185  return false;
186  }
187 
188  if(m_PDdevice.isVerbose())
189  {
190  yDebug() << m_PDdevice.getBoardInfo() << ": sendConfig2boards() correctly configured boards with datarate=" << cfg.datarate;
191  }
192 
193  return true;
194 
195 }
196 
197 bool embObjPOS::sendStart2boards(void)
198 {
199  eOprotID32_t id32 = eo_prot_ID32dummy;
200 
201  uint8_t enable=1;
202 
203  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_cmmnds_enable);
204 
205  if(false == m_PDdevice.res->setcheckRemoteValue(id32, &enable, 10, 0.010, 0.050))
206  {
207  yError() << m_PDdevice.getBoardInfo() << "FATAL error in sendStart2boards() while try to enable the boards transmission";
208  return false;
209  }
210 
211  if(m_PDdevice.isVerbose())
212  {
213  yDebug() << m_PDdevice.getBoardInfo() << ": sendStart2boards() correctly enabled the boards transmission";
214  }
215 
216  return true;
217 
218 }
219 
220 
221 bool embObjPOS::initRegulars(void)
222 {
223  // configure regular rops
224 
225  vector<eOprotID32_t> id32v(0);
226  eOprotID32_t id32 = eo_prot_ID32dummy;
227 
228  // we need to choose the id32 to put inside the vector
229 
230  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_status);;
231 
232  // and put it inside vector
233 
234  id32v.push_back(id32);
235 
236  // now we send the vector
237 
238  if(false == m_PDdevice.res->serviceSetRegulars(eomn_serv_category_pos, id32v))
239  {
240  yError() << m_PDdevice.getBoardInfo() << "initRegulars() fails to add its variables to regulars: cannot proceed any further";
241  return false;
242  }
243 
244  if(m_PDdevice.isVerbose())
245  {
246  yDebug() << m_PDdevice.getBoardInfo() << "initRegulars() added" << id32v.size() << "regular rops ";
247  char nvinfo[128];
248  for (size_t r = 0; r<id32v.size(); r++)
249  {
250  uint32_t item = id32v.at(r);
251  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
252  yDebug() << "\t it added regular rop for" << nvinfo;
253  }
254  }
255 
256 
257  return true;
258 }
259 
260 
266 int embObjPOS::read(yarp::sig::Vector &out)
267 {
268  // This method gives the data, received from embedded boards, to the analogServer
269 
270  if(!m_PDdevice.isOpen())
271  return AS_ERROR ;
272 
273  std::lock_guard<std::mutex> lock(m_mutex);
274 
275  // errors are not handled for now... it'll always be OK!!
276  out=m_data;
277 
278  return AS_OK;
279 }
280 
281 
282 
284 {
285  printf("getstate\n");
286  return AS_OK;
287 }
288 
289 
291 {
292  return static_cast<int>(eOas_pos_data_maxnumber);
293 }
294 
295 
297 {
298  return AS_OK;
299 }
300 
301 
302 int embObjPOS::calibrateSensor(const yarp::sig::Vector& value)
303 {
304  return AS_OK;
305 }
306 
307 
309 {
310  return AS_OK;
311 }
312 
313 
314 int embObjPOS::calibrateChannel(int ch, double v)
315 {
316  return AS_OK;
317 }
318 
319 
321 {
322  return eth::iethres_analogpos;
323 }
324 
325 
326 bool embObjPOS::update(eOprotID32_t id32, double timestamp, void* rxdata)
327 {
328  // called by feat_manage_analogsensors_data() which is called by:
329  // eoprot_fun_UPDT_as_pos_status
330  if(!m_PDdevice.isOpen())
331  return false;
332 
333  eOas_pos_status_t *pos_st_ptr = (eOas_pos_status_t*)rxdata;
334  uint32_t sizeOfData = pos_st_ptr->arrayofdata.head.size;
335 
336  if(sizeOfData>m_data.size())
337  yWarning() << 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!";
338 
339  std::lock_guard<std::mutex> lock(m_mutex);
340 
341  for(uint32_t i=0; i<sizeOfData; i++)
342  {
343  eOas_pos_data_t posdata = pos_st_ptr->arrayofdata.data[i];
344  //pos value is in deci-degree
345  m_data[i]= posdata.value*0.1;
346  }
347 
348  return true;
349 }
350 
351 
352 
353 
355 {
356  cleanup();
357  return true;
358 }
359 
360 
361 // void embObjPOS::printServiceConfig(void)
362 // {
363 // char loc[20] = {0};
364 // char fir[20] = {0};
365 // char pro[20] = {0};
366 //
367 // const char * boardname = (NULL != res) ? (res->getProperties().boardnameString.c_str()) : ("NOT-ASSIGNED-YET");
368 // const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString.c_str()) : ("NOT-ASSIGNED-YET");
369 //
370 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.canloc, loc, sizeof(loc));
371 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.firmware, fir, sizeof(fir));
372 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.protocol, pro, sizeof(pro));
373 //
374 // yInfo() << "The embObjPOS device using BOARD" << boardname << "w/ IP" << ipv4 << "has the following service config:";
375 // yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate;
376 // yInfo() << "- MAIS named" << serviceConfig.nameOfMais << "@" << loc << "with required protocol version =" << pro << "and required firmware version =" << fir;
377 // }
378 
379 
380 void embObjPOS::cleanup(void)
381 {
382  m_PDdevice.cleanup(static_cast <eth::IethResource*> (this));
383 }
384 
385 // eof
386 
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)
eth::AbstractEthResource * res
virtual int calibrateSensor()
Definition: embObjPOS.cpp:296
virtual int getChannels()
Definition: embObjPOS.cpp:290
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
Definition: embObjPOS.cpp:326
virtual int read(yarp::sig::Vector &out)
Read a vector from the sensor.
Definition: embObjPOS.cpp:266
virtual eth::iethresType_t type()
Definition: embObjPOS.cpp:320
virtual bool initialised()
Definition: embObjPOS.cpp:74
virtual int getState(int ch)
Definition: embObjPOS.cpp:283
bool open(yarp::os::Searchable &config)
Definition: embObjPOS.cpp:80
virtual int calibrateChannel(int ch, double v)
Definition: embObjPOS.cpp:314
static int v
Definition: iCub_Sim.cpp:42
iethresType_t
Definition: IethResource.h:61
@ iethres_analogpos
Definition: IethResource.h:74
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8
eOmn_serv_parameter_t ethservice
Definition: serviceParser.h:88