populate imu covariances when converting (gazebosim#375)

Signed-off-by: Alaa El Jawad <[email protected]>
Signed-off-by: wittenator <[email protected]>

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<>

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -20,6 +20,10 @@
 #include <string>
 #include <cstddef>
 
+#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<gz::msgs::IMU> & _msg)
@@ -819,6 +830,13 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::IMU> & _msg)
   compareTestMsg(std::make_shared<gz::msgs::Quaternion>(_msg->orientation()));
   compareTestMsg(std::make_shared<gz::msgs::Vector3d>(_msg->angular_velocity()));
   compareTestMsg(std::make_shared<gz::msgs::Vector3d>(_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)

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -20,6 +20,12 @@
 #include <string>
 #include <cstddef>
 
+#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<sensor_msgs::msg::Imu> & _msg)
@@ -1093,6 +1104,14 @@ void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::Imu> & _msg)
   compareTestMsg(_msg->orientation);
   compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_msg->angular_velocity));
   compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_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)