From d57ff3279188ff522d305e49e27668a0b01456e1 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 29 Sep 2022 16:33:15 -0700 Subject: [PATCH] convert between pose array and pose_v Signed-off-by: Ian Chen --- ros_gz_bridge/README.md | 1 + .../ros_gz_bridge/convert/geometry_msgs.hpp | 13 +++++++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 1 + ros_gz_bridge/src/convert/geometry_msgs.cpp | 29 +++++++++++++++++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 19 ++++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 ++++++ 6 files changed, 72 insertions(+) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index e76eeaa3..dd2cb0fb 100644 --- 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 index e67ee598..c71a4973 100644 --- 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 #include +#include #include #include #include @@ -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 index ce7afa37..500c127c 100644 --- 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 index 039d5bd2..2e1710db 100644 --- 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 index 7e41a596..cf533e46 100644 --- 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 & _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 & _msg) +{ + compareTestMsg(_msg->header); + + geometry_msgs::msg::PoseArray expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_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 index ad223e2e..4d129c2c 100644 --- 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 #include #include +#include #include #include #include @@ -223,6 +224,14 @@ void createTestMsg(geometry_msgs::msg::Pose & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _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 & _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);