iCub-main
Loading...
Searching...
No Matches
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
47using namespace yarp;
48using namespace yarp::os;
49using namespace yarp::dev;
50
51
52inline 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
60bool 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
82bool 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
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
156{
157 analogdata.resize(0);
158
159 if(NULL != parser)
160 {
161 delete parser;
162 parser = NULL;
163 }
164
165}
166
167
169{
170 return opened;
171}
172
173//#define TEST_MAIS_PLUS_MC
174
175
176bool 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
340bool 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
388bool 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
429void embObjMais::resetCounters()
430{
431 counterSat=0;
432 counterError=0;
433 counterTimeout=0;
434}
435
436
437void embObjMais::getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
438{
439 sat=counterSat;
440 err=counterError;
441 to=counterTimeout;
442}
443
445 return 1;
446}
447
448yarp::dev::MAS_status embObjMais::getEncoderArrayStatus(size_t sens_index) const {
449if (sens_index >= 1) return yarp::dev::MAS_UNKNOWN;
450 return yarp::dev::MAS_OK;
451}
452
453bool 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
459bool 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
467size_t embObjMais::getEncoderArraySize(size_t sens_index) const {
468 if (sens_index >= 1) return 0;
469 return analogdata.size();
470}
471
472
477
478
479bool 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
523{
524 opened = false;
525
526 cleanup();
527 return true;
528}
529
530
531void 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
550void 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
bool convert(std::string const &fromstring, eOmc_actuator_t &toactuatortype, bool &formaterror)
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 const Properties & getProperties()=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
int releaseResource2(eth::AbstractEthResource *ethresource, IethResource *interface)
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
static bool killYourself()
static TheEthManager * instance()
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
virtual bool initialised()
virtual bool getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
virtual bool getEncoderArrayName(size_t sens_index, std::string &name) const override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual size_t getNrOfEncoderArrays() const override
bool open(yarp::os::Searchable &config)
virtual yarp::dev::MAS_status getEncoderArrayStatus(size_t sens_index) const override
virtual eth::iethresType_t type()
virtual size_t getEncoderArraySize(size_t sens_index) const override
bool NOT_YET_IMPLEMENTED(const char *txt)
double sat(const double val, const double min, const double max)
Definition utils.h:183
iethresType_t
@ iethres_analogmais
Copyright (C) 2008 RobotCub Consortium.
out
Definition sine.m:8
eOmn_serv_parameter_t ethservice
std::string nameOfMais