Stream T256 tracking camera pose using analogServer device (#18)

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
 

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:
+
+```
+<positionX positionY positionZ QuaternionW QuaternionX QuaternionY QuaternionZ>
+```
 
 ## Device documentation
 

src/devices/realsense2/realsense2Tracking.cpp

@@ -460,6 +460,61 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig::
     return true;
 }
 
+int realsense2Tracking::read(yarp::sig::Vector& out)
+{
+    // Publishes the data in the analog port as:
+    // <positionX positionY positionZ QuaternionW QuaternionX QuaternionY QuaternionZ>
+    std::lock_guard<std::mutex> 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<rs2::pose_frame>();
+    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.w;
+    out[4] = m_last_pose.rotation.x;
+    out[5] = m_last_pose.rotation.y;
+    out[6] = m_last_pose.rotation.z;
+    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
 /* IPoseSensors methods */

src/devices/realsense2/realsense2Tracking.h

@@ -13,6 +13,7 @@
 
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
+#include <yarp/dev/IAnalogSensor.h>
 
 #include "realsense2Driver.h"
 #include <cstring>
@@ -31,7 +32,8 @@ 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::IAnalogSensor
 {
 private:
     typedef yarp::os::Stamp Stamp;
@@ -83,6 +85,15 @@ 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;
 
+    /* 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 ;