Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (#…

…468)

Signed-off-by: Addisu Z. Taddese <[email protected]>

ros_gz_bridge/README.md

@@ -5,63 +5,71 @@ between ROS and Gazebo Transport.
 
 The following message types can be bridged for topics:
 
-| ROS type                                     | Gazebo Transport Type          |
-|----------------------------------------------|:------------------------------:|
-| builtin_interfaces/msg/Time                  | gz.msgs.Time                   |
-| geometry_msgs/msg/Point                      | gz.msgs.Vector3d               |
-| geometry_msgs/msg/Pose                       | gz.msgs.Pose                   |
-| geometry_msgs/msg/PoseArray                  | gz.msgs.Pose_V                 |
-| geometry_msgs/msg/PoseStamped                | gz.msgs.Pose                   |
-| geometry_msgs/msg/PoseWithCovariance         | gz.msgs.PoseWithCovariance     |
-| geometry_msgs/msg/Quaternion                 | gz.msgs.Quaternion             |
-| geometry_msgs/msg/Transform                  | gz.msgs.Pose                   |
-| geometry_msgs/msg/TransformStamped           | gz.msgs.Pose                   |
-| geometry_msgs/msg/Twist                      | gz.msgs.Twist                  |
-| geometry_msgs/msg/TwistWithCovariance        | gz.msgs.TwistWithCovariance    |
-| geometry_msgs/msg/TwistWithCovarianceStamped | gz.msgs.TwistWithCovariance    |
-| geometry_msgs/msg/Vector3                    | gz.msgs.Vector3d               |
-| geometry_msgs/msg/Wrench                     | gz.msgs.Wrench                 |
-| geometry_msgs/msg/WrenchStamped              | gz.msgs.Wrench                 |
-| nav_msgs/msg/Odometry                        | gz.msgs.Odometry               |
-| nav_msgs/msg/Odometry                        | gz.msgs.OdometryWithCovariance |
-| rcl_interfaces/msg/ParameterValue            | gz.msgs.Any                    |
-| ros_gz_interfaces/msg/Altimeter              | gz.msgs.Altimeter              |
-| ros_gz_interfaces/msg/Contact                | gz.msgs.Contact                |
-| ros_gz_interfaces/msg/Contacts               | gz.msgs.Contacts               |
-| ros_gz_interfaces/msg/Dataframe              | gz.msgs.Dataframe              |
-| ros_gz_interfaces/msg/Entity                 | gz.msgs.Entity                 |
-| ros_gz_interfaces/msg/Float32Array           | gz.msgs.Float_V                |
-| ros_gz_interfaces/msg/GuiCamera              | gz.msgs.GUICamera              |
-| ros_gz_interfaces/msg/JointWrench            | gz.msgs.JointWrench            |
-| ros_gz_interfaces/msg/Light                  | gz.msgs.Light                  |
-| ros_gz_interfaces/msg/SensorNoise            | gz.msgs.SensorNoise            |
-| ros_gz_interfaces/msg/StringVec              | gz.msgs.StringMsg_V            |
-| ros_gz_interfaces/msg/TrackVisual            | gz.msgs.TrackVisual            |
-| ros_gz_interfaces/msg/VideoRecord            | gz.msgs.VideoRecord            |
-| ros_gz_interfaces/msg/WorldControl           | gz.msgs.WorldControl           |
-| rosgraph_msgs/msg/Clock*                     | gz.msgs.Clock*                 |
-| sensor_msgs/msg/BatteryState                 | gz.msgs.BatteryState           |
-| sensor_msgs/msg/CameraInfo                   | gz.msgs.CameraInfo             |
-| sensor_msgs/msg/FluidPressure                | gz.msgs.FluidPressure          |
-| sensor_msgs/msg/Image                        | gz.msgs.Image                  |
-| sensor_msgs/msg/Imu                          | gz.msgs.IMU                    |
-| sensor_msgs/msg/JointState                   | gz.msgs.Model                  |
-| sensor_msgs/msg/Joy                          | gz.msgs.Joy                    |
-| sensor_msgs/msg/LaserScan                    | gz.msgs.LaserScan              |
-| sensor_msgs/msg/MagneticField                | gz.msgs.Magnetometer           |
-| sensor_msgs/msg/NavSatFix                    | gz.msgs.NavSat                 |
-| sensor_msgs/msg/PointCloud2                  | gz.msgs.PointCloudPacked       |
-| std_msgs/msg/Bool                            | gz.msgs.Boolean                |
-| std_msgs/msg/ColorRGBA                       | gz.msgs.Color                  |
-| std_msgs/msg/Empty                           | gz.msgs.Empty                  |
-| std_msgs/msg/Float32                         | gz.msgs.Float                  |
-| std_msgs/msg/Float64                         | gz.msgs.Double                 |
-| std_msgs/msg/Header                          | gz.msgs.Header                 |
-| std_msgs/msg/Int32                           | gz.msgs.Int32                  |
-| std_msgs/msg/String                          | gz.msgs.StringMsg              |
-| std_msgs/msg/UInt32                          | gz.msgs.UInt32                 |
-| tf2_msgs/msg/TFMessage                       | gz.msgs.Pose_V                 |
-| trajectory_msgs/msg/JointTrajectory          | gz.msgs.JointTrajectory        |
+| ROS type                                       | Gazebo Transport Type               |
+| ---------------------------------------------- | :------------------------------:    |
+| actuator_msgs/msg/Actuators                    | gz.msgs.Actuators                   |
+| builtin_interfaces/msg/Time                    | gz.msgs.Time                        |
+| geometry_msgs/msg/Point                        | gz.msgs.Vector3d                    |
+| geometry_msgs/msg/Pose                         | gz.msgs.Pose                        |
+| geometry_msgs/msg/PoseArray                    | gz.msgs.Pose_V                      |
+| geometry_msgs/msg/PoseStamped                  | gz.msgs.Pose                        |
+| geometry_msgs/msg/PoseWithCovariance           | gz.msgs.PoseWithCovariance          |
+| geometry_msgs/msg/PoseWithCovarianceStamped    | gz.msgs.PoseWithCovariance          |
+| geometry_msgs/msg/Quaternion                   | gz.msgs.Quaternion                  |
+| geometry_msgs/msg/Transform                    | gz.msgs.Pose                        |
+| geometry_msgs/msg/TransformStamped             | gz.msgs.Pose                        |
+| geometry_msgs/msg/Twist                        | gz.msgs.Twist                       |
+| geometry_msgs/msg/TwistStamped                 | gz.msgs.Twist                       |
+| geometry_msgs/msg/TwistWithCovariance          | gz.msgs.TwistWithCovariance         |
+| geometry_msgs/msg/TwistWithCovarianceStamped   | gz.msgs.TwistWithCovariance         |
+| geometry_msgs/msg/Vector3                      | gz.msgs.Vector3d                    |
+| geometry_msgs/msg/Wrench                       | gz.msgs.Wrench                      |
+| geometry_msgs/msg/WrenchStamped                | gz.msgs.Wrench                      |
+| gps_msgs/msg/GPSFix                            | gz.msgs.NavSat                      |
+| nav_msgs/msg/Odometry                          | gz.msgs.Odometry                    |
+| nav_msgs/msg/Odometry                          | gz.msgs.OdometryWithCovariance      |
+| rcl_interfaces/msg/ParameterValue              | gz.msgs.Any                         |
+| ros_gz_interfaces/msg/Altimeter                | gz.msgs.Altimeter                   |
+| ros_gz_interfaces/msg/Contact                  | gz.msgs.Contact                     |
+| ros_gz_interfaces/msg/Contacts                 | gz.msgs.Contacts                    |
+| ros_gz_interfaces/msg/Dataframe                | gz.msgs.Dataframe                   |
+| ros_gz_interfaces/msg/Entity                   | gz.msgs.Entity                      |
+| ros_gz_interfaces/msg/Float32Array             | gz.msgs.Float_V                     |
+| ros_gz_interfaces/msg/GuiCamera                | gz.msgs.GUICamera                   |
+| ros_gz_interfaces/msg/JointWrench              | gz.msgs.JointWrench                 |
+| ros_gz_interfaces/msg/Light                    | gz.msgs.Light                       |
+| ros_gz_interfaces/msg/ParamVec                 | gz.msgs.Param                       |
+| ros_gz_interfaces/msg/ParamVec                 | gz.msgs.Param_V                     |
+| ros_gz_interfaces/msg/SensorNoise              | gz.msgs.SensorNoise                 |
+| ros_gz_interfaces/msg/StringVec                | gz.msgs.StringMsg_V                 |
+| ros_gz_interfaces/msg/TrackVisual              | gz.msgs.TrackVisual                 |
+| ros_gz_interfaces/msg/VideoRecord              | gz.msgs.VideoRecord                 |
+| rosgraph_msgs/msg/Clock                        | gz.msgs.Clock                       |
+| sensor_msgs/msg/BatteryState                   | gz.msgs.BatteryState                |
+| sensor_msgs/msg/CameraInfo                     | gz.msgs.CameraInfo                  |
+| sensor_msgs/msg/FluidPressure                  | gz.msgs.FluidPressure               |
+| sensor_msgs/msg/Image                          | gz.msgs.Image                       |
+| sensor_msgs/msg/Imu                            | gz.msgs.IMU                         |
+| sensor_msgs/msg/JointState                     | gz.msgs.Model                       |
+| sensor_msgs/msg/Joy                            | gz.msgs.Joy                         |
+| sensor_msgs/msg/LaserScan                      | gz.msgs.LaserScan                   |
+| sensor_msgs/msg/MagneticField                  | gz.msgs.Magnetometer                |
+| sensor_msgs/msg/NavSatFix                      | gz.msgs.NavSat                      |
+| sensor_msgs/msg/PointCloud2                    | gz.msgs.PointCloudPacked            |
+| std_msgs/msg/Bool                              | gz.msgs.Boolean                     |
+| std_msgs/msg/ColorRGBA                         | gz.msgs.Color                       |
+| std_msgs/msg/Empty                             | gz.msgs.Empty                       |
+| std_msgs/msg/Float32                           | gz.msgs.Float                       |
+| std_msgs/msg/Float64                           | gz.msgs.Double                      |
+| std_msgs/msg/Header                            | gz.msgs.Header                      |
+| std_msgs/msg/Int32                             | gz.msgs.Int32                       |
+| std_msgs/msg/String                            | gz.msgs.StringMsg                   |
+| std_msgs/msg/UInt32                            | gz.msgs.UInt32                      |
+| tf2_msgs/msg/TFMessage                         | gz.msgs.Pose_V                      |
+| trajectory_msgs/msg/JointTrajectory            | gz.msgs.JointTrajectory             |
+| vision_msgs/msg/Detection2D                    | gz.msgs.AnnotatedAxisAligned2DBox   |
+| vision_msgs/msg/Detection2DArray               | gz.msgs.AnnotatedAxisAligned2DBox_V |
+
 
 And the following for services:
 
@@ -77,7 +85,7 @@ Gazebo would be the only `/clock` publisher, the sole source of clock informatio
 You should create an unidirectional `/clock` bridge:
 
 ```bash
-ros2 run ros_gz_bridge parameter_bridge /clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock
+ros2 run ros_gz_bridge parameter_bridge /clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock
 ```
 
 ## Example 1a: Gazebo Transport talker and ROS 2 listener
@@ -87,7 +95,7 @@ Start the parameter bridge which will watch the specified topics.
 ```
 # Shell A:
 . ~/bridge_ws/install/setup.bash
-ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg
+ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@gz.msgs.StringMsg
 ```
 
 Now we start the ROS listener.
@@ -102,7 +110,7 @@ Now we start the Gazebo Transport talker.
 
 ```
 # Shell C:
-ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"'
+ign topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"'
 ```
 
 ## Example 1b: ROS 2 talker and Gazebo Transport listener
@@ -112,7 +120,7 @@ Start the parameter bridge which will watch the specified topics.
 ```
 # Shell A:
 . ~/bridge_ws/install/setup.bash
-ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg
+ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@gz.msgs.StringMsg
 ```
 
 Now we start the Gazebo Transport listener.
@@ -157,7 +165,7 @@ Then we start the parameter bridge with the previous topic.
 ```
 # Shell B:
 . ~/bridge_ws/install/setup.bash
-ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@ignition.msgs.Image
+ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image
 ```
 
 Now we start the ROS GUI:
@@ -198,7 +206,7 @@ On terminal B, we start a ROS 2 listener:
 
 And terminal C, publish an Gazebo message:
 
-`ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"'`
+`ign topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"'`
 
 At this point, you should see the ROS 2 listener echoing the message.
 
@@ -246,35 +254,35 @@ bridge may be specified:
  # Set just topic name, applies to both
 - topic_name: "chatter"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
 
 # Set just ROS topic name, applies to both
 - ros_topic_name: "chatter_ros"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
 
 # Set just GZ topic name, applies to both
 - gz_topic_name: "chatter_ign"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
 
 # Set each topic name explicitly
 - ros_topic_name: "chatter_both_ros"
   gz_topic_name: "chatter_both_ign"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
 
 # Full set of configurations
 - ros_topic_name: "ros_chatter"
   gz_topic_name: "ign_chatter"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
   subscriber_queue: 5       # Default 10
   publisher_queue: 6        # Default 10
   lazy: true                # Default "false"
   direction: BIDIRECTIONAL  # Default "BIDIRECTIONAL" - Bridge both directions
-                            # "GZ_TO_ROS" - Bridge Ignition topic to ROS
-                            # "ROS_TO_GZ" - Bridge ROS topic to Ignition
+                            # "GZ_TO_ROS" - Bridge Gz topic to ROS
+                            # "ROS_TO_GZ" - Bridge ROS topic to Gz
 ```
 
 To run the bridge node with the above configuration:

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('| {:35}| {:35}|'.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))
 

ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp

@@ -35,6 +35,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/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
@@ -178,6 +179,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(

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -41,6 +41,7 @@
         Mapping('Transform', 'Pose'),
         Mapping('TransformStamped', 'Pose'),
         Mapping('Twist', 'Twist'),
+        Mapping('TwistStamped', 'Twist'),
         Mapping('TwistWithCovariance', 'TwistWithCovariance'),
         Mapping('TwistWithCovarianceStamped', 'TwistWithCovariance'),
         Mapping('Wrench', 'Wrench'),

ros_gz_bridge/src/convert/geometry_msgs.cpp

@@ -277,6 +277,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(

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -420,6 +420,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()));
 }

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -465,6 +465,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);

ros_gz_bridge/test/utils/ros_test_msg.hpp

@@ -39,6 +39,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/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
@@ -310,6 +311,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);