Bridge between msgs::Pose_V and geometry_msgs/PoseArray msg types (#305)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
gazebosim · Oct 4, 2022 · 1dfcd51 · 1dfcd51
1 parent 84835ad
commit 1dfcd51
Show file tree

Hide file tree

Showing 6 changed files with 72 additions and 0 deletions.
diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md
@@ -22,6 +22,7 @@ The following message types can be bridged for topics:
 | geometry_msgs/msg/Vector3            | ignition::msgs::Vector3d               |
 | geometry_msgs/msg/Point              | ignition::msgs::Vector3d               |
 | geometry_msgs/msg/Pose               | ignition::msgs::Pose                   |
+| geometry_msgs/msg/PoseArray          | ignition::msgs::Pose_V                 |
 | geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance     |
 | geometry_msgs/msg/PoseStamped        | ignition::msgs::Pose                   |
 | geometry_msgs/msg/Transform          | ignition::msgs::Pose                   |

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
@@ -28,6 +28,7 @@
 // ROS 2 messages
 #include <geometry_msgs/msg/point.hpp>
 #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_stamped.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
@@ -90,6 +91,18 @@ convert_gz_to_ros(
   const ignition::msgs::Pose & gz_msg,
   geometry_msgs::msg::Pose & ros_msg);
 
+template<>
+void
+convert_ros_to_gz(
+  const geometry_msgs::msg::PoseArray & ros_msg,
+  ignition::msgs::Pose_V & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const ignition::msgs::Pose_V & gz_msg,
+  geometry_msgs::msg::PoseArray & ros_msg);
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py
@@ -30,6 +30,7 @@
     'geometry_msgs': [
         Mapping('Point', 'Vector3d'),
         Mapping('Pose', 'Pose'),
+        Mapping('PoseArray', 'Pose_V'),
         Mapping('PoseStamped', 'Pose'),
         Mapping('PoseWithCovariance', 'PoseWithCovariance'),
         Mapping('Quaternion', 'Quaternion'),

diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp
@@ -106,6 +106,35 @@ convert_gz_to_ros(
   convert_gz_to_ros(gz_msg.orientation(), ros_msg.orientation);
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const geometry_msgs::msg::PoseArray & ros_msg,
+  ignition::msgs::Pose_V & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+  gz_msg.clear_pose();
+  for (auto const & t : ros_msg.poses) {
+    auto p = gz_msg.add_pose();
+    convert_ros_to_gz(t, *p);
+  }
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const ignition::msgs::Pose_V & gz_msg,
+  geometry_msgs::msg::PoseArray & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  ros_msg.poses.clear();
+  for (auto const & p : gz_msg.pose()) {
+    geometry_msgs::msg::Pose pose;
+    convert_gz_to_ros(p, pose);
+    ros_msg.poses.push_back(pose);
+  }
+}
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp
@@ -280,6 +280,25 @@ void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::Pose> & _msg)
   compareTestMsg(_msg->orientation);
 }
 
+void createTestMsg(geometry_msgs::msg::PoseArray & _msg)
+{
+  createTestMsg(_msg.header);
+
+  geometry_msgs::msg::Pose pose;
+  createTestMsg(pose);
+  _msg.poses.push_back(pose);
+}
+
+void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::PoseArray> & _msg)
+{
+  compareTestMsg(_msg->header);
+
+  geometry_msgs::msg::PoseArray expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<geometry_msgs::msg::Pose>(_msg->poses[0]));
+}
+
 void compareTestMsg(const geometry_msgs::msg::PoseWithCovariance & _msg)
 {
   compareTestMsg(_msg.pose.position);

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp
@@ -31,6 +31,7 @@
 #include <std_msgs/msg/string.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #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_stamped.hpp>
 #include <geometry_msgs/msg/transform.hpp>
@@ -223,6 +224,14 @@ void createTestMsg(geometry_msgs::msg::Pose & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::Pose> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(geometry_msgs::msg::PoseArray & _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::PoseArray> & _msg);
+
 /// \brief Compare a message with the populated for testing.
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const geometry_msgs::msg::PoseWithCovariance & _msg);