From fa5f8f3ca20b93b2b3246398deb970996e6e7e83 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 7 Dec 2023 15:11:50 -0600 Subject: [PATCH] Backport: Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (#468) Signed-off-by: Addisu Z. Taddese --- ros_gz_bridge/README.md | 1 + .../bin/ros_gz_bridge_markdown_table | 2 +- .../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 | 20 +++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 3 +++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 12 +++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 +++++++++ 8 files changed, 60 insertions(+), 1 deletion(-) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 11135cdb..e9a062ce 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -29,6 +29,7 @@ The following message types can be bridged for topics: | geometry_msgs/msg/Transform | ignition::msgs::Pose | | geometry_msgs/msg/TransformStamped | ignition::msgs::Pose | | geometry_msgs/msg/Twist | ignition::msgs::Twist | +| geometry_msgs/msg/TwistStamped | ignition::msgs::Twist | | geometry_msgs/msg/TwistWithCovariance| ignition::msgs::TwistWithCovariance | | nav_msgs/msg/Odometry | ignition::msgs::Odometry | | nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | diff --git a/ros_gz_bridge/bin/ros_gz_bridge_markdown_table b/ros_gz_bridge/bin/ros_gz_bridge_markdown_table index 65c33e8b..05c11b72 100755 --- a/ros_gz_bridge/bin/ros_gz_bridge_markdown_table +++ b/ros_gz_bridge/bin/ros_gz_bridge_markdown_table @@ -42,7 +42,7 @@ def main(argv=sys.argv[1:]): for mapping in mappings(msgs_ver): rows.append('| {:32}| {:32}|'.format( - mapping.ros2_package_name + '/' + mapping.ros2_message_name, + mapping.ros2_package_name + '/msg/' + mapping.ros2_message_name, mapping.gz_string())) print('\n'.join(rows)) 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 3256f68e..43860e69 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 @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,18 @@ convert_gz_to_ros( const gz::msgs::Twist & gz_msg, geometry_msgs::msg::Twist & ros_msg); +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::TwistStamped & ros_msg, + gz::msgs::Twist & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::Twist & gz_msg, + geometry_msgs::msg::TwistStamped & 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 33edc5c5..c8788c89 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -40,6 +40,7 @@ Mapping('Transform', 'Pose'), Mapping('TransformStamped', 'Pose'), Mapping('Twist', 'Twist'), + Mapping('TwistStamped', 'Twist'), Mapping('TwistWithCovariance', 'TwistWithCovariance'), Mapping('Wrench', 'Wrench'), Mapping('WrenchStamped', 'Wrench'), diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp index be5f755c..60ae1413 100644 --- a/ros_gz_bridge/src/convert/geometry_msgs.cpp +++ b/ros_gz_bridge/src/convert/geometry_msgs.cpp @@ -256,6 +256,26 @@ convert_gz_to_ros( convert_gz_to_ros(gz_msg.angular(), ros_msg.angular); } +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::TwistStamped & ros_msg, + gz::msgs::Twist & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + convert_ros_to_gz(ros_msg.twist, gz_msg); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::Twist & gz_msg, + geometry_msgs::msg::TwistStamped & ros_msg) +{ + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + convert_gz_to_ros(gz_msg, ros_msg.twist); +} + 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 index 52d076ab..505ed922 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -417,6 +417,9 @@ void createTestMsg(gz::msgs::Twist & _msg) void compareTestMsg(const std::shared_ptr & _msg) { + if (_msg->has_header()) { + compareTestMsg(std::make_shared(_msg->header())); + } compareTestMsg(std::make_shared(_msg->linear())); compareTestMsg(std::make_shared(_msg->angular())); } diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index df757852..6c30e98b 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -447,6 +447,18 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->angular)); } +void createTestMsg(geometry_msgs::msg::TwistStamped & _msg) +{ + createTestMsg(_msg.header); + createTestMsg(_msg.twist); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->twist)); + compareTestMsg(std::make_shared(_msg->header)); +} + void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg) { createTestMsg(_msg.twist.linear); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index e1ec8338..d1e6caf2 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -297,6 +298,14 @@ void createTestMsg(geometry_msgs::msg::Twist & _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::TwistStamped & _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 Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg);