allow converting from/to PoseWithCovarianceStamped

Signed-off-by: Alaa El Jawad <ejalaa12@gmail.com>
gazebosim · Apr 11, 2023 · ae56ea9 · ae56ea9
1 parent 7c62a1c
commit ae56ea9
Show file tree

Hide file tree

Showing 6 changed files with 57 additions and 0 deletions.
diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp
@@ -30,6 +30,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
@@ -109,6 +110,18 @@ convert_ros_to_gz(
   const geometry_msgs::msg::PoseWithCovariance & ros_msg,
   gz::msgs::PoseWithCovariance & gz_msg);
 
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::PoseWithCovariance & gz_msg,
+  geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg);
+
+template<>
+void
+convert_ros_to_gz(
+  const geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg,
+  gz::msgs::PoseWithCovariance & gz_msg);
+
 template<>
 void
 convert_gz_to_ros(

diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py
@@ -33,6 +33,7 @@
         Mapping('PoseArray', 'Pose_V'),
         Mapping('PoseStamped', 'Pose'),
         Mapping('PoseWithCovariance', 'PoseWithCovariance'),
+        Mapping('PoseWithCovarianceStamped', 'PoseWithCovariance'),
         Mapping('Quaternion', 'Quaternion'),
         Mapping('Transform', 'Pose'),
         Mapping('TransformStamped', 'Pose'),

diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp
@@ -165,6 +165,27 @@ convert_gz_to_ros(
   }
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg,
+  gz::msgs::PoseWithCovariance & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_pose()->mutable_header()));
+  convert_ros_to_gz(ros_msg.pose, gz_msg);
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::PoseWithCovariance & gz_msg,
+  geometry_msgs::msg::PoseWithCovariance & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.pose().header(), ros_msg.header);
+  convert_gz_to_ros(gz_msg, ros_msg.pose);
+}
+
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp
@@ -323,6 +323,7 @@ void createTestMsg(gz::msgs::PoseWithCovariance & _msg)
 {
   createTestMsg(*_msg.mutable_pose()->mutable_position());
   createTestMsg(*_msg.mutable_pose()->mutable_orientation());
+  createTestMsg(*_msg.mutable_pose()->mutable_header());
   for (int i = 0; i < 36; i++) {
     _msg.mutable_covariance()->add_data(i);
   }

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp
@@ -343,6 +343,18 @@ void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::PoseWithCovariance
   }
 }
 
+void createTestMsg(geometry_msgs::msg::PoseWithCovarianceStamped & _msg)
+{
+  createTestMsg(_msg.header);
+  createTestMsg(_msg.pose);
+}
+
+void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped> & _msg)
+{
+  compareTestMsg(std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(_msg->pose);
+  compareTestMsg(std::make_shared<std_msgs::msg::Header>(_msg->header));
+}
+
 void createTestMsg(geometry_msgs::msg::PoseStamped & _msg)
 {
   createTestMsg(_msg.header);

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp
@@ -33,6 +33,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/transform.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
@@ -241,6 +242,14 @@ void compareTestMsg(const geometry_msgs::msg::PoseWithCovariance & _msg);
 /// \param[out] _msg The message populated.
 void createTestMsg(geometry_msgs::msg::PoseWithCovariance & _msg);
 
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & _msg);
+
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(geometry_msgs::msg::PoseWithCovarianceStamped & _msg);
+
 /// \brief Compare a message with the populated for testing.
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::PoseWithCovariance> & _msg);