From a982161db38058f3c3b6b69f660184930484f077 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 7 Dec 2023 15:11:50 -0600 Subject: [PATCH] Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (#468) Signed-off-by: Addisu Z. Taddese --- ros_gz_bridge/README.md | 148 +++++++++--------- .../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, 137 insertions(+), 71 deletions(-) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index e7204b00..7a911460 100644 --- a/ros_gz_bridge/README.md +++ b/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: diff --git a/ros_gz_bridge/bin/ros_gz_bridge_markdown_table b/ros_gz_bridge/bin/ros_gz_bridge_markdown_table index 29b068fd..6e5d12e4 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('| {: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)) 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 e9661356..6416fa35 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 @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -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( diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 6c44a9e1..5aeb3fad 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/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'), diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp index 49bfce8e..01c782a5 100644 --- a/ros_gz_bridge/src/convert/geometry_msgs.cpp +++ b/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( diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 3dd63e9e..6cf82a3d 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/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 & _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 d9fcd06b..1de83956 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -465,6 +465,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 a8897901..f477df66 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -310,6 +311,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);