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
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 b &= pipelineStartup();
214 if (b==false)
215 {
216 yCError(REALSENSE2TRACKING) << "Pipeline initialization failed";
217 return false;
218 }
219
220 return true;
221}
222
224{
225 pipelineShutdown();
226 return true;
227}
228
229//---------------------------------------------------------------------------------------------------------------
230/* IThreeAxisGyroscopes methods */
232{
233 return 1;
234}
235
241
243{
244 if (sens_index != 0) { return false; }
246 return true;
247}
248
249bool realsense2Tracking::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string& frameName) const
250{
251 if (sens_index != 0) { return false; }
252 frameName = m_gyroFrameName;
253 return true;
254}
255
257{
258 if (sens_index != 0) {
259 return false;
260 }
261
262 std::lock_guard<std::mutex> guard(m_mutex);
263 rs2::frameset dataframe;
264 try
265 {
266 dataframe = m_pipeline.wait_for_frames();
267 }
268 catch (const rs2::error& e)
269 {
270 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
271 m_lastError = e.what();
272 return false;
273 }
274 auto fg = dataframe.first_or_default(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
275 rs2::motion_frame gyro = fg.as<rs2::motion_frame>();
276 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
277 else if (m_timestamp_type == rs_timestamp) { timestamp = gyro.get_timestamp(); }
278 else timestamp=0;
279 m_last_gyro = gyro.get_motion_data();
280 out.resize(3);
281 out[0] = m_last_gyro.x;
282 out[1] = m_last_gyro.y;
283 out[2] = m_last_gyro.z;
284 return true;
285}
286
287//-------------------------------------------------------------------------------------------------------
288
289/* IThreeAxisLinearAccelerometers methods */
291{
292 return 1;
293}
294
300
302{
303 if (sens_index != 0) { return false; }
305 return true;
306}
307
309{
310 if (sens_index != 0) { return false; }
311 frameName = m_accelFrameName;
312 return true;
313}
314
316{
317 if (sens_index != 0) { return false; }
318
319 std::lock_guard<std::mutex> guard(m_mutex);
320 rs2::frameset dataframe;
321 try
322 {
323 dataframe = m_pipeline.wait_for_frames();
324 }
325 catch (const rs2::error& e)
326 {
327 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
328 m_lastError = e.what();
329 return false;
330 }
331 auto fa = dataframe.first_or_default(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
332 rs2::motion_frame accel = fa.as<rs2::motion_frame>();
333 m_last_accel = accel.get_motion_data();
334 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
335 else if (m_timestamp_type == rs_timestamp) { timestamp = accel.get_timestamp(); }
336 else timestamp = 0;
337 out.resize(3);
338 out[0] = m_last_accel.x;
339 out[1] = m_last_accel.y;
340 out[2] = m_last_accel.z;
341 return true;
342}
343
344//-------------------------------------------------------------------------------------------------------
345
346/* IOrientationSensors methods */
348{
349 return 1;
350}
351
357
358bool realsense2Tracking::getOrientationSensorName(size_t sens_index, std::string& name) const
359{
360 if (sens_index != 0) { return false; }
362 return true;
363}
364
365bool realsense2Tracking::getOrientationSensorFrameName(size_t sens_index, std::string& frameName) const
366{
367 if (sens_index != 0) { return false; }
368 frameName = m_orientationFrameName;
369 return true;
370}
371
372
374{
375 if (sens_index != 0) { return false; }
376
377 std::lock_guard<std::mutex> guard(m_mutex);
378 rs2::frameset dataframe;
379 try
380 {
381 dataframe = m_pipeline.wait_for_frames();
382 }
383 catch (const rs2::error& e)
384 {
385 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
386 m_lastError = e.what();
387 return false;
388 }
389 auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
390 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
391 m_last_pose = pose.get_pose_data();
392 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
393 else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); }
394 else timestamp = 0;
395 yarp::math::Quaternion q(m_last_pose.rotation.x, m_last_pose.rotation.y, m_last_pose.rotation.z, m_last_pose.rotation.w);
396 yarp::sig::Matrix mat = q.toRotationMatrix3x3();
398 rpy.resize(3);
399 rpy[0] = 0 + rpy_temp[0] * 180 / M_PI; //here we can eventually adjust the sign and/or sum an offset
400 rpy[1] = 0 + rpy_temp[1] * 180 / M_PI;
401 rpy[2] = 0 + rpy_temp[2] * 180 / M_PI;
402 return true;
403}
404
405//-------------------------------------------------------------------------------------------------------
406
407/* IPositionSensors methods */
409{
410 return 1;
411}
412
418
419bool realsense2Tracking::getPositionSensorName(size_t sens_index, std::string& name) const
420{
421 if (sens_index != 0) { return false; }
423 return true;
424}
425
426bool realsense2Tracking::getPositionSensorFrameName(size_t sens_index, std::string& frameName) const
427{
428 if (sens_index != 0) { return false; }
429 frameName = m_poseFrameName;
430 return true;
431}
432
433
435{
436 if (sens_index != 0) { return false; }
437
438 std::lock_guard<std::mutex> guard(m_mutex);
439 rs2::frameset dataframe;
440 try
441 {
442 dataframe = m_pipeline.wait_for_frames();
443 }
444 catch (const rs2::error& e)
445 {
446 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
447 m_lastError = e.what();
448 return false;
449 }
450 auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
451 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
452 m_last_pose = pose.get_pose_data();
453 if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
454 else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); }
455 else timestamp = 0;
456 xyz.resize(3);
457 xyz[0] = m_last_pose.translation.x;
458 xyz[1] = m_last_pose.translation.y;
459 xyz[2] = m_last_pose.translation.z;
460 return true;
461}
462
464{
465 // Publishes the data in the analog port as:
466 // <positionX positionY positionZ QuaternionW QuaternionX QuaternionY QuaternionZ>
467 std::lock_guard<std::mutex> guard(m_mutex);
468 rs2::frameset dataframe = m_pipeline.wait_for_frames();
469 auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
470 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
471 m_last_pose = pose.get_pose_data();
472
473 out.resize(7);
474 out[0] = m_last_pose.translation.x;
475 out[1] = m_last_pose.translation.y;
476 out[2] = m_last_pose.translation.z;
477 out[3] = m_last_pose.rotation.w;
478 out[4] = m_last_pose.rotation.x;
479 out[5] = m_last_pose.rotation.y;
480 out[6] = m_last_pose.rotation.z;
481 return 0;
482}
483
485{
486 return 0;
487}
488
489
491{
492 return 7;
493}
494
496{
497 return 0;
498}
499
501{
502 return 0;
503}
504
506{
507 return 0;
508}
509
511{
512 return 0;
513}
514
515
516
517
518//-------------------------------------------------------------------------------------------------------
519#if 0
520/* IPoseSensors methods */
521size_t realsense2Tracking::getNrOfPoseSensors() const
522{
523 return 1;
524}
525
526yarp::dev::MAS_status realsense2Tracking::getPoseSensorStatus(size_t sens_index) const
527{
530}
531
532bool realsense2Tracking::getPoseSensorName(size_t sens_index, std::string& name) const
533{
534 if (sens_index != 0) { return false; }
536 return true;
537}
538
539bool realsense2Tracking::getPoseSensorFrameName(size_t sens_index, std::string& frameName) const
540{
541 if (sens_index != 0) { return false; }
542 frameName = m_poseFrameName;
543 return true;
544}
545
546bool realsense2Tracking::getPoseSensorMeasureAsXYZRPY(size_t sens_index, yarp::sig::Vector& xyzrpy, double& timestamp) const
547{
548 std::lock_guard<std::mutex> guard(m_mutex);
549 rs2::frameset dataframe;
550 try
551 {
552 dataframe = m_pipeline.wait_for_frames();
553 }
554 catch (const rs2::error& e)
555 {
556 yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
557 m_lastError = e.what();
558 return false;
559 }
560 auto fa = dataframe.first_or_default(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
561 rs2::pose_frame pose = fa.as<rs2::pose_frame>();
562 m_last_pose = pose.get_pose_data();
563 xyzrpy.resize(6);
564 xyzrpy[0] = m_last_pose.translation.x;
565 xyzrpy[1] = m_last_pose.translation.y;
566 xyzrpy[2] = m_last_pose.translation.z;
567 yarp::math::Quaternion q(m_last_pose.rotation.x, m_last_pose.rotation.y, m_last_pose.rotation.z, m_last_pose.rotation.w);
568 yarp::sig::Matrix mat = q.toRotationMatrix3x3();
570 xyzrpy[3] = 0 + rpy[0] * 180 / M_PI;
571 xyzrpy[4] = 0 + rpy[1] * 180 / M_PI;
572 xyzrpy[5] = 0 + rpy[2] * 180 / M_PI;
573 return true;
574}
575#endif
576//-----------------------------------------------------------------------------
#define M_PI
float t
bool ret
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Get the status of the specified sensor.
int read(yarp::sig::Vector &out) override
Read a vector from the 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.
int getChannels() override
Get the number of channels of the sensor.
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.
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
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.
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 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.
int calibrateSensor() override
Calibrates the whole sensor.
int calibrateChannel(int ch) override
Calibrates one single channel.
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.
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.
int getState(int ch) override
Check the state value of a given channel.
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.
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)