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,12 @@ 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);
+
+    m_profile = m_cfg.resolve(m_pipeline);
+    std::vector< rs2::sensor > sensors = m_profile.get_device().query_sensors();
+    sensors[0].set_option(RS2_OPTION_ENABLE_POSE_JUMPING, 0.f);
+
+
     b &= pipelineStartup();
     if (b==false)
     {
@@ -341,6 +347,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 +525,91 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig::
     return true;
 }
 
+/* IVelocitySensors methods */
+size_t realsense2Tracking::getNrOfVelocitySensors() const
+{
+    return 1;
+}
+
+yarp::dev::MAS_status realsense2Tracking::getVelocitySensorStatus(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::getVelocitySensorName(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::getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const
+{
+    if (sens_index != 0) { return false; }
+    frameName = m_poseFrameName;
+    return true;
+}
+
+
+bool realsense2Tracking::getVelocitySensorMeasure(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(19);
     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.velocity.x;
+    out[8] = m_last_pose.velocity.y;
+    out[9] = m_last_pose.velocity.z;
+    out[10] = m_last_pose.angular_velocity.x;
+    out[11] = m_last_pose.angular_velocity.y;
+    out[12] = m_last_pose.angular_velocity.z;
+    out[13] = m_last_pose.acceleration.x;
+    out[14] = m_last_pose.acceleration.y;
+    out[15] = m_last_pose.acceleration.z;
+    out[16] = m_last_pose.angular_acceleration.x;
+    out[17] = m_last_pose.angular_acceleration.y;
+    out[18] = m_last_pose.angular_acceleration.z;
     return 0;
 }
 
@@ -489,7 +621,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::IVelocitySensors,
         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;
 
+    /* IVelocitySensors methods */
+    size_t getNrOfVelocitySensors() const override;
+    yarp::dev::MAS_status getVelocitySensorStatus(size_t sens_index) const override;
+    bool getVelocitySensorName(size_t sens_index, std::string& name) const override;
+    bool getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override;
+    bool getVelocitySensorMeasure(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;