Backport: Add conversion for geometry_msgs/msg/TwistStamped <-> gz.ms…

…gs.Twist (gazebosim#468) Signed-off-by: Addisu Z. Taddese <[email protected]>
azeey · Dec 12, 2023 · e9e0341 · e9e0341
1 parent 514d53c
commit e9e0341
Show file tree

Hide file tree

Showing 8 changed files with 60 additions and 1 deletion.
diff --git 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
@@ -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
@@ -34,6 +34,7 @@
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
@@ -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
@@ -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
@@ -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
@@ -417,6 +417,9 @@ void createTestMsg(gz::msgs::Twist & _msg)
 
 void compareTestMsg(const std::shared_ptr<gz::msgs::Twist> & _msg)
 {
+  if (_msg->has_header()) {
+    compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+  }
   compareTestMsg(std::make_shared<gz::msgs::Vector3d>(_msg->linear()));
   compareTestMsg(std::make_shared<gz::msgs::Vector3d>(_msg->angular()));
 }

diff --git 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<geometry_msgs::msg::Twist> & _msg)
   compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_msg->angular));
 }
 
+void createTestMsg(geometry_msgs::msg::TwistStamped & _msg)
+{
+  createTestMsg(_msg.header);
+  createTestMsg(_msg.twist);
+}
+
+void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::TwistStamped> & _msg)
+{
+  compareTestMsg(std::make_shared<geometry_msgs::msg::Twist>(_msg->twist));
+  compareTestMsg(std::make_shared<std_msgs::msg::Header>(_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
@@ -38,6 +38,7 @@
 #include <geometry_msgs/msg/transform.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
@@ -297,6 +298,14 @@ void createTestMsg(geometry_msgs::msg::Twist & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::Twist> & _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<geometry_msgs::msg::TwistStamped> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg);