From cc32f8ab20335c19b985220bb43f3327928e0c5d Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Thu, 10 Jun 2021 19:29:59 +0200 Subject: [PATCH] * added IFrameGrabberImageRaw interface to stream one fisheye camera * major refactor: a periodic thread is now used to sample the measurements from librealsense2 and store them in an internal buffer, accessed by the various get methods * added usage example with yarprobotinterface: it instantiates the device plus multiple wrappers for yarp and ros --- .../realsense2Tracking/realsense2Tracking.xml | 11 ++ .../realsense2Tracking/sensors/realsense.xml | 8 + .../wrappers/grabber_yarp.xml | 14 ++ .../realsense2Tracking/wrappers/pose_ros.xml | 15 ++ .../realsense2Tracking/wrappers/pose_yarp.xml | 13 ++ .../realsense2Tracking/yarprobotinterface.ini | 1 + src/devices/realsense2/realsense2Driver.h | 1 + src/devices/realsense2/realsense2Tracking.cpp | 184 +++++++++--------- src/devices/realsense2/realsense2Tracking.h | 26 ++- 9 files changed, 183 insertions(+), 90 deletions(-) create mode 100644 app/devices/realsense2Tracking/realsense2Tracking.xml create mode 100644 app/devices/realsense2Tracking/sensors/realsense.xml create mode 100644 app/devices/realsense2Tracking/wrappers/grabber_yarp.xml create mode 100644 app/devices/realsense2Tracking/wrappers/pose_ros.xml create mode 100644 app/devices/realsense2Tracking/wrappers/pose_yarp.xml create mode 100644 app/devices/realsense2Tracking/yarprobotinterface.ini diff --git a/app/devices/realsense2Tracking/realsense2Tracking.xml b/app/devices/realsense2Tracking/realsense2Tracking.xml new file mode 100644 index 0000000..f597c6d --- /dev/null +++ b/app/devices/realsense2Tracking/realsense2Tracking.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/app/devices/realsense2Tracking/sensors/realsense.xml b/app/devices/realsense2Tracking/sensors/realsense.xml new file mode 100644 index 0000000..ef73ebf --- /dev/null +++ b/app/devices/realsense2Tracking/sensors/realsense.xml @@ -0,0 +1,8 @@ + + + + + + 1 + + diff --git a/app/devices/realsense2Tracking/wrappers/grabber_yarp.xml b/app/devices/realsense2Tracking/wrappers/grabber_yarp.xml new file mode 100644 index 0000000..41e3758 --- /dev/null +++ b/app/devices/realsense2Tracking/wrappers/grabber_yarp.xml @@ -0,0 +1,14 @@ + + + + + 33 + /camera:o + RAW + + + realsense2dev + + + + diff --git a/app/devices/realsense2Tracking/wrappers/pose_ros.xml b/app/devices/realsense2Tracking/wrappers/pose_ros.xml new file mode 100644 index 0000000..1b35c09 --- /dev/null +++ b/app/devices/realsense2Tracking/wrappers/pose_ros.xml @@ -0,0 +1,15 @@ + + + + + 0.01 + /tracking + /ros_tracking + /tracking_node + + + realsense2dev + + + + diff --git a/app/devices/realsense2Tracking/wrappers/pose_yarp.xml b/app/devices/realsense2Tracking/wrappers/pose_yarp.xml new file mode 100644 index 0000000..c369b4c --- /dev/null +++ b/app/devices/realsense2Tracking/wrappers/pose_yarp.xml @@ -0,0 +1,13 @@ + + + + + 10 + /tracking + + + realsense2dev + + + + diff --git a/app/devices/realsense2Tracking/yarprobotinterface.ini b/app/devices/realsense2Tracking/yarprobotinterface.ini new file mode 100644 index 0000000..8018e9d --- /dev/null +++ b/app/devices/realsense2Tracking/yarprobotinterface.ini @@ -0,0 +1 @@ +config ./realsense2Tracking.xml \ No newline at end of file diff --git a/src/devices/realsense2/realsense2Driver.h b/src/devices/realsense2/realsense2Driver.h index b09fef8..2e54d25 100644 --- a/src/devices/realsense2/realsense2Driver.h +++ b/src/devices/realsense2/realsense2Driver.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index a1f149b..e544efa 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -26,7 +26,7 @@ /**********************************************************************************************************/ // This software module is experimental. - // It is provided with uncomplete documentation and it may be modified/renamed/removed without any notice. + // It is provided with incomplete documentation and it may be modified/renamed/removed without any notice. /**********************************************************************************************************/ using namespace yarp::dev; @@ -147,10 +147,66 @@ static void settingErrorMsg(const string& error, bool& ret) } #endif -realsense2Tracking::realsense2Tracking() +realsense2Tracking::realsense2Tracking() : PeriodicThread (0.010) { } +bool realsense2Tracking::threadInit() +{ + bool b = pipelineStartup(); + if (b == false) + { + yCError(REALSENSE2TRACKING) << "Pipeline initialization failed"; + return false; + } + return true; +} + +void realsense2Tracking::threadRelease() +{ + pipelineShutdown(); +} + +void realsense2Tracking::run() +{ + m_yarp_timestamp = yarp::os::Time::now(); + + rs2::frameset dataframe; + try + { + dataframe = m_pipeline.wait_for_frames(); + } + catch (const rs2::error & e) + { + yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:" << "(" << e.what() << ")"; + m_lastError = e.what(); + return; + } + auto fg = dataframe.first_or_default(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F); + rs2::motion_frame gyro = fg.as(); + + auto fa = dataframe.first_or_default(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F); + rs2::motion_frame accel = fa.as(); + + auto fp = dataframe.first_or_default(RS2_STREAM_POSE); + rs2::pose_frame pose = fp.as(); + + auto fr = dataframe.first_or_default(RS2_STREAM_FISHEYE); + + std::lock_guard guard(m_mutex); + m_last_gyro = gyro.get_motion_data(); + m_last_accel = accel.get_motion_data(); + m_last_pose = pose.get_pose_data(); + m_fisheye_data = fr.get_data(); + m_fisheye_size = fr.get_data_size(); + + if (m_timestamp_type == yarp_timestamp) { m_timestamp = m_yarp_timestamp; } + else if (m_timestamp_type == rs_timestamp) { m_timestamp = m_rs_timestamp = gyro.get_timestamp(); } + else m_timestamp = 0; + + return; +} + bool realsense2Tracking::pipelineStartup() { try { @@ -189,7 +245,7 @@ bool realsense2Tracking::pipelineRestart() bool realsense2Tracking::open(Searchable& config) { yCWarning(REALSENSE2TRACKING) << "This software module is experimental."; - yCWarning(REALSENSE2TRACKING) << "It is provided with uncomplete documentation and it may be modified/renamed/removed without any notice."; + yCWarning(REALSENSE2TRACKING) << "It is provided with incomplete documentation and it may be modified/renamed/removed without any notice."; string sensor_is = "t265"; m_timestamp_type = timestamp_enumtype::yarp_timestamp; @@ -205,24 +261,20 @@ bool realsense2Tracking::open(Searchable& config) } } - bool b= true; - + yCInfo(REALSENSE2TRACKING) << "Starting the pipeline"; m_cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F); m_cfg.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F); m_cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF); - b &= pipelineStartup(); - if (b==false) - { - yCError(REALSENSE2TRACKING) << "Pipeline initialization failed"; - return false; - } + m_cfg.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8); + m_cfg.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8); + this->start(); return true; } bool realsense2Tracking::close() { - pipelineShutdown(); + askToStop(); return true; } @@ -260,23 +312,6 @@ bool realsense2Tracking::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::s } std::lock_guard guard(m_mutex); - rs2::frameset dataframe; - try - { - dataframe = m_pipeline.wait_for_frames(); - } - catch (const rs2::error& e) - { - yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")"; - m_lastError = e.what(); - return false; - } - auto fg = dataframe.first_or_default(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F); - rs2::motion_frame gyro = fg.as(); - if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); } - else if (m_timestamp_type == rs_timestamp) { timestamp = gyro.get_timestamp(); } - else timestamp=0; - m_last_gyro = gyro.get_motion_data(); out.resize(3); out[0] = m_last_gyro.x; out[1] = m_last_gyro.y; @@ -317,23 +352,6 @@ bool realsense2Tracking::getThreeAxisLinearAccelerometerMeasure(size_t sens_inde if (sens_index != 0) { return false; } std::lock_guard guard(m_mutex); - rs2::frameset dataframe; - try - { - dataframe = m_pipeline.wait_for_frames(); - } - catch (const rs2::error& e) - { - yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")"; - m_lastError = e.what(); - return false; - } - auto fa = dataframe.first_or_default(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F); - rs2::motion_frame accel = fa.as(); - m_last_accel = accel.get_motion_data(); - if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); } - else if (m_timestamp_type == rs_timestamp) { timestamp = accel.get_timestamp(); } - else timestamp = 0; out.resize(3); out[0] = m_last_accel.x; out[1] = m_last_accel.y; @@ -375,23 +393,6 @@ bool realsense2Tracking::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_i if (sens_index != 0) { return false; } std::lock_guard guard(m_mutex); - rs2::frameset dataframe; - try - { - dataframe = m_pipeline.wait_for_frames(); - } - catch (const rs2::error& e) - { - yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")"; - m_lastError = e.what(); - return false; - } - auto fa = dataframe.first_or_default(RS2_STREAM_POSE); - rs2::pose_frame pose = fa.as(); - m_last_pose = pose.get_pose_data(); - if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); } - else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); } - else timestamp = 0; yarp::math::Quaternion q(m_last_pose.rotation.x, m_last_pose.rotation.y, m_last_pose.rotation.z, m_last_pose.rotation.w); yarp::sig::Matrix mat = q.toRotationMatrix3x3(); yarp::sig::Vector rpy_temp = yarp::math::dcm2rpy(mat); @@ -404,6 +405,34 @@ bool realsense2Tracking::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_i //------------------------------------------------------------------------------------------------------- +bool realsense2Tracking::getImage(yarp::sig::ImageOf& image) +{ + image.resize(848,800); + std::lock_guard guard(m_mutex); + if (m_fisheye_data==nullptr) return false; + unsigned char* pi = (unsigned char*)m_fisheye_data; + unsigned char* po = image.getRawImage(); + for (size_t i=0; i< m_fisheye_width * m_fisheye_height; i++) + { + *po=*pi; + po++; + pi++; + } + return true; +} + +int realsense2Tracking::height() const +{ + return m_fisheye_height; +} + +int realsense2Tracking::width() const +{ + return m_fisheye_width; +} + +//------------------------------------------------------------------------------------------------------- + /* IPositionSensors methods */ size_t realsense2Tracking::getNrOfPositionSensors() const { @@ -436,23 +465,6 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig:: if (sens_index != 0) { return false; } std::lock_guard guard(m_mutex); - rs2::frameset dataframe; - try - { - dataframe = m_pipeline.wait_for_frames(); - } - catch (const rs2::error& e) - { - yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")"; - m_lastError = e.what(); - return false; - } - auto fa = dataframe.first_or_default(RS2_STREAM_POSE); - rs2::pose_frame pose = fa.as(); - m_last_pose = pose.get_pose_data(); - if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); } - else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); } - else timestamp = 0; xyz.resize(3); xyz[0] = m_last_pose.translation.x; xyz[1] = m_last_pose.translation.y; @@ -460,16 +472,15 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig:: return true; } +//------------------------------------------------------------------------------------------------------- +// IAnalogSensor methods. Beware! this is just a temporary test. +// This methods will be removed soon: realsesne2Tracking should NOT derive from IAnalogSensor! + int realsense2Tracking::read(yarp::sig::Vector& out) { // Publishes the data in the analog port as: // std::lock_guard guard(m_mutex); - rs2::frameset dataframe = m_pipeline.wait_for_frames(); - auto fa = dataframe.first_or_default(RS2_STREAM_POSE); - rs2::pose_frame pose = fa.as(); - m_last_pose = pose.get_pose_data(); - out.resize(7); out[0] = m_last_pose.translation.x; out[1] = m_last_pose.translation.y; @@ -512,9 +523,6 @@ int realsense2Tracking::calibrateChannel(int ch, double value) return 0; } - - - //------------------------------------------------------------------------------------------------------- #if 0 /* IPoseSensors methods */ diff --git a/src/devices/realsense2/realsense2Tracking.h b/src/devices/realsense2/realsense2Tracking.h index 101fc87..0f9ce13 100644 --- a/src/devices/realsense2/realsense2Tracking.h +++ b/src/devices/realsense2/realsense2Tracking.h @@ -14,6 +14,7 @@ #include #include #include +#include #include "realsense2Driver.h" #include @@ -24,7 +25,7 @@ /**********************************************************************************************************/ // This software module is experimental. - // It is provided with uncomplete documentation and it may be modified/renamed/removed without any notice. + // It is provided with incomplete documentation and it may be modified/renamed/removed without any notice. /**********************************************************************************************************/ class realsense2Tracking : @@ -33,7 +34,9 @@ class realsense2Tracking : public yarp::dev::IThreeAxisLinearAccelerometers, public yarp::dev::IOrientationSensors, public yarp::dev::IPositionSensors, - public yarp::dev::IAnalogSensor + public yarp::dev::IAnalogSensor, + public yarp::dev::IFrameGrabberImageRaw, + public yarp::os::PeriodicThread { private: typedef yarp::os::Stamp Stamp; @@ -47,6 +50,11 @@ class realsense2Tracking : bool open(yarp::os::Searchable& config) override; bool close() override; + //Periodic Thread + bool threadInit() override; + void run() override; + void threadRelease() override; + private: //method inline bool initializeRealsenseDevice(); @@ -94,6 +102,11 @@ class realsense2Tracking : int calibrateChannel(int ch) override; int calibrateChannel(int ch, double value) override; + /* IFrameGrabberImageRaw */ + bool getImage(yarp::sig::ImageOf& image) override; + int height() const override; + int width() const override; + #if 0 /* IPoseSensors methods */ size_t getNrOfPoseSensors() const ; @@ -108,6 +121,10 @@ class realsense2Tracking : mutable rs2_vector m_last_gyro; mutable rs2_vector m_last_accel; mutable rs2_pose m_last_pose; + mutable const void* m_fisheye_data = nullptr; + mutable int m_fisheye_size; + const size_t m_fisheye_width = 848; + const size_t m_fisheye_height = 800; //strings std::string m_inertial_sensor_name_prefix; @@ -130,6 +147,11 @@ class realsense2Tracking : enum timestamp_enumtype {yarp_timestamp=0, rs_timestamp}; timestamp_enumtype m_timestamp_type; + double m_yarp_timestamp; + double m_rs_timestamp; + double m_timestamp; + + /* rs2::context m_ctx;