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
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
23using namespace yarp::dev;
24using namespace yarp::os;
25
26YARP_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
86bool 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
171bool 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
218bool 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.
225bool 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, "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
515bool 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, "attach to subdevice failed");
545 }
546 return true;
547}
548
550{
551 return true;
552}
553
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
virtual bool read(yarp::sig::Vector &out)=0
Read a vector from the sensor.
virtual bool getChannels(int *nc)=0
Get the number of channels of the sensor.
virtual bool calibrate(int ch, double v)=0
Calibrate the sensor, single channel.
virtual yarp::os::Stamp getLastInputStamp()=0
Return the time stamp relative to the last acquisition.
A container for a device driver.
Definition: PolyDriver.h:23
bool close() override
Close the DeviceDriver.
Definition: PolyDriver.cpp:173
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
bool open(const std::string &txt)
Construct and configure a device by its common name.
Definition: PolyDriver.cpp:140
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:64
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
T & get()
A synonym of PortWriterBuffer::prepare.
void write(bool forceStrict=false)
Try to write the last buffer returned by PortWriterBuffer::get.
void attach(Port &port)
Set the Port to which objects will be written.
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 topic(const std::string &name)
Set topic to publish to.
Definition: Publisher.h:63
void write(bool forceStrict=false)
Write the current object being returned by Publisher::prepare.
Definition: Publisher.h:148
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::Publisher::write.
Definition: Publisher.h:123
A base class for nested structures that can be searched.
Definition: Searchable.h:56
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 Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
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
bool stop()
Stop the thread.
Definition: Thread.cpp:81
bool isStopping()
Returns true if the thread is stopping (Thread::stop has been called).
Definition: Thread.cpp:99
bool start()
Start the new thread running.
Definition: Thread.cpp:93
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