iCub-main
embObjMultipleFTsensors.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  * Author: Luca Tricerri
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
10 #include <ethManager.h>
11 
12 #include <yarp/os/Log.h>
13 #include <yarp/os/LogStream.h>
14 
15 #include <iostream>
16 #include <string>
17 
18 #include "EOnv_hid.h"
19 #include "EoAnalogSensors.h"
20 #include "EoProtocol.h"
21 #include "EoProtocolAS.h"
22 #include "EoProtocolMN.h"
23 
24 #ifdef WIN32
25 #pragma warning(once : 4355)
26 #endif
27 
28 using namespace yarp;
29 using namespace yarp::os;
30 using namespace yarp::dev;
31 
32 embObjMultipleFTsensors::embObjMultipleFTsensors()
33 {
34  yInfo() << "MultipleFTSensors has been created";
35  device_ = std::make_shared<yarp::dev::embObjDevPrivData>("embObjMultipleFTsensors");
36 }
37 
38 embObjMultipleFTsensors::embObjMultipleFTsensors(std::shared_ptr<yarp::dev::embObjDevPrivData> device) : device_(device)
39 {
40 }
41 
43 {
44  close();
45 }
46 
48 {
49  return device_->isOpen();
50 }
51 
52 bool embObjMultipleFTsensors::open(yarp::os::Searchable &config)
53 {
54  yInfo() << "embObjMultipleFTsensors::open(): preparing ETH resource";
55  if (!device_->prerareEthService(config, this))
56  return false;
57 
58  yInfo() << device_->getBoardInfo() << " embObjMultipleFTsensors::open(): browsing xml files which describe the service";
60  if (!parser.parse(config))
61  {
62  yError() << device_->getBoardInfo() << "open() fails to parse xml... cannot continue ";
63  return false;
64  }
65 
66  yInfo() << device_->getBoardInfo() << " embObjMultipleFTsensors::open(): verify the presence of the board and if its protocol version is correct";
67  if (!device_->res->verifyEPprotocol(eoprot_endpoint_analogsensors))
68  {
69  yError() << device_->getBoardInfo() << " open() fails to verifyEPprotocol... cannot continue ";
70  cleanup();
71  return false;
72  }
73 
74  yInfo() << device_->getBoardInfo() << " embObjMultipleFTsensors::open(): verify and activate the FT service";
75  eOmn_serv_parameter_t ftData;
76  ftData.configuration.type = eomn_serv_AS_ft;
77  ftData.configuration.diagnosticsmode = eomn_serv_diagn_mode_NONE;
78  ftData.configuration.diagnosticsparam = 0;
79  parser.toEomn(ftData.configuration.data.as.ft);
80  if (!device_->res->serviceVerifyActivate(eomn_serv_category_ft, &ftData, 5.0))
81  {
82  yError() << device_->getBoardInfo() << " open() fails to serviceVerifyActivate... cannot continue ";
83  cleanup();
84  return false;
85  }
86 
87  yInfo() << device_->getBoardInfo() << " embObjMultipleFTsensors::open(): configure the FT service";
88  if (false == sendConfig2boards(parser, device_->res))
89  {
90  yError() << device_->getBoardInfo() << " open() fails to sendConfig2boards... cannot continue";
91  cleanup();
92  return false;
93  }
94 
95  yInfo() << device_->getBoardInfo() << " embObjMultipleFTsensors::open(): impose the network variable which the ETH bord must stream up";
96  if (false == initRegulars(parser, device_->res))
97  {
98  yError() << device_->getBoardInfo() << " open() fails to initRegulars... cannot continue";
99  cleanup();
100  return false;
101  }
102 
103  yInfo() << device_->getBoardInfo() << " embObjMultipleFTsensors::open(): start the FT service";
104  if (!device_->res->serviceStart(eomn_serv_category_ft))
105  {
106  yError() << device_->getBoardInfo() << " open() fails to serviceStart... cannot continue";
107  cleanup();
108  return false;
109  }
110  else
111  {
112  if (device_->isVerbose())
113  {
114  yDebug() << device_->getBoardInfo() << " open() correctly starts service";
115  }
116  }
117 
118  yInfo() << device_->getBoardInfo() << " embObjMultipleFTsensors::open(): start streaming of FT data";
119  if (!sendStart2boards(parser, device_->res))
120  {
121  yError() << device_->getBoardInfo() << " open() fails to sendStart2boards... cannot continue";
122  cleanup();
123  return false;
124  }
125 
126  device_->setOpen(true);
127  return true;
128 }
129 
131 {
132  auto &ftInfos = parser.getFtInfo();
133  int index = 0;
134  for (const auto &[id, data] : ftInfos)
135  {
136  eOprotID32_t id32 = eo_prot_ID32dummy;
137  eOas_ft_config_t cfg;
138  cfg.ftperiod = data.ftAcquisitionRate;
139  cfg.temperatureperiod = data.temperatureAcquisitionRate / 1000;
140  cfg.mode = data.useCalibration;
141  cfg.calibrationset = 0;
142  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_ft, index, eoprot_tag_as_ft_config);
143 
144  if (false == deviceRes->setcheckRemoteValue(id32, &cfg, 10, 0.010, 0.050))
145  {
146  yError() << device_->getBoardInfo() << " sendConfig2boards() while try to configure ftPeriod=" << cfg.ftperiod;
147  return false;
148  }
149 
150  if (device_->isVerbose())
151  {
152  yDebug() << device_->getBoardInfo() << " sendConfig2boards() correctly configured boards with ftPeriod=" << cfg.ftperiod;
153  }
154  ++index;
155 
156  eOprotIndex_t eoprotIndex = eoprot_ID2index(id32);
157  std::unique_lock<std::shared_mutex> lck(mutex_);
158  ftSensorsData_[eoprotIndex] = {{0, 0, 0, 0, 0, 0}, 0, id,data.frameName};
159  }
160  return true;
161 }
162 
164 {
165  eOprotID32_t id32 = eo_prot_ID32dummy;
166 
167  uint8_t enable = 1;
168 
169  const auto &ftInfos = parser.getFtInfo();
170  int index = 0;
171  for (const auto &[id, data] : ftInfos)
172  {
173  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_ft, index, eoprot_tag_as_ft_cmmnds_enable);
174 
175  if (false == deviceRes->setcheckRemoteValue(id32, &enable, 10, 0.010, 0.050))
176  {
177  yError() << device_->getBoardInfo() << " sendStart2boards() while try to enable the boards transmission";
178  return false;
179  }
180 
181  if (device_->isVerbose())
182  {
183  yDebug() << device_->getBoardInfo() << " sendStart2boards() correctly enabled the boards transmission";
184  }
185  ++index;
186  }
187  return true;
188 }
189 
191 {
192  // configure regular rops
193 
194  vector<eOprotID32_t> id32v;
195  eOprotID32_t id32 = eo_prot_ID32dummy;
196 
197  const auto &ftInfos = parser.getFtInfo();
198  int index = 0;
199  for (const auto &[id, data] : ftInfos)
200  {
201  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_ft, index, eoprot_tag_as_ft_status_timedvalue);
202  id32v.push_back(id32);
203  ++index;
204  }
205 
206  if (false == deviceRes->serviceSetRegulars(eomn_serv_category_ft, id32v))
207  {
208  yError() << device_->getBoardInfo() << " initRegulars() fails to add its variables to regulars: cannot proceed any further";
209  return false;
210  }
211 
212  if (device_->isVerbose())
213  {
214  yDebug() << device_->getBoardInfo() << " initRegulars() added" << id32v.size() << "regular rops ";
215  char nvinfo[128];
216  for (size_t r = 0; r < id32v.size(); r++)
217  {
218  uint32_t item = id32v.at(r);
219  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
220  yDebug() << device_->getBoardInfo() << "\t it added regular rop for" << nvinfo;
221  }
222  }
223 
224  return true;
225 }
226 
228 {
229  return eth::iethres_analogft;
230 }
231 
232 bool embObjMultipleFTsensors::update(eOprotID32_t id32, double timestamp, void *rxdata)
233 {
234  if (!device_->isOpen())
235  return false;
236 
237  eOprotIndex_t eoprotIndex = eoprot_ID2index(id32);
238  if (eoprotIndex > 3)
239  {
240  yError() << device_->getBoardInfo() << " update() index too big";
241  return false;
242  }
243 
244  eOprotEntity_t entity = eoprot_ID2entity(id32);
245  if (entity != eoprot_entity_as_ft)
246  {
247  yError() << device_->getBoardInfo() << " update() wrong entity";
248  return false;
249  }
250 
251  eOprotTag_t tag = eoprot_ID2tag(id32);
252  if (tag != eoprot_tag_as_ft_status_timedvalue)
253  {
254  yError() << device_->getBoardInfo() << " update() wrong tag";
255  return false;
256  }
257 
258  eOas_ft_timedvalue_t *data = (eOas_ft_timedvalue_t *)rxdata;
259  if (!checkUpdateTimeout(id32, data->age))
260  {
261  return false;
262  }
263 
264  std::unique_lock<std::shared_mutex> lck(mutex_);
265 
266  for (int index = 0; index < eoas_ft_6axis; ++index)
267  {
268  ftSensorsData_[eoprotIndex].data_[index] = data->values[index];
269  }
270  ftSensorsData_[eoprotIndex].timeStamp_ = data->age;
271 
272  temperaturesensordata_[eoprotIndex].data_ = data->temperature;
273  temperaturesensordata_[eoprotIndex].timeStamp_ = calculateBoardTime(data->age);
274  return true;
275 }
276 
278 {
279  cleanup();
280  return true;
281 }
282 
284 {
285  device_->cleanup(static_cast<eth::IethResource *>(this));
286 }
287 
288 bool embObjMultipleFTsensors::getSixAxisForceTorqueSensorMeasure(size_t sensorIndex, yarp::sig::Vector &out, double &timestamp) const
289 {
290  if (!device_->isOpen())
291  return false;
292 
293  std::shared_lock<std::shared_mutex> lck(mutex_);
294 
295  if (ftSensorsData_.find(sensorIndex) == ftSensorsData_.end())
296  {
297  yError() << device_->getBoardInfo() << " getSixAxisForceTorqueSensorMeasure() fails data for index:" << sensorIndex << " not found";
298  return false;
299  }
300 
301  FtData sensorData = ftSensorsData_.at(sensorIndex);
302 
303  out.resize(ftChannels_);
304  for (size_t k = 0; k < ftChannels_; k++)
305  {
306  out[k] = sensorData.data_[k];
307  }
308  timestamp = ftSensorsData_.at(sensorIndex).timeStamp_;
309  return true;
310 }
311 
313 {
314  return ftSensorsData_.size();
315 }
316 
317 yarp::dev::MAS_status embObjMultipleFTsensors::getSixAxisForceTorqueSensorStatus(size_t sensorindex) const
318 {
319  return masStatus_[sensorindex];
320 }
321 
322 bool embObjMultipleFTsensors::getSixAxisForceTorqueSensorName(size_t sensorindex, std::string &name) const
323 {
324  std::shared_lock<std::shared_mutex> lck(mutex_);
325  name = ftSensorsData_.at(sensorindex).sensorName_;
326  return true;
327 }
328 
329 bool embObjMultipleFTsensors::getSixAxisForceTorqueSensorFrameName(size_t sensorindex, std::string &frameName) const
330 {
331  std::shared_lock<std::shared_mutex> lck(mutex_);
332  frameName = ftSensorsData_.at(sensorindex).frameName_;
333  return true;
334 }
335 
337 {
338  return temperaturesensordata_.size();
339 }
340 
341 yarp::dev::MAS_status embObjMultipleFTsensors::getTemperatureSensorStatus(size_t sensorindex) const
342 {
343  return masStatus_[sensorindex];
344 }
345 
346 bool embObjMultipleFTsensors::getTemperatureSensorName(size_t sensorindex, std::string &name) const
347 {
348  std::shared_lock<std::shared_mutex> lck(mutex_);
349  name = ftSensorsData_.at(sensorindex).sensorName_;
350  return true;
351 }
352 
353 bool embObjMultipleFTsensors::getTemperatureSensorFrameName(size_t sensorindex, std::string &frameName) const
354 {
355  std::shared_lock<std::shared_mutex> lck(mutex_);
356  frameName = ftSensorsData_.at(sensorindex).frameName_;
357  return true;
358 }
359 
360 bool embObjMultipleFTsensors::getTemperatureSensorMeasure(size_t sensorIndex, double &out, double &timestamp) const
361 {
362  if (!device_->isOpen())
363  return false;
364 
365  std::shared_lock<std::shared_mutex> lck(mutex_);
366 
367  if (temperaturesensordata_.find(sensorIndex) == temperaturesensordata_.end())
368  {
369  yError() << device_->getBoardInfo() << " getTemperatureSensorMeasure() fails data for index:" << sensorIndex << " not found";
370  return false;
371  }
372 
373  out = 0.1 * temperaturesensordata_.at(sensorIndex).data_;
374  timestamp = temperaturesensordata_.at(sensorIndex).timeStamp_;
375  return true;
376 }
377 
378 bool embObjMultipleFTsensors::getTemperatureSensorMeasure(size_t sensorIndex, yarp::sig::Vector &out, double &timestamp) const
379 {
380  double value{0};
381  getTemperatureSensorMeasure(sensorIndex, value, timestamp);
382  out.resize(1);
383  out[0] = value;
384  return true;
385 }
386 
387 bool embObjMultipleFTsensors::checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
388 {
390  {
391  return true;
392  }
393 
394  eOabstime_t diff = current - timeoutUpdate_[id32];
395  if (timeoutUpdate_[id32] != 0 && current > timeoutUpdate_[id32] + updateTimeout_)
396  {
397  yError() << device_->getBoardInfo() << " update timeout for index:" << eoprot_ID2index(id32);
398  timeoutUpdate_[id32] = current;
399  masStatus_[eoprot_ID2index(id32)] = MAS_TIMEOUT;
400  return false;
401  }
402  masStatus_[eoprot_ID2index(id32)] = MAS_OK;
403  timeoutUpdate_[id32] = current;
404  return true;
405 }
406 
408 {
409  if (!useBoardTimeFlag_)
410  {
411  return yarp::os::Time::now();
412  }
413 
414  // Simulate real board time
415  if (firstYarpTimestamp_ == 0)
416  {
417  firstYarpTimestamp_ = yarp::os::Time::now();
418  firstCanTimestamp_ = current;
419  }
420  double realtime = firstYarpTimestamp_ + (double)(current - firstCanTimestamp_) / 1000000; // Simulate real board time
421  return realtime;
422 }
@ data
yarp::sig::Vector data_
bool parse(const yarp::os::Searchable &config)
std::map< std::string, FtInfo > & getFtInfo()
bool toEomn(eOmn_serv_config_data_as_ft_t &out) const
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 serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
std::vector< yarp::dev::MAS_status > masStatus_
virtual bool getTemperatureSensorFrameName(size_t sensorindex, std::string &frameName) const override
bool checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sensorindex) const override
bool initRegulars(ServiceParserMultipleFt &parser, eth::AbstractEthResource *deviceRes)
bool open(yarp::os::Searchable &config)
std::map< eOprotID32_t, FtData > ftSensorsData_
bool sendStart2boards(ServiceParserMultipleFt &parser, eth::AbstractEthResource *deviceRes)
virtual bool getTemperatureSensorMeasure(size_t sensorindex, double &out, double &timestamp) const override
virtual size_t getNrOfTemperatureSensors() const override
double calculateBoardTime(eOabstime_t current)
virtual bool getSixAxisForceTorqueSensorFrameName(size_t sensorindex, std::string &frameName) const override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
std::map< eOprotID32_t, TemperatureData > temperaturesensordata_
virtual size_t getNrOfSixAxisForceTorqueSensors() const override
static constexpr eOabstime_t updateTimeout_
bool sendConfig2boards(ServiceParserMultipleFt &parser, eth::AbstractEthResource *deviceRes)
virtual bool getSixAxisForceTorqueSensorMeasure(size_t sensorindex, yarp::sig::Vector &out, double &timestamp) const override
std::shared_ptr< yarp::dev::embObjDevPrivData > device_
std::map< eOprotID32_t, eOabstime_t > timeoutUpdate_
virtual bool getSixAxisForceTorqueSensorName(size_t sensorindex, std::string &name) const override
virtual yarp::dev::MAS_status getTemperatureSensorStatus(size_t sensorindex) const override
virtual bool getTemperatureSensorName(size_t sensorindex, std::string &name) const override
static constexpr int ftChannels_
iethresType_t
Definition: IethResource.h:61
@ iethres_analogft
Definition: IethResource.h:75
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8