iCub-main
embObjMultiEnc.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: Valentina Gaggero
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 <embObjMultiEnc.h>
29 #include <ethManager.h>
30 #include <yarp/os/LogStream.h>
31 //#include "EoAnalogSensors.h"
32 #include "EoMeasures.h"
33 #include "EoMotionControl.h"
34 #include "EOnv_hid.h"
35 
36 #include "EoProtocol.h"
37 #include "EoProtocolMN.h"
38 //#include "EoProtocolAS.h"
39 #include "EoProtocolMC.h"
40 
41 #include <yarp/os/NetType.h>
42 #include <yarp/conf/environment.h>
43 
44 #ifdef WIN32
45 #pragma warning(once:4355)
46 #endif
47 
48 
49 
50 using namespace yarp;
51 using namespace yarp::os;
52 using namespace yarp::dev;
53 
54 
55 inline bool NOT_YET_IMPLEMENTED(const char *txt)
56 {
57  yWarning() << std::string(txt) << " not yet implemented for embObjMultiEnc\n";
58  return false;
59 }
60 
61 // generic function that checks is key1 is present in input bottle and that the result has size elements
62 // return true/false
63 bool embObjMultiEnc::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
64 {
65  size++; // size includes also the name of the parameter
66  Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
67  if (tmp.isNull())
68  {
69  yError ("%s not found\n", key1.c_str());
70  return false;
71  }
72 
73  if(tmp.size()!=size)
74  {
75  yError("%s incorrect number of entries\n", key1.c_str());
76  return false;
77  }
78 
79  out=tmp;
80 
81  return true;
82 }
83 
84 
85 bool embObjMultiEnc::fromConfig(yarp::os::Searchable &_config)
86 {
87  yDebug()<< "configurazione: ";;
88  yDebug() << _config.toString();
89 
90  Bottle general = _config.findGroup("JOINTS");
91  if(general.isNull())
92  {
93  yError() << "embObjMultiEnc cannot find general group";
94  return false;
95  }
96 
97  Bottle jointsbottle = general.findGroup("listofjoints");
98  if (jointsbottle.isNull())
99  {
100  yError() << "embObjMultiEnc cannot find listofjoints param";
101  return false;
102  }
103 
104  Bottle encsbottle = general.findGroup("encoderConversionFactor");
105  if (encsbottle.isNull())
106  {
107  yError() << "embObjMultiEnc cannot find encoderConversionFactor param";
108  return false;
109  }
110 
111 
112  //jointsbottle contains: "listofjoints 0 1 2 3. So the num of joints is jointsbottle->size() -1 "
113  numofjoints = jointsbottle.size() -1;
114 
115  listofjoints.clear();
116  for (int i = 1; i < jointsbottle.size(); i++) listofjoints.push_back(jointsbottle.get(i).asInt32());
117 
118  yDebug()<< " embObjMultiEnc List of joints: " << numofjoints;
119  for(int i=0; i<numofjoints; i++) yDebug() << "pos="<< i << "val="<< listofjoints[i];
120 
121  analogdata.resize(numofencperjoint*numofjoints, 0.0);
122  encoderConversionFactor.resize(numofencperjoint*numofjoints, 1.0);
123 
124  if (numofencperjoint*numofjoints!=encsbottle.size()-1)
125  {
126  yError() << "embObjMultiEnc invalid size of encoderConversionFactor param";
127  return false;
128  }
129  for (int i=0; i<encsbottle.size()-1; i++)
130  {
131  encoderConversionFactor[i]=encsbottle.get(i+1).asFloat64();
132  }
133 
134  return true;
135 }
136 
137 
138 embObjMultiEnc::embObjMultiEnc()
139 {
140  memset(&serviceConfig.ethservice, 0, sizeof(serviceConfig.ethservice));
141 
142  timeStamp = 0;
143 
144  counterSat=0;
145  counterError=0;
146  counterTimeout=0;
147 
148  status = IAnalogSensor::AS_OK;
149 
150 
151  opened = false;
152 
153  analogdata.resize(0);
154 
155  numofjoints = default_numofjoints;
156  numofencperjoint = default_numofencperjoint;
157 
158 
159  std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
160  if (tmp != "")
161  {
162  verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
163  }
164  else
165  {
166  verbosewhenok = false;
167  }
168 
169  parser = NULL;
170  res = NULL;
171 }
172 
173 
174 embObjMultiEnc::~embObjMultiEnc()
175 {
176  analogdata.resize(0);
177 
178  if(NULL != parser)
179  {
180  delete parser;
181  parser = NULL;
182  }
183 
184 }
185 
186 
187 bool embObjMultiEnc::initialised()
188 {
189  return opened;
190 }
191 
192 //#define TEST_MAIS_PLUS_MC
193 
194 
195 bool embObjMultiEnc::open(yarp::os::Searchable &config)
196 {
197  // - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
198 
199  ethManager = eth::TheEthManager::instance();
200  if(NULL == ethManager)
201  {
202  yFatal() << "embObjMultiEnc::open() fails to instantiate ethManager";
203  return false;
204  }
205 
206 
207  if(false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
208  {
209  yError() << "embObjMultiEnc::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
210  return false;
211  }
212  // add specific info about this device ...
213 
214 
215  // - now all other things
216 
217 
218 // std::string str;
219 // if(config.findGroup("GENERAL").find("verbose").asBool())
220 // str=config.toString().c_str();
221 // else
222 // str="\n";
223 // yTrace() << str;
224 
225 
226  if(NULL == parser)
227  {
228  parser = new ServiceParser;
229  }
230 
231  // read stuff from config file
232  if(!fromConfig(config))
233  {
234  yError() << "embObjMultiEnc missing some configuration parameter. Check logs and your config file.";
235  return false;
236  }
237 
238  // -- instantiate EthResource etc.
239 
240  res = ethManager->requestResource2(this, config);
241  if(NULL == res)
242  {
243  yError() << "embObjMultiEnc::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring << " ... unable to continue";
244  return false;
245  }
246 
247  printServiceConfig();
248 
249 
250  if(!res->verifyEPprotocol(eoprot_endpoint_motioncontrol))
251  {
252  cleanup();
253  return false;
254  }
255 
256 
257 
258 
259 
260 
262 // #if defined(EMBOBJMULTIENC_USESERVICEPARSER)
263 // const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
264 // #else
265 // const eOmn_serv_parameter_t* servparam = NULL;
266 // #endif
267 //
268 // //servparam = NULL;
269 //
270 // if(false == res->serviceVerifyActivate(eomn_serv_category_mais, servparam, 5.0))
271 // {
272 // yarp::os::Time::delay(1);
273 // yError() << "embObjMais::open() has an error in call of ethResources::serviceVerifyActivate() for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
274 // //printServiceConfig();
275 // cleanup();
276 // return false;
277 // }
278 //
279 // //printServiceConfig();
280 //
281 //
282 // // configure the service: aka, send to the remote board information about the whereabouts of the can boards mais, strain, mtb which offers the service.
283 // // so far nothing to do
284 //
285 //
286 // // configure the sensor(s)
287 //
288 //
289 // if(false == sendConfig2Mais())
290 // {
291 // cleanup();
292 // return false;
293 // }
294 
295  // Set variable to be signalled
296  if(false == initRegulars())
297  {
298  cleanup();
299  return false;
300  }
301 
302 
303 // if(false == res->serviceStart(eomn_serv_category_mais))
304 // {
305 // yError() << "embObjMais::open() fails to start as service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
306 // cleanup();
307 // return false;
308 // }
309 // else
310 // {
311 // if(verbosewhenok)
312 // {
313 // yDebug() << "embObjMais::open() correctly starts as service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
314 // }
315 // }
316 
317 
318  opened = true;
319  return true;
320 }
321 
322 
323 
324 
325 // bool embObjMultiEnc::sendConfig2Mais(void)
326 // {
327 // version with read-back
328 //
329 // eOprotID32_t id32 = eo_prot_ID32dummy;
330 //
331 // -- mais datarate
332 //
333 // uint8_t datarate = serviceConfig.acquisitionrate;
334 // id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_datarate);
335 //
336 // if(false == res->setRemoteValueUntilVerified(id32, &datarate, sizeof(datarate), 10, 0.010, 0.050, 2))
337 // {
338 // yError() << "FATAL: embObjMais::sendConfig2Mais() had an error while calling setRemoteValueUntilVerified() for mais datarate in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
339 // return false;
340 // }
341 // else
342 // {
343 // if(verbosewhenok)
344 // {
345 // yDebug() << "embObjMais::sendConfig2Mais() correctly configured mais datarate at value" << datarate << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
346 // }
347 // }
348 //
349 // -- mais tx mode
350 //
351 // eOenum08_t maismode = eoas_maismode_txdatacontinuously; // use eOas_maismode_t for value BUT USE for type (their sizes can be different !!)
352 // id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_mode);
353 //
354 // if(false == res->setRemoteValueUntilVerified(id32, &maismode, sizeof(maismode), 10, 0.010, 0.050, 2))
355 // {
356 // yError() << "FATAL: embObjMais::sendConfig2Mais() had an error while calling setRemoteValueUntilVerified() for mais mode in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
357 // return false;
358 // }
359 // else
360 // {
361 // if(verbosewhenok)
362 // {
363 // yDebug() << "embObjMais::sendConfig2Mais() correctly configured mais mode at value" << maismode << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
364 // }
365 // }
366 //
367 // return true;
368 //
369 // }
370 
371 
372 
373 bool embObjMultiEnc::initRegulars()
374 {
375  // configure regular rops
376 
377  vector<eOprotID32_t> id32v(0);
378  eOprotID32_t id32 = eo_prot_ID32dummy;
379 
380  id32v.clear();
381  for(int j=0; j<numofjoints; j++)
382  {
383  // we need to choose the id32 to put inside the vector
384  id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, listofjoints[j], eoprot_tag_mc_joint_status_addinfo_multienc);
385 
386  // and put it inside vector
387 
388  id32v.push_back(id32);
389  }
390  // now we send the vector
391 
392  if(false == res->serviceSetRegulars(eomn_serv_category_mc, id32v))
393  {
394  yError() << "embObjMultiEnc::initRegulars() fails to add its variables to regulars: cannot proceed any further";
395  return false;
396  }
397  else
398  {
399  if(verbosewhenok)
400  {
401  yDebug() << "embObjMultiEnc::initRegulars() added" << id32v.size() << "regular rops to BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
402  char nvinfo[128];
403  for (size_t r = 0; r<id32v.size(); r++)
404  {
405  uint32_t item = id32v.at(r);
406  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
407  yDebug() << "\t it added regular rop for" << nvinfo;
408  }
409  }
410  }
411 
412  return true;
413 }
414 
415 
421 int embObjMultiEnc::read(yarp::sig::Vector &out)
422 {
423  // This method gives analogdata to the analogServer
424 
425  if(false == opened)
426  {
427  return false;
428  }
429 
430  std::lock_guard<std::mutex> lck(mtx);
431 
432  // errors are not handled for now... it'll always be OK!!
433  if (status != IAnalogSensor::AS_OK)
434  {
435  switch (status)
436  {
437  case IAnalogSensor::AS_OVF:
438  {
439  counterSat++;
440  } break;
441  case IAnalogSensor::AS_ERROR:
442  {
443  counterError++;
444  } break;
445  case IAnalogSensor::AS_TIMEOUT:
446  {
447  counterTimeout++;
448  } break;
449  default:
450  {
451  counterError++;
452  } break;
453  }
454  return status;
455  }
456 
457  out.resize(analogdata.size());
458  for (size_t k = 0; k<analogdata.size(); k++)
459  {
460  out[k] = analogdata[k];
461  }
462 
463  return status;
464 }
465 
466 
467 void embObjMultiEnc::resetCounters()
468 {
469  counterSat=0;
470  counterError=0;
471  counterTimeout=0;
472 }
473 
474 
475 void embObjMultiEnc::getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
476 {
477  sat=counterSat;
478  err=counterError;
479  to=counterTimeout;
480 }
481 
482 
483 int embObjMultiEnc::getState(int ch)
484 {
485  printf("getstate\n");
486  return AS_OK;
487 }
488 
489 
490 int embObjMultiEnc::getChannels()
491 {
492  return analogdata.size();
493 }
494 
495 
496 int embObjMultiEnc::calibrateSensor()
497 {
498  return AS_OK;
499 }
500 
501 
502 int embObjMultiEnc::calibrateSensor(const yarp::sig::Vector& value)
503 {
504  return AS_OK;
505 }
506 
507 
508 int embObjMultiEnc::calibrateChannel(int ch)
509 {
510  return AS_OK;
511 }
512 
513 
514 int embObjMultiEnc::calibrateChannel(int ch, double v)
515 {
516  return AS_OK;
517 }
518 
519 
520 eth::iethresType_t embObjMultiEnc::type()
521 {
523 }
524 
525 
526 bool embObjMultiEnc::update(eOprotID32_t id32, double timestamp, void* rxdata)
527 {
528  timestamp = timestamp;
529 
530  int joint = eoprot_ID2index(id32);
531 
532  if(false == opened)
533  {
534  return false;
535  }
536 
537  // called by feat_manage_analogsensors_data() which is called by:
538  // eoprot_fun_UPDT_as_mais_status_the15values()
539  // the void* parameter inside this function is a eOas_arrayofupto36bytes_t*
540  // and can be treated as a EOarray
541 
542  eOmeas_position_t* multienc = (eOmeas_position_t*)rxdata;
543 
544  int startindex = joint * numofencperjoint;
545 
546  std::lock_guard<std::mutex> lck(mtx);
547  for(int i=0; i< numofencperjoint; i++)
548  {
549  analogdata[startindex + i]=((double) multienc[i])/encoderConversionFactor[startindex + i];
550  }
551 
552  return true;
553 }
554 
555 
556 
557 
558 bool embObjMultiEnc::close()
559 {
560  opened = false;
561 
562  cleanup();
563  return true;
564 }
565 
566 
567 void embObjMultiEnc::printServiceConfig(void)
568 {
569 // char loc[20] = {0};
570 // char fir[20] = {0};
571 // char pro[20] = {0};
572 //
573 // const char * boardname = (NULL != res) ? (res->getProperties().boardnameString) : ("NOT-ASSIGNED-YET");
574 // const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString) : ("NOT-ASSIGNED-YET");
575 //
576 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.canloc, loc, sizeof(loc));
577 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.firmware, fir, sizeof(fir));
578 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.protocol, pro, sizeof(pro));
579 //
580 // yInfo() << "The embObjMais device using BOARD" << boardname << "w/ IP" << ipv4 << "has the following service config:";
581 // yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate;
582 // yInfo() << "- MAIS named" << serviceConfig.nameOfMais << "@" << loc << "with required protocol version =" << pro << "and required firmware version =" << fir;
583 }
584 
585 
586 void embObjMultiEnc::cleanup(void)
587 {
588  if(ethManager == NULL) return;
589 
590  int ret = ethManager->releaseResource2(res, this);
591  res = NULL;
592  if(ret == -1)
593  ethManager->killYourself();
594 }
595 
596 // eof
597 
static TheEthManager * instance()
Definition: ethManager.cpp:159
bool NOT_YET_IMPLEMENTED(const char *txt)
static int v
Definition: iCub_Sim.cpp:42
double sat(const double val, const double min, const double max)
Definition: utils.h:183
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
iethresType_t
Definition: IethResource.h:61
@ iethres_analogmultienc
Definition: IethResource.h:70
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