From 9377674444ea7616515c2648f607b02b3adacd4f Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Tue, 30 Apr 2024 10:52:21 +0200 Subject: [PATCH] populate imu covariances when converting (#375) (#540) Signed-off-by: Alaa El Jawad Signed-off-by: wittenator <9154515+wittenator@users.noreply.github.com> Co-authored-by: El Jawad Alaa --- ros_gz_bridge/src/convert/sensor_msgs.cpp | 42 ++++++++++++++++++++++- ros_gz_bridge/test/utils/gz_test_msg.cpp | 18 ++++++++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 19 ++++++++++ 3 files changed, 78 insertions(+), 1 deletion(-) diff --git a/ros_gz_bridge/src/convert/sensor_msgs.cpp b/ros_gz_bridge/src/convert/sensor_msgs.cpp index b885b738..691b823c 100644 --- a/ros_gz_bridge/src/convert/sensor_msgs.cpp +++ b/ros_gz_bridge/src/convert/sensor_msgs.cpp @@ -17,6 +17,12 @@ #include "convert/utils.hpp" #include "ros_gz_bridge/convert/sensor_msgs.hpp" +#include "gz/msgs/config.hh" + +#if GZ_MSGS_MAJOR_VERSION >= 10 +#define GZ_MSGS_IMU_HAS_COVARIANCE +#endif + namespace ros_gz_bridge { @@ -274,6 +280,18 @@ convert_ros_to_gz( convert_ros_to_gz(ros_msg.orientation, (*gz_msg.mutable_orientation())); convert_ros_to_gz(ros_msg.angular_velocity, (*gz_msg.mutable_angular_velocity())); convert_ros_to_gz(ros_msg.linear_acceleration, (*gz_msg.mutable_linear_acceleration())); + +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + for (const auto & elem : ros_msg.linear_acceleration_covariance) { + gz_msg.mutable_linear_acceleration_covariance()->add_data(elem); + } + for (const auto & elem : ros_msg.orientation_covariance) { + gz_msg.mutable_orientation_covariance()->add_data(elem); + } + for (const auto & elem : ros_msg.angular_velocity_covariance) { + gz_msg.mutable_angular_velocity_covariance()->add_data(elem); + } +#endif } template<> @@ -287,7 +305,29 @@ convert_gz_to_ros( convert_gz_to_ros(gz_msg.angular_velocity(), ros_msg.angular_velocity); convert_gz_to_ros(gz_msg.linear_acceleration(), ros_msg.linear_acceleration); - // Covariances not supported in gz::msgs::IMU +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + int data_size = gz_msg.linear_acceleration_covariance().data_size(); + if (data_size == 9) { + for (int i = 0; i < data_size; ++i) { + auto data = gz_msg.linear_acceleration_covariance().data()[i]; + ros_msg.linear_acceleration_covariance[i] = data; + } + } + data_size = gz_msg.angular_velocity_covariance().data_size(); + if (data_size == 9) { + for (int i = 0; i < data_size; ++i) { + auto data = gz_msg.angular_velocity_covariance().data()[i]; + ros_msg.angular_velocity_covariance[i] = data; + } + } + data_size = gz_msg.orientation_covariance().data_size(); + if (data_size == 9) { + for (int i = 0; i < data_size; ++i) { + auto data = gz_msg.orientation_covariance().data()[i]; + ros_msg.orientation_covariance[i] = data; + } + } +#endif } template<> diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 4ee4aba7..7142d7c4 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -20,6 +20,10 @@ #include #include +#if GZ_MSGS_MAJOR_VERSION >= 10 +#define GZ_MSGS_IMU_HAS_COVARIANCE +#endif + namespace ros_gz_bridge { namespace testing @@ -811,6 +815,13 @@ void createTestMsg(gz::msgs::IMU & _msg) _msg.mutable_orientation()->CopyFrom(quaternion_msg); _msg.mutable_angular_velocity()->CopyFrom(vector3_msg); _msg.mutable_linear_acceleration()->CopyFrom(vector3_msg); +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + for (int i = 0; i < 9; i++) { + _msg.mutable_orientation_covariance()->add_data(i + 1); + _msg.mutable_angular_velocity_covariance()->add_data(i + 1); + _msg.mutable_linear_acceleration_covariance()->add_data(i + 1); + } +#endif } void compareTestMsg(const std::shared_ptr & _msg) @@ -819,6 +830,13 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->orientation())); compareTestMsg(std::make_shared(_msg->angular_velocity())); compareTestMsg(std::make_shared(_msg->linear_acceleration())); +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + for (int i = 0; i < 9; i++) { + EXPECT_EQ(_msg->orientation_covariance().data(i), i + 1); + EXPECT_EQ(_msg->angular_velocity_covariance().data(i), i + 1); + EXPECT_EQ(_msg->linear_acceleration_covariance().data(i), i + 1); + } +#endif } void createTestMsg(gz::msgs::Axis & _msg) diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 634c8ebd..9d9e3c25 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -20,6 +20,12 @@ #include #include +#include "gz/msgs/config.hh" + +#if GZ_MSGS_MAJOR_VERSION >= 10 +#define GZ_MSGS_IMU_HAS_COVARIANCE +#endif + namespace ros_gz_bridge { namespace testing @@ -1085,6 +1091,11 @@ void createTestMsg(sensor_msgs::msg::Imu & _msg) _msg.orientation = quaternion_msg; _msg.angular_velocity = vector3_msg; _msg.linear_acceleration = vector3_msg; +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + _msg.orientation_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + _msg.angular_velocity_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + _msg.linear_acceleration_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9}; +#endif } void compareTestMsg(const std::shared_ptr & _msg) @@ -1093,6 +1104,14 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(_msg->orientation); compareTestMsg(std::make_shared(_msg->angular_velocity)); compareTestMsg(std::make_shared(_msg->linear_acceleration)); + +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + for (int i = 0; i < 9; ++i) { + EXPECT_EQ(_msg->orientation_covariance[i], i + 1); + EXPECT_EQ(_msg->angular_velocity_covariance[i], i + 1); + EXPECT_EQ(_msg->linear_acceleration_covariance[i], i + 1); + } +#endif } void createTestMsg(sensor_msgs::msg::JointState & _msg)