YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
realsense2Tracking.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3 * All rights reserved.
4 *
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
12#include <yarp/os/Value.h>
13
14#include <yarp/sig/ImageUtils.h>
15
17#include <yarp/math/Math.h>
18
19#include <algorithm>
20#include <cmath>
21#include <cstdint>
22#include <iomanip>
23#include <librealsense2/rsutil.h>
24#include <librealsense2/rs.hpp>
25#include <mutex>
26
27 /**********************************************************************************************************/
28 // This software module is experimental.
29 // It is provided with uncomplete documentation and it may be modified/renamed/removed without any notice.
30 /**********************************************************************************************************/
31
32using namespace yarp::dev;
33using namespace yarp::sig;
34using namespace yarp::os;
35
36using namespace std;
37
38namespace {
39YARP_LOG_COMPONENT(REALSENSE2TRACKING, "yarp.device.realsense2Tracking")
40}
41
42
43struct float3 {
44 float x;
45 float y;
46 float z;
47
49 {
50 return { x * t, y * t, z * t };
51 }
52
54 {
55 return { x - t, y - t, z - t };
56 }
57
58 void operator*=(float t)
59 {
60 x = x * t;
61 y = y * t;
62 z = z * t;
63 }
64
65 void add(float t1, float t2, float t3)
66 {
67 x += t1;
68 y += t2;
69 z += t3;
70 }
71};
72
73//---------------------------------------------------------------
74
75#if 0
76static std::string get_device_information(const rs2::device& dev)
77{
78
79 std::stringstream ss;
80 ss << "Device information: " << std::endl;
81
82 for (int i = 0; i < static_cast<int>(RS2_CAMERA_INFO_COUNT); i++) {
83 auto info_type = static_cast<rs2_camera_info>(i);
84 ss << " " << std::left << std::setw(20) << info_type << " : ";
85
86 if (dev.supports(info_type))
87 ss << dev.get_info(info_type) << std::endl;
88 else
89 ss << "N/A" << std::endl;
90 }
91 return ss.str();
92}
93
94static bool setOption(rs2_option option, const rs2::sensor* sensor, float value)
95{
96
97 if (!sensor) {
98 return false;
99 }
100
101 // First, verify that the sensor actually supports this option
102 if (!sensor->supports(option)) {
103 yCError(REALSENSE2TRACKING) << "The option" << rs2_option_to_string(option) << "is not supported by this sensor";
104 return false;
105 }
106
107 // To set an option to a different value, we can call set_option with a new value
108 try {
109 sensor->set_option(option, value);
110 } catch (const rs2::error& e) {
111 // Some options can only be set while the camera is streaming,
112 // and generally the hardware might fail so it is good practice to catch exceptions from set_option
113 yCError(REALSENSE2TRACKING) << "Failed to set option " << rs2_option_to_string(option) << ". (" << e.what() << ")";
114 return false;
115 }
116 return true;
117}
118
119static bool getOption(rs2_option option, const rs2::sensor* sensor, float& value)
120{
121 if (!sensor) {
122 return false;
123 }
124
125 // First, verify that the sensor actually supports this option
126 if (!sensor->supports(option)) {
127 yCError(REALSENSE2TRACKING) << "The option" << rs2_option_to_string(option) << "is not supported by this sensor";
128 return false;
129 }
130
131 // To set an option to a different value, we can call set_option with a new value
132 try {
133 value = sensor->get_option(option);
134 } catch (const rs2::error& e) {
135 // Some options can only be set while the camera is streaming,
136 // and generally the hardware might fail so it is good practice to catch exceptions from set_option
137 yCError(REALSENSE2TRACKING) << "Failed to get option " << rs2_option_to_string(option) << ". (" << e.what() << ")";
138 return false;
139 }
140 return true;
141}
142
143static void settingErrorMsg(const string& error, bool& ret)
144{
145 yCError(REALSENSE2TRACKING) << error.c_str();
146 ret = false;
147}
148#endif
149
153
154bool realsense2Tracking::pipelineStartup()
155{
156 try {
157 m_profile = m_pipeline.start(m_cfg);
158 } catch (const rs2::error& e) {
159 yCError(REALSENSE2TRACKING) << "Failed to start the pipeline:"
160 << "(" << e.what() << ")";
161 m_lastError = e.what();
162 return false;
163 }
164 return true;
165}
166
167bool realsense2Tracking::pipelineShutdown()
168{
169 try {
170 m_pipeline.stop();
171 } catch (const rs2::error& e) {
172 yCError(REALSENSE2TRACKING) << "Failed to stop the pipeline:"
173 << "(" << e.what() << ")";
174 m_lastError = e.what();
175 return false;
176 }
177 return true;
178}
179
180bool realsense2Tracking::pipelineRestart()
181{
182 std::lock_guard<std::mutex> guard(m_mutex);
183 if (!pipelineShutdown())
184 return false;
185
186 return pipelineStartup();
187}
188
190{
191 yCWarning(REALSENSE2TRACKING) << "This software module is experimental.";
192 yCWarning(REALSENSE2TRACKING) << "It is provided with uncomplete documentation and it may be modified/renamed/removed without any notice.";
193
194 string sensor_is = "t265";
196 if (config.check("timestamp"))
197 {
198 string temp = config.find("timestamp").asString();
200 if (temp == "realsense") m_timestamp_type = timestamp_enumtype::rs_timestamp;
201 else
202 {
203 yCError(REALSENSE2TRACKING) << "Invalid value for option 'timestamp'. Valid values are 'yarp','realsense'";
204 return false;
205 }
206 }
207
208 bool b= true;
209
212 m_cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
213
214 b &= pipelineStartup();
215 if (b==false)
216 {
217 yCError(REALSENSE2TRACKING) << "Pipeline initialization failed";
218 return false;
219 }
220
221 return true;
222}
223
225{
226 pipelineShutdown();
227 return true;
228}
229
230//---------------------------------------------------------------------------------------------------------------
231/* IThreeAxisGyroscopes methods */
233{
234 return 1;
235}
236
242
244{
245 if (sens_index != 0) { return false; }
247 return true;
248}
249
250bool realsense2Tracking::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string& frameName) const
251{
252 if (sens_index != 0) { return false; }
253 frameName = m_gyroFrameName;
254 return true;
255}
256
258{
259 if (sens_index != 0) {
260 return false;
261 }
262
263 std::lock_guard<std::mutex> guard(m_mutex);
264 rs2::frameset dataframe;
265 try
266 {
267 dataframe = m_pipeline.wait_for_frames();
268 }
269 catch (const rs2::error& e)
270 {
271 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
272 m_lastError = e.what();
273 return false;
274 }
275 auto fg = dataframe.first_or_default(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
276 rs2::motion_frame gyro = fg.as<rs2::motion_frame>();
277 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
278 else if (m_timestamp_type == rs_timestamp) { timestamp = gyro.get_timestamp(); }
279 else timestamp=0;
280 m_last_gyro = gyro.get_motion_data();
281 out.resize(3);
282 out[0] = m_last_gyro.x;
283 out[1] = m_last_gyro.y;
284 out[2] = m_last_gyro.z;
285 return true;
286}
287
288//-------------------------------------------------------------------------------------------------------
289
290/* IThreeAxisLinearAccelerometers methods */
292{
293 return 1;
294}
295
301
303{
304 if (sens_index != 0) { return false; }
306 return true;
307}
308
310{
311 if (sens_index != 0) { return false; }
312 frameName = m_accelFrameName;
313 return true;
314}
315
317{
318 if (sens_index != 0) { return false; }
319
320 std::lock_guard<std::mutex> guard(m_mutex);
321 rs2::frameset dataframe;
322 try
323 {
324 dataframe = m_pipeline.wait_for_frames();
325 }
326 catch (const rs2::error& e)
327 {
328 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
329 m_lastError = e.what();
330 return false;
331 }
332 auto fa = dataframe.first_or_default(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
333 rs2::motion_frame accel = fa.as<rs2::motion_frame>();
334 m_last_accel = accel.get_motion_data();
335 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
336 else if (m_timestamp_type == rs_timestamp) { timestamp = accel.get_timestamp(); }
337 else timestamp = 0;
338 out.resize(3);
339 out[0] = m_last_accel.x;
340 out[1] = m_last_accel.y;
341 out[2] = m_last_accel.z;
342 return true;
343}
344
345/* IThreeAxisAngularAcclerometer Sensors methods */
350
356
358{
359 if (sens_index != 0) { return false; }
361 return true;
362}
363
365{
366 if (sens_index != 0) { return false; }
367 frameName = m_poseFrameName;
368 return true;
369}
370
371
373{
374 if (sens_index != 0) { return false; }
375
376 std::lock_guard<std::mutex> guard(m_mutex);
377 rs2::frameset dataframe;
378 try
379 {
380 dataframe = m_pipeline.wait_for_frames();
381 }
382 catch (const rs2::error& e)
383 {
384 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
385 m_lastError = e.what();
386 return false;
387 }
388 auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
389 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
390 m_last_pose = pose.get_pose_data();
391 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
392 else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); }
393 else timestamp = 0;
394 out.resize(3);
395 out[0] = m_last_pose.angular_acceleration.x;
396 out[1] = m_last_pose.angular_acceleration.y;
397 out[2] = m_last_pose.angular_acceleration.z;
398
399 return true;
400}
401
402
403
404//-------------------------------------------------------------------------------------------------------
405
406/* IOrientationSensors methods */
408{
409 return 1;
410}
411
417
418bool realsense2Tracking::getOrientationSensorName(size_t sens_index, std::string& name) const
419{
420 if (sens_index != 0) { return false; }
422 return true;
423}
424
425bool realsense2Tracking::getOrientationSensorFrameName(size_t sens_index, std::string& frameName) const
426{
427 if (sens_index != 0) { return false; }
428 frameName = m_orientationFrameName;
429 return true;
430}
431
432
434{
435 if (sens_index != 0) { return false; }
436
437 std::lock_guard<std::mutex> guard(m_mutex);
438 rs2::frameset dataframe;
439 try
440 {
441 dataframe = m_pipeline.wait_for_frames();
442 }
443 catch (const rs2::error& e)
444 {
445 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
446 m_lastError = e.what();
447 return false;
448 }
449 auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
450 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
451 m_last_pose = pose.get_pose_data();
452 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
453 else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); }
454 else timestamp = 0;
455 yarp::math::Quaternion q(m_last_pose.rotation.x, m_last_pose.rotation.y, m_last_pose.rotation.z, m_last_pose.rotation.w);
458 rpy.resize(3);
459 rpy[0] = 0 + rpy_temp[0] * 180 / M_PI; //here we can eventually adjust the sign and/or sum an offset
460 rpy[1] = 0 + rpy_temp[1] * 180 / M_PI;
461 rpy[2] = 0 + rpy_temp[2] * 180 / M_PI;
462 return true;
463}
464
465//-------------------------------------------------------------------------------------------------------
466
467/* IPositionSensors methods */
469{
470 return 1;
471}
472
478
479bool realsense2Tracking::getPositionSensorName(size_t sens_index, std::string& name) const
480{
481 if (sens_index != 0) { return false; }
483 return true;
484}
485
486bool realsense2Tracking::getPositionSensorFrameName(size_t sens_index, std::string& frameName) const
487{
488 if (sens_index != 0) { return false; }
489 frameName = m_poseFrameName;
490 return true;
491}
492
493
495{
496 if (sens_index != 0) { return false; }
497
498 std::lock_guard<std::mutex> guard(m_mutex);
499 rs2::frameset dataframe;
500 try
501 {
502 dataframe = m_pipeline.wait_for_frames();
503 }
504 catch (const rs2::error& e)
505 {
506 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
507 m_lastError = e.what();
508 return false;
509 }
510 auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
511 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
512 m_last_pose = pose.get_pose_data();
513 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
514 else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); }
515 else timestamp = 0;
516 xyz.resize(3);
517 xyz[0] = m_last_pose.translation.x;
518 xyz[1] = m_last_pose.translation.y;
519 xyz[2] = m_last_pose.translation.z;
520 return true;
521}
522
523/* ILinearVelocitySensors methods */
525{
526 return 1;
527}
528
534
536{
537 if (sens_index != 0) { return false; }
539 return true;
540}
541
542bool realsense2Tracking::getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const
543{
544 if (sens_index != 0) { return false; }
545 frameName = m_poseFrameName;
546 return true;
547}
548
549
551{
552 if (sens_index != 0) { return false; }
553
554 std::lock_guard<std::mutex> guard(m_mutex);
555 rs2::frameset dataframe;
556 try
557 {
558 dataframe = m_pipeline.wait_for_frames();
559 }
560 catch (const rs2::error& e)
561 {
562 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
563 m_lastError = e.what();
564 return false;
565 }
566 auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
567 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
568 m_last_pose = pose.get_pose_data();
569 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
570 else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); }
571 else timestamp = 0;
572 xyz.resize(3);
573 xyz[0] = m_last_pose.velocity.x;
574 xyz[1] = m_last_pose.velocity.y;
575 xyz[2] = m_last_pose.velocity.z;
576
577 return true;
578}
579
580
581//-------------------------------------------------------------------------------------------------------
582#if 0
583/* IPoseSensors methods */
584size_t realsense2Tracking::getNrOfPoseSensors() const
585{
586 return 1;
587}
588
589yarp::dev::MAS_status realsense2Tracking::getPoseSensorStatus(size_t sens_index) const
590{
593}
594
595bool realsense2Tracking::getPoseSensorName(size_t sens_index, std::string& name) const
596{
597 if (sens_index != 0) { return false; }
599 return true;
600}
601
602bool realsense2Tracking::getPoseSensorFrameName(size_t sens_index, std::string& frameName) const
603{
604 if (sens_index != 0) { return false; }
605 frameName = m_poseFrameName;
606 return true;
607}
608
609bool realsense2Tracking::getPoseSensorMeasureAsXYZRPY(size_t sens_index, yarp::sig::Vector& xyzrpy, double& timestamp) const
610{
611 std::lock_guard<std::mutex> guard(m_mutex);
612 rs2::frameset dataframe;
613 try
614 {
615 dataframe = m_pipeline.wait_for_frames();
616 }
617 catch (const rs2::error& e)
618 {
619 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
620 m_lastError = e.what();
621 return false;
622 }
623 auto fa = dataframe.first_or_default(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
624 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
625 m_last_pose = pose.get_pose_data();
626 xyzrpy.resize(6);
627 xyzrpy[0] = m_last_pose.translation.x;
628 xyzrpy[1] = m_last_pose.translation.y;
629 xyzrpy[2] = m_last_pose.translation.z;
630 yarp::math::Quaternion q(m_last_pose.rotation.x, m_last_pose.rotation.y, m_last_pose.rotation.z, m_last_pose.rotation.w);
631 yarp::sig::Matrix mat = q.toRotationMatrix3x3();
633 xyzrpy[3] = 0 + rpy[0] * 180 / M_PI;
634 xyzrpy[4] = 0 + rpy[1] * 180 / M_PI;
635 xyzrpy[5] = 0 + rpy[2] * 180 / M_PI;
636 return true;
637}
638#endif
639//-----------------------------------------------------------------------------
#define M_PI
bool ret
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getLinearVelocitySensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
std::string m_orientationFrameName
timestamp_enumtype m_timestamp_type
bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
size_t getNrOfPositionSensors() const override
Get the number of position sensors exposed by this device.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool getPositionSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
const std::string m_accel_sensor_tag
size_t getNrOfThreeAxisLinearAccelerometers() const override
Get the number of three axis linear accelerometers exposed by this device.
size_t getNrOfOrientationSensors() const override
Get the number of orientation sensors exposed by this device.
bool getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
const std::string m_pose_sensor_tag
bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
std::string m_inertial_sensor_name_prefix
bool getLinearVelocitySensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
const std::string m_gyro_sensor_tag
bool close() override
Close the DeviceDriver.
const std::string m_orientation_sensor_tag
bool getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double &timestamp) const override
Get the last reading of the position sensor as x y z.
bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
size_t getNrOfThreeAxisAngularAccelerometers() const override
Get the number of three axis angular accelerometers exposed by this device.
bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getPositionSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
rs2::pipeline_profile m_profile
bool getOrientationSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
yarp::dev::MAS_status getLinearVelocitySensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
size_t getNrOfLinearVelocitySensors() const override
Get the number of linear velocity sensors exposed by this device.
bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double &timestamp) const override
Get the last reading of the orientation sensor as roll pitch yaw.
bool getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double &timestamp) const override
Get the last reading of the linear velocity sensor as x y z.
yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the gyroscope.
bool getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
yarp::dev::MAS_status getPositionSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
size_t getNrOfThreeAxisGyroscopes() const override
Get the number of three axis gyroscopes exposed by this sensor.
yarp::sig::Matrix toRotationMatrix3x3() const
Converts a quaternion to a rotation matrix.
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
A class for a Matrix.
Definition Matrix.h:39
void resize(size_t size) override
Resize the vector.
Definition Vector.h:221
#define yCError(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
STL namespace.
For streams capable of holding different kinds of content, check what they actually have.
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.
@ MAS_UNKNOWN
The sensor is in an unknown state.
@ MAS_OK
The sensor is working correctly.
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
Definition math.cpp:808
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
An interface to the operating system, including Port based communication.
static bool setOption(rs2_option option, const rs2::sensor *sensor, float value)
static bool getOption(rs2_option option, const rs2::sensor *sensor, float &value)
static std::string get_device_information(const rs2::device &dev)
static void settingErrorMsg(const string &error, bool &ret)
void operator*=(float t)
float3 operator-(float t)
void add(float t1, float t2, float t3)
float3 operator*(float t)