Add Angular Acceleration and Linear Velocity Values

.gitignore

@@ -37,3 +37,5 @@ build*/
 .cproject
 .settings/
 .vscode/
+.cache/
+compile_commands.json

src/devices/realsense2/realsense2Tracking.cpp

@@ -210,6 +210,7 @@ bool realsense2Tracking::open(Searchable& config)
     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)
     {
@@ -341,6 +342,65 @@ bool realsense2Tracking::getThreeAxisLinearAccelerometerMeasure(size_t sens_inde
     return true;
 }
 
+/* IThreeAxisAngularAcclerometer Sensors methods */
+size_t realsense2Tracking::getNrOfThreeAxisAngularAccelerometers() const
+{
+    return 1;
+}
+
+yarp::dev::MAS_status realsense2Tracking::getThreeAxisAngularAccelerometerStatus(size_t sens_index) const
+{
+    if (sens_index != 0) { return yarp::dev::MAS_status::MAS_UNKNOWN; }
+    return yarp::dev::MAS_status::MAS_OK;
+}
+
+bool realsense2Tracking::getThreeAxisAngularAccelerometerName(size_t sens_index, std::string& name) const
+{
+    if (sens_index != 0) { return false; }
+    name = m_inertial_sensor_name_prefix + "/" + m_pose_sensor_tag;
+    return true;
+}
+
+bool realsense2Tracking::getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string& frameName) const
+{
+    if (sens_index != 0) { return false; }
+    frameName = m_poseFrameName;
+    return true;
+}
+
+
+bool realsense2Tracking::getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
+{
+    if (sens_index != 0) { return false; }
+
+    std::lock_guard<std::mutex> 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<rs2::pose_frame>();
+    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;
+    out.resize(3);
+    out[0] = m_last_pose.angular_acceleration.x;
+    out[1] = m_last_pose.angular_acceleration.y;
+    out[2] = m_last_pose.angular_acceleration.z;
+
+    return true;
+}
+
+
+
 //-------------------------------------------------------------------------------------------------------
 
 /* IOrientationSensors methods */
@@ -460,24 +520,85 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig::
     return true;
 }
 
+/* ILinearVelocitySensors methods */
+size_t realsense2Tracking::getNrOfLinearVelocitySensors() const
+{
+    return 1;
+}
+
+yarp::dev::MAS_status realsense2Tracking::getLinearVelocitySensorStatus(size_t sens_index) const
+{
+    if (sens_index != 0) { return yarp::dev::MAS_status::MAS_UNKNOWN; }
+    return yarp::dev::MAS_status::MAS_OK;
+}
+
+bool realsense2Tracking::getLinearVelocitySensorName(size_t sens_index, std::string& name) const
+{
+    if (sens_index != 0) { return false; }
+    name = m_inertial_sensor_name_prefix + "/" + m_pose_sensor_tag;
+    return true;
+}
+
+bool realsense2Tracking::getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const
+{
+    if (sens_index != 0) { return false; }
+    frameName = m_poseFrameName;
+    return true;
+}
+
+
+bool realsense2Tracking::getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const
+{
+    if (sens_index != 0) { return false; }
+
+    std::lock_guard<std::mutex> 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<rs2::pose_frame>();
+    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.velocity.x;
+    xyz[1] = m_last_pose.velocity.y;
+    xyz[2] = m_last_pose.velocity.z;
+
+    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.resize(getChannels());
     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;
+    out[7] = m_last_pose.angular_velocity.x;
+    out[8] = m_last_pose.angular_velocity.y;
+    out[9] = m_last_pose.angular_velocity.z;
+    out[10] = m_last_pose.acceleration.x;
+    out[11] = m_last_pose.acceleration.y;
+    out[12] = m_last_pose.acceleration.z;
     return 0;
 }
 
@@ -489,7 +610,7 @@ int realsense2Tracking::getState(int ch)
 
 int realsense2Tracking::getChannels()
 {
-    return 7;
+    return 19;
 }
 
 int realsense2Tracking::calibrateSensor()

src/devices/realsense2/realsense2Tracking.h

@@ -31,8 +31,10 @@ class realsense2Tracking :
         public yarp::dev::DeviceDriver,
         public yarp::dev::IThreeAxisGyroscopes,
         public yarp::dev::IThreeAxisLinearAccelerometers,
+        public yarp::dev::IThreeAxisAngularAccelerometers,
         public yarp::dev::IOrientationSensors,
         public yarp::dev::IPositionSensors,
+        public yarp::dev::ILinearVelocitySensors,
         public yarp::dev::IAnalogSensor
 {
 private:
@@ -71,6 +73,13 @@ class realsense2Tracking :
     bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string& frameName) const override;
     bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
 
+    /* IThreeAxisAngularAccelerometers methods */
+    size_t getNrOfThreeAxisAngularAccelerometers() const override;
+    yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const override;
+    bool getThreeAxisAngularAccelerometerName(size_t sens_index, std::string& name) const override;
+    bool getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string& frameName) const override;
+    bool getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
+
     /* IOrientationSensors methods */
     size_t getNrOfOrientationSensors() const override;
     yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override;
@@ -85,6 +94,13 @@ 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;
 
+    /* ILinearVelocitySensors methods */
+    size_t getNrOfLinearVelocitySensors() const override;
+    yarp::dev::MAS_status getLinearVelocitySensorStatus(size_t sens_index) const override;
+    bool getLinearVelocitySensorName(size_t sens_index, std::string& name) const override;
+    bool getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override;
+    bool getLinearVelocitySensorMeasure(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;