YARP
Yet Another Robot Platform
ServerInertial.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 
7 #include "ServerInertial.h"
8 #include <cstdio>
9 
10 #include <yarp/os/BufferedPort.h>
11 #include <yarp/dev/PolyDriver.h>
13 #include <yarp/os/Time.h>
14 #include <yarp/os/Network.h>
15 #include <yarp/os/Thread.h>
16 #include <yarp/os/Vocab.h>
17 #include <yarp/os/Bottle.h>
19 #include <yarp/os/Log.h>
20 #include <yarp/os/LogStream.h>
21 
22 
23 using namespace yarp::dev;
24 using namespace yarp::os;
25 
26 YARP_LOG_COMPONENT(SERVERINERTIAL, "yarp.devices.ServerInertial")
27 
28 
29 
33  IMU_polydriver(nullptr),
34  ownDevices(false),
35  subDeviceOwned(nullptr)
36 {
37  IMU = nullptr;
38  spoke = false;
39  iTimed=nullptr;
40  period = 0.005;
41  prev_timestamp_counter=0;
42  curr_timestamp_counter=0;
43  trap = 0;
44  strict = false;
45  partName = "Server Inertial";
46 
47  // init ROS data
48  frame_id = "";
49  rosNodeName = "";
50  rosTopicName = "";
51  rosNode = nullptr;
52  rosMsgCounter = 0;
53  useROS = ROS_disabled;
54 
55  // init a fake covariance matrix
56  covariance.resize(9);
57  covariance.assign(9, 0);
58 
59 // rosData.angular_velocity.x = 0;
60 // rosData.angular_velocity.y = 0;
61 // rosData.angular_velocity.z = 0;
62 // rosData.angular_velocity_covariance = covariance;
63 
64 // rosData.linear_acceleration.x = 0;
65 // rosData.linear_acceleration.y = 0;
66 // rosData.linear_acceleration.z = 0;
67 // rosData.linear_acceleration_covariance = covariance;
68 
69 // rosData.orientation.x = 0;
70 // rosData.orientation.y = 0;
71 // rosData.orientation.z = 0;
72 // rosData.orientation.w = 0;
73 // rosData.orientation_covariance = covariance;
74 
75 // yCDebug(SERVERINERTIAL) << "covariance size is " << covariance.size();
76 }
77 
79 {
80  if (IMU != nullptr) {
81  close();
82  }
83 }
84 
85 
86 bool ServerInertial::checkROSParams(yarp::os::Searchable &config)
87 {
88  // check for ROS parameter group
89  if(!config.check("ROS") )
90  {
91  useROS = ROS_disabled;
92  yCInfo(SERVERINERTIAL) << "No ROS group found in config file ... skipping ROS initialization.";
93  return true;
94  }
95 
96  yCInfo(SERVERINERTIAL) << "ROS group was FOUND in config file.";
97 
98  Bottle &rosGroup = config.findGroup("ROS");
99  if(rosGroup.isNull())
100  {
101  yCError(SERVERINERTIAL) << partName << "ROS group params is not a valid group or empty";
102  useROS = ROS_config_error;
103  return false;
104  }
105 
106  // check for useROS parameter
107  if(!rosGroup.check("useROS"))
108  {
109  yCError(SERVERINERTIAL) << partName << " cannot find useROS parameter, mandatory when using ROS message. \n \
110  Allowed values are true, false, ROS_only";
111  useROS = ROS_config_error;
112  return false;
113  }
114  std::string ros_use_type = rosGroup.find("useROS").asString();
115  if(ros_use_type == "false")
116  {
117  yCInfo(SERVERINERTIAL) << partName << "useROS topic if set to 'false'";
118  useROS = ROS_disabled;
119  return true;
120  }
121  else if(ros_use_type == "true")
122  {
123  yCInfo(SERVERINERTIAL) << partName << "useROS topic if set to 'true'";
124  useROS = ROS_enabled;
125  }
126  else if(ros_use_type == "only")
127  {
128  yCInfo(SERVERINERTIAL) << partName << "useROS topic if set to 'only";
129  useROS = ROS_only;
130  }
131  else
132  {
133  yCInfo(SERVERINERTIAL) << partName << "useROS parameter is seet to unvalid value ('" << ros_use_type << "'), supported values are 'true', 'false', 'only'";
134  useROS = ROS_config_error;
135  return false;
136  }
137 
138  // check for ROS_nodeName parameter
139  if(!rosGroup.check("ROS_nodeName"))
140  {
141  yCError(SERVERINERTIAL) << partName << " cannot find ROS_nodeName parameter, mandatory when using ROS message";
142  useROS = ROS_config_error;
143  return false;
144  }
145  rosNodeName = rosGroup.find("ROS_nodeName").asString(); // TODO: check name is correct
146  yCInfo(SERVERINERTIAL) << partName << "rosNodeName is " << rosNodeName;
147 
148  // check for ROS_topicName parameter
149  if(!rosGroup.check("ROS_topicName"))
150  {
151  yCError(SERVERINERTIAL) << partName << " cannot find ROS_topicName parameter, mandatory when using ROS message";
152  useROS = ROS_config_error;
153  return false;
154  }
155  rosTopicName = rosGroup.find("ROS_topicName").asString();
156  yCInfo(SERVERINERTIAL) << partName << "ROS_topicName is " << rosTopicName;
157 
158  // check for frame_id parameter
159  if(!rosGroup.check("frame_id"))
160  {
161  yCError(SERVERINERTIAL) << partName << " cannot find frame_id parameter, mandatory when using ROS message";
162  useROS = ROS_config_error;
163  return false;
164  }
165  frame_id = rosGroup.find("frame_id").asString();
166  yCInfo(SERVERINERTIAL) << partName << "frame_id is " << frame_id;
167 
168  return true;
169 }
170 
171 bool ServerInertial::initialize_ROS()
172 {
173  bool success = false;
174  switch(useROS)
175  {
176  case ROS_enabled:
177  case ROS_only:
178  {
179  rosNode = new yarp::os::Node(rosNodeName); // add a ROS node
180 
181  if(rosNode == nullptr)
182  {
183  yCError(SERVERINERTIAL) << " opening " << rosNodeName << " Node, check your yarp-ROS network configuration";
184  success = false;
185  break;
186  }
187 
188  if (!rosPublisherPort.topic(rosTopicName) )
189  {
190  yCError(SERVERINERTIAL) << " opening " << rosTopicName << " Topic, check your yarp-ROS network configuration";
191  success = false;
192  break;
193  }
194  success = true;
195  } break;
196 
197  case ROS_disabled:
198  {
199  yCInfo(SERVERINERTIAL) << partName << ": no ROS initialization required";
200  success = true;
201  } break;
202 
203  case ROS_config_error:
204  {
205  yCError(SERVERINERTIAL) << partName << " ROS parameter are not correct, check your configuration file";
206  success = false;
207  } break;
208 
209  default:
210  {
211  yCError(SERVERINERTIAL) << partName << " something went wrong with ROS configuration, we should never be here!!!";
212  success = false;
213  } break;
214  }
215  return success;
216 }
217 
218 bool ServerInertial::openDeferredAttach(yarp::os::Property& prop)
219 {
220  return true;
221 }
222 
223 // If a subdevice parameter is given to the wrapper, it will open it as well
224 // and attach to it immediately.
225 bool ServerInertial::openAndAttachSubDevice(yarp::os::Property& prop)
226 {
227  yarp::os::Value &subdevice = prop.find("subdevice");
228  IMU_polydriver = new yarp::dev::PolyDriver;
229 
230  yCDebug(SERVERINERTIAL, "Subdevice %s", subdevice.toString().c_str());
231  if (subdevice.isString())
232  {
233  // maybe user isn't doing nested configuration
235  p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
236  p.fromString(prop.toString());
237  p.put("device",subdevice.toString());
238  IMU_polydriver->open(p);
239  } else {
240  IMU_polydriver->open(subdevice);
241  }
242 
243  if (!IMU_polydriver->isValid())
244  {
245  yCError(SERVERINERTIAL, "cannot create device <%s>", subdevice.toString().c_str());
246  return false;
247  }
248 
249  // if we are here, poly is valid
250  IMU_polydriver->view(IMU); // attach to subdevice
251  if(IMU == nullptr)
252  {
253  yCError(SERVERINERTIAL, "Error, subdevice <%s> has no valid IMU interface", subdevice.toString().c_str());
254  IMU_polydriver->close();
255  return false;
256  }
257 
258  // iTimed interface
259  IMU_polydriver->view(iTimed); // not mandatory
260  return true;
261 }
262 
274 {
275  yCWarning(SERVERINERTIAL) << "The 'inertial' device is deprecated in favour of 'multipleanalogsensorsremapper' + 'multipleanalogsensorsserver' + 'IMURosPublisher'.";
276  yCWarning(SERVERINERTIAL) << "The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.";
277  yCWarning(SERVERINERTIAL) << "Please update your scripts.";
278 
279  Property prop;
280  prop.fromString(config.toString());
281 
282  p.setReader(*this);
283 
284  period = config.check("period",yarp::os::Value(0.005),"maximum period").asFloat64();
285  strict = config.check("strict",yarp::os::Value(false),"strict write").asBool();
286 
287  //Look for the device name (serial Port). Usually /dev/ttyUSB0
288  // check if we need to create subdevice or if they are
289  // passed later on thorugh attachAll()
290  if(prop.check("subdevice"))
291  {
292  ownDevices=true;
293  if(! openAndAttachSubDevice(prop))
294  {
295  yCError(SERVERINERTIAL, "ControlBoardWrapper: error while opening subdevice");
296  return false;
297  }
298  }
299  else
300  {
301  ownDevices=false;
302  if (!openDeferredAttach(prop)) {
303  return false;
304  }
305  }
306 
307 
308  checkROSParams(config);
309 
310 
311 
312  //Look for the portname to register (--name option), defaulting to /inertial if missing
313  std::string portName;
314  if(useROS != ROS_only)
315  {
316  if (config.check("name")) {
317  portName = config.find("name").asString();
318  } else {
319  yCInfo(SERVERINERTIAL) << "Using default values for port name, you can change it by using '--name /myPortName' parameter";
320  portName = "/inertial";
321  }
322 
323  if(!p.open(portName))
324  {
325  yCError(SERVERINERTIAL) << "ServerInertial, cannot open port " << portName;
326  return false;
327  }
328  writer.attach(p);
329  }
330 
331  // call ROS node/topic initialization, if needed
332  if(!initialize_ROS() )
333  {
334  return false;
335  }
336 
337  if( (ownDevices) && (IMU!=nullptr) )
338  {
339  start();
340  }
341 
342  return true;
343 }
344 
346 {
347  yCInfo(SERVERINERTIAL, "Closing Server Inertial...");
348  stop();
349 
350  if(rosNode!=nullptr) {
351  rosNode->interrupt();
352  delete rosNode;
353  rosNode = nullptr;
354  }
355 
356  if( (ownDevices) && (IMU_polydriver != nullptr) )
357  {
358  IMU_polydriver->close();
359  IMU = nullptr;
360  }
361  return true;
362 }
363 
364 
366 {
367  if (IMU==nullptr)
368  {
369  return false;
370  }
371  else
372  {
373  int nchannels;
374  IMU->getChannels (&nchannels);
375 
376  yarp::sig::Vector indata(nchannels);
377  bool worked(false);
378 
379  worked=IMU->read(indata);
380  if (worked)
381  {
382  bot.clear();
383 
384  // Euler+accel+gyro+magn orientation values
385  for (int i = 0; i < nchannels; i++) {
386  bot.addFloat64(indata[i]);
387  }
388  }
389  else
390  {
391  bot.clear(); //dummy info.
392  }
393 
394  return(worked);
395  }
396 }
397 
399 {
400  double before, now;
401  yCInfo(SERVERINERTIAL, "Starting server Inertial thread");
402  while (!isStopping())
403  {
404  before = yarp::os::Time::now();
405  if (IMU!=nullptr)
406  {
407  Bottle imuData;
408  bool res = getInertial(imuData);
409 
410  // publish data on YARP port if required
411  if(useROS != ROS_only)
412  {
413  yarp::os::Bottle& bot = writer.get();
414  bot = imuData;
415  if (res)
416  {
417  static yarp::os::Stamp ts;
418  if (iTimed) {
419  ts=iTimed->getLastInputStamp();
420  } else {
421  ts.update();
422  }
423 
424 
425  curr_timestamp_counter = ts.getCount();
426 
427  if (curr_timestamp_counter!=prev_timestamp_counter)
428  {
429  if (!spoke)
430  {
431  yCDebug(SERVERINERTIAL, "Writing an Inertial measurement.");
432  spoke = true;
433  }
434  p.setEnvelope(ts);
435  writer.write(strict);
436  }
437  else
438  {
439  trap++;
440  }
441 
442  prev_timestamp_counter = curr_timestamp_counter;
443  }
444  }
445 
446  // publish ROS topic if required
447  if(useROS != ROS_disabled)
448  {
449  double euler_xyz[3], quaternion[4];
450 
451  euler_xyz[0] = imuData.get(0).asFloat64();
452  euler_xyz[1] = imuData.get(1).asFloat64();
453  euler_xyz[2] = imuData.get(2).asFloat64();
454 
455  convertEulerAngleYXZdegrees_to_quaternion(euler_xyz, quaternion);
456 
457  yarp::rosmsg::sensor_msgs::Imu &rosData = rosPublisherPort.prepare();
458 
459  rosData.header.seq = rosMsgCounter++;
460  rosData.header.stamp = yarp::os::Time::now();
461  rosData.header.frame_id = frame_id;
462 
463  rosData.orientation.x = quaternion[0];
464  rosData.orientation.y = quaternion[1];
465  rosData.orientation.z = quaternion[2];
466  rosData.orientation.w = quaternion[3];
467  rosData.orientation_covariance = covariance;
468 
469  rosData.linear_acceleration.x = imuData.get(3).asFloat64(); // [m/s^2]
470  rosData.linear_acceleration.y = imuData.get(4).asFloat64(); // [m/s^2]
471  rosData.linear_acceleration.z = imuData.get(5).asFloat64(); // [m/s^2]
472  rosData.linear_acceleration_covariance = covariance;
473 
474  rosData.angular_velocity.x = imuData.get(6).asFloat64(); // to be converted into rad/s (?) - verify with users
475  rosData.angular_velocity.y = imuData.get(7).asFloat64(); // to be converted into rad/s (?) - verify with users
476  rosData.angular_velocity.z = imuData.get(8).asFloat64(); // to be converted into rad/s (?) - verify with users
477  rosData.angular_velocity_covariance = covariance;
478 
479  rosPublisherPort.write();
480  }
481  }
482 
485  if ((now - before) < period) {
486  double k = period-(now-before);
488  }
489  }
490  yCInfo(SERVERINERTIAL, "Server Intertial thread finished");
491 }
492 
494 {
495  yarp::os::Bottle cmd, response;
496  cmd.read(connection);
497  yCTrace(SERVERINERTIAL, "command received: %s", cmd.toString().c_str());
498 
499  // We receive a command but don't do anything with it.
500  return true;
501 }
502 
504 {
505  if (IMU == nullptr) { return false; }
506  return IMU->read (out);
507 }
508 
510 {
511  if (IMU == nullptr) { return false; }
512  return IMU->getChannels (nc);
513 }
514 
515 bool ServerInertial::calibrate(int ch, double v)
516 {
517  if (IMU==nullptr) {return false;}
518  return IMU->calibrate(ch, v);
519 }
520 
521 
523 {
525  if(!poly)
526  {
527  yCError(SERVERINERTIAL, "ServerInertial: device to attach to is not valid!");
528  return false;
529  }
530  IMU_polydriver = poly;
531  IMU_polydriver->view(IMU);
532 
533  // iTimed interface
534  IMU_polydriver->view(iTimed); // not mandatory
535 
536  if(IMU != nullptr)
537  {
538  if (!Thread::isRunning()) {
539  start();
540  }
541  }
542  else
543  {
544  yCError(SERVERINERTIAL, "ControlBoardWrapper: attach to subdevice failed");
545  }
546  return true;
547 }
548 
550 {
551  return true;
552 }
553 
554 bool ServerInertial::attachAll(const PolyDriverList &imuToAttachTo)
555 {
556  if (imuToAttachTo.size() != 1)
557  {
558  yCError(SERVERINERTIAL, "ServerInertial: cannot attach more than one device");
559  return false;
560  }
561 
562  return attach(imuToAttachTo[0]->poly);
563 }
564 
566 {
567  IMU = nullptr;
568  return true;
569 }
const yarp::os::LogComponent & SERVERINERTIAL()
inertial deprecated: Export an inertial sensor.
bool attach(yarp::dev::PolyDriver *poly) override
IWrapper interface Attach to another object.
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the device driver.
bool detach() override
Detach the object (you must have first called attach).
virtual bool getInertial(yarp::os::Bottle &bot)
bool calibrate(int ch, double v) override
Calibrate the sensor, single channel.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
void run() override
Main body of the new thread.
~ServerInertial() override
bool attachAll(const yarp::dev::PolyDriverList &p) override
IMultipleWrapper interface Attach to a list of objects.
bool getChannels(int *nc) override
Get the number of channels of the sensor.
bool detachAll() override
Detach the object (you must have first called attach).
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
A container for a device driver.
Definition: PolyDriver.h:23
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition: Bottle.cpp:240
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition: Bottle.cpp:158
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Bottle.cpp:277
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:121
bool isNull() const override
Checks if the object is invalid.
Definition: Bottle.cpp:370
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:211
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition: Bottle.cpp:287
An interface for reading from a network connection.
The Node class.
Definition: Node.h:23
void interrupt()
interrupt delegates the call to the Node port interrupt.
Definition: Node.cpp:597
A class for storing options and configuration information.
Definition: Property.h:33
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition: Property.cpp:1051
std::string toString() const override
Return a standard text representation of the content of the object.
Definition: Property.cpp:1069
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1063
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Property.cpp:1041
bool write(ConnectionWriter &writer) const override
Write this object to a network connection.
Definition: Property.cpp:1134
A base class for nested structures that can be searched.
Definition: Searchable.h:63
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:21
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition: Stamp.cpp:124
int getCount() const
Get the sequence number.
Definition: Stamp.cpp:29
A single value (typically within a Bottle).
Definition: Value.h:43
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:222
virtual bool isString() const
Checks if value is a string.
Definition: Value.cpp:156
std::string toString() const override
Return a standard text representation of the content of the object.
Definition: Value.cpp:356
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
yarp::conf::float64_t y
Definition: Vector3.h:38
yarp::conf::float64_t z
Definition: Vector3.h:39
yarp::conf::float64_t x
Definition: Vector3.h:37
yarp::rosmsg::std_msgs::Header header
Definition: Imu.h:55
std::vector< yarp::conf::float64_t > orientation_covariance
Definition: Imu.h:57
yarp::rosmsg::geometry_msgs::Vector3 angular_velocity
Definition: Imu.h:58
std::vector< yarp::conf::float64_t > linear_acceleration_covariance
Definition: Imu.h:61
yarp::rosmsg::geometry_msgs::Vector3 linear_acceleration
Definition: Imu.h:60
yarp::rosmsg::geometry_msgs::Quaternion orientation
Definition: Imu.h:56
std::vector< yarp::conf::float64_t > angular_velocity_covariance
Definition: Imu.h:59
yarp::rosmsg::TickTime stamp
Definition: Header.h:45
yarp::os::Node * rosNode
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCTrace(component,...)
Definition: LogComponent.h:84
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition: Time.cpp:111
An interface to the operating system, including Port based communication.
bool convertEulerAngleYXZdegrees_to_quaternion(double *eulerXYZ, double *quaternion)
Definition: yarpRosHelper.h:67
@ ROS_only
Definition: yarpRosHelper.h:20
@ ROS_config_error
Definition: yarpRosHelper.h:17
@ ROS_enabled
Definition: yarpRosHelper.h:19
@ ROS_disabled
Definition: yarpRosHelper.h:18