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