From 94f8b4966851ccbb83da1a302782929bfb816296 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Tue, 10 Sep 2024 11:58:45 +0200 Subject: [PATCH 01/11] added .cache and compile_commands to git ignore --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index b648732..19a3e79 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,5 @@ build*/ .cproject .settings/ .vscode/ +.cache/ +compile_commands.json From 629111167d5d3830240c44340c87c11cf657889f Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Tue, 10 Sep 2024 11:59:00 +0200 Subject: [PATCH 02/11] Added velocity functions --- src/devices/realsense2/realsense2Tracking.cpp | 57 +++++++++++++++++++ src/devices/realsense2/realsense2Tracking.h | 8 +++ 2 files changed, 65 insertions(+) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index a1f149b..f1bf398 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -460,6 +460,63 @@ 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 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.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: diff --git a/src/devices/realsense2/realsense2Tracking.h b/src/devices/realsense2/realsense2Tracking.h index 101fc87..075ce8a 100644 --- a/src/devices/realsense2/realsense2Tracking.h +++ b/src/devices/realsense2/realsense2Tracking.h @@ -33,6 +33,7 @@ class realsense2Tracking : public yarp::dev::IThreeAxisLinearAccelerometers, public yarp::dev::IOrientationSensors, public yarp::dev::IPositionSensors, + public yarp::dev::IVelocitySensors, public yarp::dev::IAnalogSensor { private: @@ -85,6 +86,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; From 179b597a946a01a571e3ef3419ff25d09f051a3e Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Mon, 16 Sep 2024 11:40:05 +0200 Subject: [PATCH 03/11] Disabled pose jumping --- src/devices/realsense2/realsense2Tracking.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index f1bf398..d31380e 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/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) { From 002f3c2843539742d8dda6efc6701ac62002d180 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Mon, 16 Sep 2024 11:42:05 +0200 Subject: [PATCH 04/11] Added additional measurments to analogserver --- src/devices/realsense2/realsense2Tracking.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index d31380e..32fe5e4 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -525,8 +525,6 @@ bool realsense2Tracking::getVelocitySensorMeasure(size_t sens_index, yarp::sig:: 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); @@ -541,6 +539,18 @@ int realsense2Tracking::read(yarp::sig::Vector& out) 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; } @@ -552,7 +562,7 @@ int realsense2Tracking::getState(int ch) int realsense2Tracking::getChannels() { - return 7; + return 19; } int realsense2Tracking::calibrateSensor() From 22fb0071fac5835bad646817df4e6f54b555e8a1 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 18 Sep 2024 15:14:24 +0200 Subject: [PATCH 05/11] fixed bug for outputting values over yarp port --- src/devices/realsense2/realsense2Tracking.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index 32fe5e4..a1c4fb3 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -531,7 +531,7 @@ int realsense2Tracking::read(yarp::sig::Vector& out) rs2::pose_frame pose = fa.as(); 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; From 2ff08844db2c7e60ad2bc9f7aec2506169b43581 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 19 Sep 2024 11:15:40 +0200 Subject: [PATCH 06/11] Finished adding Ang Acc interface support --- src/devices/realsense2/realsense2Tracking.cpp | 59 +++++++++++++++++++ src/devices/realsense2/realsense2Tracking.h | 8 +++ 2 files changed, 67 insertions(+) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index a1c4fb3..8db7fb9 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -347,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& xyz, double& timestamp) const +{ + 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.angular_acceleration.x; + xyz[1] = m_last_pose.angular_acceleration.y; + xyz[2] = m_last_pose.angular_acceleration.z; + + return true; +} + + + //------------------------------------------------------------------------------------------------------- /* IOrientationSensors methods */ diff --git a/src/devices/realsense2/realsense2Tracking.h b/src/devices/realsense2/realsense2Tracking.h index 075ce8a..777921a 100644 --- a/src/devices/realsense2/realsense2Tracking.h +++ b/src/devices/realsense2/realsense2Tracking.h @@ -31,6 +31,7 @@ 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, @@ -72,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; From 4d3902c2a4105ceb71fc788ed87e8959dcaa3a92 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 13 Nov 2024 15:52:40 +0100 Subject: [PATCH 07/11] Changed naming convention --- src/devices/realsense2/realsense2Tracking.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index 8db7fb9..cb67c6a 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -374,7 +374,7 @@ bool realsense2Tracking::getThreeAxisAngularAccelerometerFrameName(size_t sens_i } -bool realsense2Tracking::getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const +bool realsense2Tracking::getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const { if (sens_index != 0) { return false; } @@ -396,10 +396,10 @@ bool realsense2Tracking::getThreeAxisAngularAccelerometerMeasure(size_t sens_ind 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.angular_acceleration.x; - xyz[1] = m_last_pose.angular_acceleration.y; - xyz[2] = m_last_pose.angular_acceleration.z; + 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; } From 5c4d5c08f3aa05f366bae2d90f15e8f1d83e1c76 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Sat, 16 Nov 2024 16:10:34 +0100 Subject: [PATCH 08/11] changed velocity sensor interface name --- src/devices/realsense2/realsense2Tracking.cpp | 12 ++++++------ src/devices/realsense2/realsense2Tracking.h | 14 +++++++------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index cb67c6a..1196914 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -525,26 +525,26 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig:: return true; } -/* IVelocitySensors methods */ -size_t realsense2Tracking::getNrOfVelocitySensors() const +/* ILinearVelocitySensors methods */ +size_t realsense2Tracking::getNrOfLinearVelocitySensors() const { return 1; } -yarp::dev::MAS_status realsense2Tracking::getVelocitySensorStatus(size_t sens_index) const +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::getVelocitySensorName(size_t sens_index, std::string& name) const +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::getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const +bool realsense2Tracking::getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const { if (sens_index != 0) { return false; } frameName = m_poseFrameName; @@ -552,7 +552,7 @@ bool realsense2Tracking::getVelocitySensorFrameName(size_t sens_index, std::stri } -bool realsense2Tracking::getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const +bool realsense2Tracking::getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const { if (sens_index != 0) { return false; } diff --git a/src/devices/realsense2/realsense2Tracking.h b/src/devices/realsense2/realsense2Tracking.h index 777921a..1e88006 100644 --- a/src/devices/realsense2/realsense2Tracking.h +++ b/src/devices/realsense2/realsense2Tracking.h @@ -34,7 +34,7 @@ class realsense2Tracking : public yarp::dev::IThreeAxisAngularAccelerometers, public yarp::dev::IOrientationSensors, public yarp::dev::IPositionSensors, - public yarp::dev::IVelocitySensors, + public yarp::dev::ILinearVelocitySensors, public yarp::dev::IAnalogSensor { private: @@ -94,12 +94,12 @@ 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; + /* 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; From 7e557541fe5db34d9fa9f1100bd96f5325ecfab5 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Sat, 16 Nov 2024 16:30:59 +0100 Subject: [PATCH 09/11] Removed hard-coded value --- src/devices/realsense2/realsense2Tracking.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index 1196914..c4ab4d0 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -590,7 +590,7 @@ int realsense2Tracking::read(yarp::sig::Vector& out) rs2::pose_frame pose = fa.as(); m_last_pose = pose.get_pose_data(); - out.resize(19); + out.resize(getChannels()); out[0] = m_last_pose.translation.x; out[1] = m_last_pose.translation.y; out[2] = m_last_pose.translation.z; From 3496b477835ab2b9c1ecd44102d3e3c2a0127f12 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Sat, 16 Nov 2024 16:37:53 +0100 Subject: [PATCH 10/11] Removed option for pose jumping --- src/devices/realsense2/realsense2Tracking.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index c4ab4d0..25b8b59 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -211,11 +211,6 @@ bool realsense2Tracking::open(Searchable& config) 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) { From cce5f2cbb3351a757750505cb5eb8db40039ebea Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 12 Dec 2024 15:44:36 +0100 Subject: [PATCH 11/11] Revered change for deprecated feature --- src/devices/realsense2/realsense2Tracking.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/src/devices/realsense2/realsense2Tracking.cpp b/src/devices/realsense2/realsense2Tracking.cpp index 25b8b59..0ad62d0 100644 --- a/src/devices/realsense2/realsense2Tracking.cpp +++ b/src/devices/realsense2/realsense2Tracking.cpp @@ -593,18 +593,12 @@ int realsense2Tracking::read(yarp::sig::Vector& out) 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; + 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; }