From 4dd8f52f973fccc63c80f014639addb11eff2a64 Mon Sep 17 00:00:00 2001 From: hosameldin Date: Thu, 28 Jan 2021 11:31:16 +0100 Subject: [PATCH 1/4] add IAnalogSensor interface to stream pose --- src/devices/realsense2/realsense2Tracking.cpp | 85 +++++++++++++++++++ src/devices/realsense2/realsense2Tracking.h | 21 ++++- 2 files changed, 105 insertions(+), 1 deletion(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index 0f479ff..3194eda 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -217,6 +217,7 @@ bool realsense2Tracking::open(Searchable& config) return false; } + m_pose_data.resize(7); return true; } @@ -459,6 +460,90 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig:: xyz[2] = m_last_pose.translation.z; return true; } +/* +bool realsense2Tracking::read(yarp::sig::Vector& out) +{ + 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; + out[2] = m_last_pose.translation.z; + out[3] = m_last_pose.rotation.x; + out[4] = m_last_pose.rotation.y; + out[5] = m_last_pose.rotation.z; + out[6] = m_last_pose.rotation.w; + + return true; +} + +bool realsense2Tracking::getChannels(int* nc) +{ + return true; +} + +bool realsense2Tracking::calibrate(int ch, double v) +{ + return true; +} +*/ + +int realsense2Tracking::read(yarp::sig::Vector& out) +{ + 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; + out[2] = m_last_pose.translation.z; + out[3] = m_last_pose.rotation.x; + out[4] = m_last_pose.rotation.y; + out[5] = m_last_pose.rotation.z; + out[6] = m_last_pose.rotation.w; + return 0; +} + +int realsense2Tracking::getState(int ch) +{ + return 0; +} + + +int realsense2Tracking::getChannels() +{ + return 7; +} + +int realsense2Tracking::calibrateSensor() +{ + return 0; +} + +int realsense2Tracking::calibrateSensor(const yarp::sig::Vector& value) +{ + return 0; +} + +int realsense2Tracking::calibrateChannel(int ch) +{ + return 0; +} + +int realsense2Tracking::calibrateChannel(int ch, double value) +{ + return 0; +} + + + //------------------------------------------------------------------------------------------------------- #if 0 diff --git a/src/devices/realsense2/realsense2Tracking.h b/src/devices/realsense2/realsense2Tracking.h index 74207b5..6179171 100644 --- a/src/devices/realsense2/realsense2Tracking.h +++ b/src/devices/realsense2/realsense2Tracking.h @@ -13,6 +13,8 @@ #include #include +#include +#include #include "realsense2Driver.h" #include @@ -31,7 +33,9 @@ class realsense2Tracking : public yarp::dev::IThreeAxisGyroscopes, public yarp::dev::IThreeAxisLinearAccelerometers, public yarp::dev::IOrientationSensors, - public yarp::dev::IPositionSensors + public yarp::dev::IPositionSensors, + //public yarp::dev::IGenericSensor, + public yarp::dev::IAnalogSensor { private: typedef yarp::os::Stamp Stamp; @@ -83,6 +87,20 @@ class realsense2Tracking : bool getPositionSensorFrameName(size_t sens_index, std::string& frameName) const override; bool getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override; + /* IGenericSensor methods */ + //bool read(yarp::sig::Vector &out) override; + //bool getChannels(int *nc) override; + //bool calibrate(int ch, double v) override; + + /* IAnalogSensor methods */ + int read(yarp::sig::Vector &out) override; + int getState(int ch) override; + int getChannels() override; + int calibrateSensor() override; + int calibrateSensor(const yarp::sig::Vector& value) override; + int calibrateChannel(int ch) override; + int calibrateChannel(int ch, double value) override; + #if 0 /* IPoseSensors methods */ size_t getNrOfPoseSensors() const ; @@ -119,6 +137,7 @@ class realsense2Tracking : enum timestamp_enumtype {yarp_timestamp=0, rs_timestamp}; timestamp_enumtype m_timestamp_type; + yarp::sig::Vector m_pose_data; /* rs2::context m_ctx; From 4014f85c18d92c08056a41739b3244767f1f2062 Mon Sep 17 00:00:00 2001 From: hosameldin Date: Thu, 28 Jan 2021 15:38:01 +0100 Subject: [PATCH 2/4] IAnalogSensor change quaternion order --- src/devices/realsense2/realsense2Tracking.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index 3194eda..1816989 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -504,10 +504,10 @@ int realsense2Tracking::read(yarp::sig::Vector& out) out[0] = m_last_pose.translation.x; out[1] = m_last_pose.translation.y; out[2] = m_last_pose.translation.z; - out[3] = m_last_pose.rotation.x; - out[4] = m_last_pose.rotation.y; - out[5] = m_last_pose.rotation.z; - out[6] = m_last_pose.rotation.w; + out[3] = m_last_pose.rotation.w; + out[4] = m_last_pose.rotation.x; + out[5] = m_last_pose.rotation.y; + out[6] = m_last_pose.rotation.z; return 0; } From c5e5516835f32f8ce16df686fd1915b6cab9eaa4 Mon Sep 17 00:00:00 2001 From: Hosameldin Date: Wed, 26 May 2021 17:02:24 +0200 Subject: [PATCH 3/4] clean up and update README --- README.md | 13 +++++++ src/devices/realsense2/realsense2Tracking.cpp | 34 ++----------------- src/devices/realsense2/realsense2Tracking.h | 8 ----- 3 files changed, 15 insertions(+), 40 deletions(-) diff --git a/README.md b/README.md index 353c36d..0e2c866 100644 --- a/README.md +++ b/README.md @@ -163,6 +163,19 @@ The type on information currently made available are: - the Accelerometer measures, and - the Pose (position and orientation). +You can also publish RealSense T256 pose using the `analogServer` device. Run it using: + +```bash +yarpdev --device analogServer --name /t265 --period 10 --subdevice realsense2Tracking +``` + +:bulb: **NOTE:** also here, the user should specify the parameters `--name` and `--period` as needed. + +The command will open a port `/t256` that streams the pose as follows: + +``` + +``` ## Device documentation diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index 1816989..a1f149b 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -217,7 +217,6 @@ bool realsense2Tracking::open(Searchable& config) return false; } - m_pose_data.resize(7); return true; } @@ -460,40 +459,11 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig:: xyz[2] = m_last_pose.translation.z; return true; } -/* -bool realsense2Tracking::read(yarp::sig::Vector& out) -{ - 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; - out[2] = m_last_pose.translation.z; - out[3] = m_last_pose.rotation.x; - out[4] = m_last_pose.rotation.y; - out[5] = m_last_pose.rotation.z; - out[6] = m_last_pose.rotation.w; - - return true; -} - -bool realsense2Tracking::getChannels(int* nc) -{ - return true; -} - -bool realsense2Tracking::calibrate(int ch, double v) -{ - return true; -} -*/ 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); diff --git a/src/devices/realsense2/realsense2Tracking.h b/src/devices/realsense2/realsense2Tracking.h index 6179171..101fc87 100644 --- a/src/devices/realsense2/realsense2Tracking.h +++ b/src/devices/realsense2/realsense2Tracking.h @@ -13,7 +13,6 @@ #include #include -#include #include #include "realsense2Driver.h" @@ -34,7 +33,6 @@ class realsense2Tracking : public yarp::dev::IThreeAxisLinearAccelerometers, public yarp::dev::IOrientationSensors, public yarp::dev::IPositionSensors, - //public yarp::dev::IGenericSensor, public yarp::dev::IAnalogSensor { private: @@ -87,11 +85,6 @@ class realsense2Tracking : bool getPositionSensorFrameName(size_t sens_index, std::string& frameName) const override; bool getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override; - /* IGenericSensor methods */ - //bool read(yarp::sig::Vector &out) override; - //bool getChannels(int *nc) override; - //bool calibrate(int ch, double v) override; - /* IAnalogSensor methods */ int read(yarp::sig::Vector &out) override; int getState(int ch) override; @@ -137,7 +130,6 @@ class realsense2Tracking : enum timestamp_enumtype {yarp_timestamp=0, rs_timestamp}; timestamp_enumtype m_timestamp_type; - yarp::sig::Vector m_pose_data; /* rs2::context m_ctx; From 0603892a9e6c6b55b90239a9d7c531bde4b2baa3 Mon Sep 17 00:00:00 2001 From: Hosameldin Date: Wed, 26 May 2021 18:04:02 +0200 Subject: [PATCH 4/4] update CHANGELOG --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index f405a87..a1aa73d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). ## [Unreleased] +### Added +- Added `analogServer` device to stream T256 pose. (See [!18](https://github.com/robotology/yarp-device-realsense2/pull/18)). ## [0.1.0] - 2021-04-27