iCub-main
embObjMais.cpp
Go to the documentation of this file.
1 
2 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
3 
4 /*
5 * Copyright (C) 2012 Robotcub Consortium
6 * Author: Alberto Cardellino
7 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
8 *
9 */
10 
11 // general purpose stuff.
12 #include <string>
13 #include <iostream>
14 #include <string.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 <stdarg.h>
21 #include <stdio.h>
22 #include <yarp/dev/PolyDriver.h>
23 #include <ace/config.h>
24 #include <ace/Log_Msg.h>
25 
26 
27 // specific to this device driver.
28 #include <embObjMais.h>
29 #include <ethManager.h>
30 #include <yarp/os/LogStream.h>
31 #include "EoAnalogSensors.h"
32 #include "EOnv_hid.h"
33 
34 #include "EoProtocol.h"
35 #include "EoProtocolMN.h"
36 #include "EoProtocolAS.h"
37 
38 #include <yarp/os/NetType.h>
39 #include <yarp/conf/environment.h>
40 
41 #ifdef WIN32
42 #pragma warning(once:4355)
43 #endif
44 
45 
46 
47 using namespace yarp;
48 using namespace yarp::os;
49 using namespace yarp::dev;
50 
51 
52 inline bool NOT_YET_IMPLEMENTED(const char *txt)
53 {
54  yWarning() << std::string(txt) << " not yet implemented for embObjMais\n";
55  return false;
56 }
57 
58 // generic function that checks is key1 is present in input bottle and that the result has size elements
59 // return true/false
60 bool embObjMais::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
61 {
62  size++; // size includes also the name of the parameter
63  Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
64  if (tmp.isNull())
65  {
66  yError ("%s not found\n", key1.c_str());
67  return false;
68  }
69 
70  if(tmp.size()!=size)
71  {
72  yError("%s incorrect number of entries\n", key1.c_str());
73  return false;
74  }
75 
76  out=tmp;
77 
78  return true;
79 }
80 
81 
82 bool embObjMais::fromConfig(yarp::os::Searchable &_config)
83 {
84 #if defined(EMBOBJMAIS_USESERVICEPARSER)
85 
86 
87  if(false == parser->parseService(_config, serviceConfig))
88  {
89  return false;
90  }
91 
92  return true;
93 
94 #else
95 
96  Bottle xtmp;
97 
98  int _period = 0;
99 
100  // Analog Sensor stuff
101  Bottle config = _config.findGroup("GENERAL");
102  if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
103  {
104  yError() << "embObjMais Using default value = 0 (disabled)";
105  _period = 0;
106 
107  }
108  else
109  {
110  _period = xtmp.get(1).asInt32();
111  yDebug() << "embObjMais::fromConfig() detects embObjMais Using value of" << _period;
112  }
113 
114  serviceConfig.acquisitionrate = _period;
115 
116  return true;
117 #endif
118 }
119 
120 
121 embObjMais::embObjMais()
122 {
123  serviceConfig.acquisitionrate = 0;
124  memset(&serviceConfig.ethservice, 0, sizeof(serviceConfig.ethservice));
125 
126  timeStamp = 0;
127 
128  counterSat=0;
129  counterError=0;
130  counterTimeout=0;
131 
132  status = MAS_status::MAS_OK;
133 
134 
135  opened = false;
136 
137  analogdata.resize(0);
138 
139 
140  std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
141  if (tmp != "")
142  {
143  verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
144  }
145  else
146  {
147  verbosewhenok = false;
148  }
149 
150  parser = NULL;
151  res = NULL;
152 }
153 
154 
155 embObjMais::~embObjMais()
156 {
157  analogdata.resize(0);
158 
159  if(NULL != parser)
160  {
161  delete parser;
162  parser = NULL;
163  }
164 
165 }
166 
167 
168 bool embObjMais::initialised()
169 {
170  return opened;
171 }
172 
173 //#define TEST_MAIS_PLUS_MC
174 
175 
176 bool embObjMais::open(yarp::os::Searchable &config)
177 {
178  // - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
179 
180  ethManager = eth::TheEthManager::instance();
181  if(NULL == ethManager)
182  {
183  yFatal() << "embObjMais::open() fails to instantiate ethManager";
184  return false;
185  }
186 
187 
188  if(false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
189  {
190  yError() << "embObjMais::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
191  return false;
192  }
193  // add specific info about this device ...
194 
195 
196  // - now all other things
197 
198 
199 // std::string str;
200 // if(config.findGroup("GENERAL").find("verbose").asBool())
201 // str=config.toString().c_str();
202 // else
203 // str="\n";
204 // yTrace() << str;
205 
206 
207  if(NULL == parser)
208  {
209  parser = new ServiceParser;
210  }
211 
212  // read stuff from config file
213  if(!fromConfig(config))
214  {
215  yError() << "embObjMais missing some configuration parameter. Check logs and your config file.";
216  return false;
217  }
218 
219  // and prepare analogdata
220  {
221  // must be of size: mais_Channels, and 0.0-initted
222  analogdata.resize(mais_Channels, 0.0);
223  }
224 
225 
226 
227  // -- instantiate EthResource etc.
228 
229  res = ethManager->requestResource2(this, config);
230  if(NULL == res)
231  {
232  yError() << "embObjMais::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring << " ... unable to continue";
233  return false;
234  }
235 
236  printServiceConfig();
237 
238 
239  if(!res->verifyEPprotocol(eoprot_endpoint_analogsensors))
240  {
241  cleanup();
242  return false;
243  }
244 
245 #if defined(TEST_MAIS_PLUS_MC)
246 
247  // sarebbe bene chiamare requestResource2(ptr2fakemccontroller, config) ma pazienza
248 
249  const eOmn_serv_parameter_t* mcservparam = NULL;
250 
251  if(false == res->serviceVerifyActivate(eomn_serv_category_mc, mcservparam, 5.0))
252  {
253  yError() << "embObjMais::open() has an error in call of ethResources::serviceVerifyActivate(MC) for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
254  cleanup();
255  return false;
256  }
257  if(false == res->serviceStart(eomn_serv_category_mc))
258  {
259  yError() << "embObjMais::open() fails to start MC service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
260  cleanup();
261  return false;
262  }
263  else
264  {
265  if(verbosewhenok)
266  {
267  yDebug() << "embObjMais::open() correctly starts MC service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
268  }
269  }
270 
271 
272 #endif
273 
274 
275 
276 
277 
278 
279 #if defined(EMBOBJMAIS_USESERVICEPARSER)
280  const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
281 #else
282  const eOmn_serv_parameter_t* servparam = NULL;
283 #endif
284 
285  //servparam = NULL;
286 
287  if(false == res->serviceVerifyActivate(eomn_serv_category_mais, servparam, 5.0))
288  {
289  SystemClock::delaySystem(1);
290  yError() << "embObjMais::open() has an error in call of ethResources::serviceVerifyActivate() for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
291  //printServiceConfig();
292  cleanup();
293  return false;
294  }
295 
296  //printServiceConfig();
297 
298 
299  // configure the service: aka, send to the remote board information about the whereabouts of the can boards mais, strain, mtb which offers the service.
300  // so far nothing to do
301 
302 
303  // configure the sensor(s)
304 
305 
306  if(false == sendConfig2Mais())
307  {
308  cleanup();
309  return false;
310  }
311 
312  // Set variable to be signalled
313  if(false == initRegulars())
314  {
315  cleanup();
316  return false;
317  }
318 
319 
320  if(false == res->serviceStart(eomn_serv_category_mais))
321  {
322  yError() << "embObjMais::open() fails to start as service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
323  cleanup();
324  return false;
325  }
326  else
327  {
328  if(verbosewhenok)
329  {
330  yDebug() << "embObjMais::open() correctly starts as service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
331  }
332  }
333 
334 
335  opened = true;
336  return true;
337 }
338 
339 
340 bool embObjMais::sendConfig2Mais(void)
341 {
342  // version with read-back
343 
344  eOprotID32_t id32 = eo_prot_ID32dummy;
345 
346  // -- mais datarate
347 
348  uint8_t datarate = serviceConfig.acquisitionrate;
349  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_datarate);
350 
351  if(false == res->setcheckRemoteValue(id32, &datarate, 10, 0.010, 0.050))
352  {
353  yError() << "FATAL: embObjMais::sendConfig2Mais() had an error while calling setcheckRemoteValue() for mais datarate in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
354  return false;
355  }
356  else
357  {
358  if(verbosewhenok)
359  {
360  yDebug() << "embObjMais::sendConfig2Mais() correctly configured mais datarate at value" << datarate << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
361  }
362  }
363 
364  // -- mais tx mode
365 
366  eOenum08_t maismode = eoas_maismode_txdatacontinuously; // use eOas_maismode_t for value BUT USE for type (their sizes can be different !!)
367  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_mode);
368 
369  if(false == res->setcheckRemoteValue(id32, &maismode, 10, 0.010, 0.050))
370  {
371  yError() << "FATAL: embObjMais::sendConfig2Mais() had an error while calling setcheckRemoteValue() for mais mode in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
372  return false;
373  }
374  else
375  {
376  if(verbosewhenok)
377  {
378  yDebug() << "embObjMais::sendConfig2Mais() correctly configured mais mode at value" << maismode << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
379  }
380  }
381 
382  return true;
383 
384 }
385 
386 
387 
388 bool embObjMais::initRegulars()
389 {
390  // configure regular rops
391 
392  vector<eOprotID32_t> id32v(0);
393  eOprotID32_t id32 = eo_prot_ID32dummy;
394 
395  // we need to choose the id32 to put inside the vector
396 
397  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_status_the15values);;
398 
399  // and put it inside vector
400 
401  id32v.push_back(id32);
402 
403  // now we send the vector
404 
405  if(false == res->serviceSetRegulars(eomn_serv_category_mais, id32v))
406  {
407  yError() << "embObjMais::initRegulars() fails to add its variables to regulars: cannot proceed any further";
408  return false;
409  }
410  else
411  {
412  if(verbosewhenok)
413  {
414  yDebug() << "embObjMais::initRegulars() added" << id32v.size() << "regular rops to BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
415  char nvinfo[128];
416  for (size_t r = 0; r<id32v.size(); r++)
417  {
418  uint32_t item = id32v.at(r);
419  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
420  yDebug() << "\t it added regular rop for" << nvinfo;
421  }
422  }
423  }
424 
425  return true;
426 }
427 
428 
429 void embObjMais::resetCounters()
430 {
431  counterSat=0;
432  counterError=0;
433  counterTimeout=0;
434 }
435 
436 
437 void embObjMais::getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
438 {
439  sat=counterSat;
440  err=counterError;
441  to=counterTimeout;
442 }
443 
444 size_t embObjMais::getNrOfEncoderArrays() const {
445  return 1;
446 }
447 
448 yarp::dev::MAS_status embObjMais::getEncoderArrayStatus(size_t sens_index) const {
449 if (sens_index >= 1) return yarp::dev::MAS_UNKNOWN;
450  return yarp::dev::MAS_OK;
451 }
452 
453 bool embObjMais::getEncoderArrayName(size_t sens_index, std::string &name) const {
454  if (sens_index >= 1) return false;
455  name = serviceConfig.nameOfMais;
456  return true;
457 }
458 
459 bool embObjMais::getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const {
460  if (sens_index >= 1) return false;
461  timestamp = this->timeStamp;
462  out.resize(analogdata.size());
463  out = analogdata;
464  return true;
465 }
466 
467 size_t embObjMais::getEncoderArraySize(size_t sens_index) const {
468  if (sens_index >= 1) return 0;
469  return analogdata.size();
470 }
471 
472 
473 eth::iethresType_t embObjMais::type()
474 {
476 }
477 
478 
479 bool embObjMais::update(eOprotID32_t id32, double timestamp, void* rxdata)
480 {
481  id32 = id32;
482  this->timeStamp = timestamp;
483 
484  if(false == opened)
485  {
486  return false;
487  }
488 
489  // called by feat_manage_analogsensors_data() which is called by:
490  // eoprot_fun_UPDT_as_mais_status_the15values()
491  // the void* parameter inside this function is a eOas_arrayofupto36bytes_t*
492  // and can be treated as a EOarray
493 
494 
495  EOarray *array = (EOarray*)rxdata;
496  uint8_t size = eo_array_Size(array);
497  uint8_t itemsize = eo_array_ItemSize(array); // marco.accame: must be 1, as the code after uses this convention
498  if((0 == size) || (1 != itemsize))
499  {
500  return false;
501  }
502 
503  std::lock_guard<std::mutex> lck(mtx);
504 
505  for (size_t k = 0; k<analogdata.size(); k++)
506  {
507  uint8_t* tmp = (uint8_t*) eo_array_At(array, k);
508  if(NULL != tmp)
509  {
510  uint8_t val = *tmp; // marco.accame: i see that we treat the array as if containing items of 1 byte.
511  // Get the kth element of the array
512  analogdata[k] = (double)val;
513  }
514  }
515 
516  return true;
517 }
518 
519 
520 
521 
522 bool embObjMais::close()
523 {
524  opened = false;
525 
526  cleanup();
527  return true;
528 }
529 
530 
531 void embObjMais::printServiceConfig(void)
532 {
533  char loc[20] = {0};
534  char fir[20] = {0};
535  char pro[20] = {0};
536 
537  const char * boardname = (NULL != res) ? (res->getProperties().boardnameString.c_str()) : ("NOT-ASSIGNED-YET");
538  const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString.c_str()) : ("NOT-ASSIGNED-YET");
539 
540  parser->convert(serviceConfig.ethservice.configuration.data.as.mais.canloc, loc, sizeof(loc));
541  parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.firmware, fir, sizeof(fir));
542  parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.protocol, pro, sizeof(pro));
543 
544  yInfo() << "The embObjMais device using BOARD" << boardname << "w/ IP" << ipv4 << "has the following service config:";
545  yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate;
546  yInfo() << "- MAIS named" << serviceConfig.nameOfMais << "@" << loc << "with required protocol version =" << pro << "and required firmware version =" << fir;
547 }
548 
549 
550 void embObjMais::cleanup(void)
551 {
552  if(ethManager == NULL) return;
553 
554  int ret = ethManager->releaseResource2(res, this);
555  res = NULL;
556  if(ret == -1)
557  ethManager->killYourself();
558 }
559 
560 // eof
561 
static TheEthManager * instance()
Definition: ethManager.cpp:159
bool NOT_YET_IMPLEMENTED(const char *txt)
Definition: embObjMais.cpp:52
double sat(const double val, const double min, const double max)
Definition: utils.h:183
iethresType_t
Definition: IethResource.h:62
@ iethres_analogmais
Definition: IethResource.h:65
Copyright (C) 2008 RobotCub Consortium.
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
out
Definition: sine.m:8