diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 30d0719e..248dd0b4 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -5,61 +5,62 @@ between ROS and Gazebo Transport. The following message types can be bridged for topics: -| ROS type | Gazebo Transport Type | -|------------------------------------|:----------------------------------:| -| builtin_interfaces/Time | gz.msgs.Time | -| geometry_msgs/Point | gz.msgs.Vector3d | -| geometry_msgs/Pose | gz.msgs.Pose | -| geometry_msgs/msg/PoseArray | gz.msgs.Pose_V | -| geometry_msgs/PoseStamped | gz.msgs.Pose | -| geometry_msgs/PoseWithCovariance | gz.msgs.PoseWithCovariance | -| geometry_msgs/Quaternion | gz.msgs.Quaternion | -| geometry_msgs/Transform | gz.msgs.Pose | -| geometry_msgs/TransformStamped | gz.msgs.Pose | -| geometry_msgs/Twist | gz.msgs.Twist | -| geometry_msgs/TwistWithCovariance | gz.msgs.TwistWithCovariance | -| geometry_msgs/Vector3 | gz.msgs.Vector3d | -| geometry_msgs/Wrench | gz.msgs.Wrench | -| gps_msgs/GPSFix | gz.msgs.NavSat | -| nav_msgs/Odometry | gz.msgs.Odometry | -| nav_msgs/Odometry | gz.msgs.OdometryWithCovariance | -| rcl_interfaces/ParameterValue | gz.msgs.Any | -| ros_gz_interfaces/Contact | gz.msgs.Contact | -| ros_gz_interfaces/Contacts | gz.msgs.Contacts | -| ros_gz_interfaces/Dataframe | gz.msgs.Dataframe | -| ros_gz_interfaces/Entity | gz.msgs.Entity | -| ros_gz_interfaces/msg/Float32Array | gz.msgs.Float_V | -| ros_gz_interfaces/GuiCamera | gz.msgs.GUICamera | -| ros_gz_interfaces/JointWrench | gz.msgs.JointWrench | -| ros_gz_interfaces/Light | gz.msgs.Light | -| ros_gz_interfaces/ParamVec | gz.msgs.Param | -| ros_gz_interfaces/ParamVec | gz.msgs.Param_V | -| ros_gz_interfaces/StringVec | gz.msgs.StringMsg_V | -| ros_gz_interfaces/TrackVisual | gz.msgs.TrackVisual | -| ros_gz_interfaces/VideoRecord | gz.msgs.VideoRecord | -| rosgraph_msgs/Clock | gz.msgs.Clock | -| sensor_msgs/BatteryState | gz.msgs.BatteryState | -| sensor_msgs/CameraInfo | gz.msgs.CameraInfo | -| sensor_msgs/FluidPressure | gz.msgs.FluidPressure | -| sensor_msgs/Image | gz.msgs.Image | -| sensor_msgs/Imu | gz.msgs.IMU | -| sensor_msgs/JointState | gz.msgs.Model | -| sensor_msgs/Joy | gz.msgs.Joy | -| sensor_msgs/LaserScan | gz.msgs.LaserScan | -| sensor_msgs/MagneticField | gz.msgs.Magnetometer | -| sensor_msgs/NavSatFix | gz.msgs.NavSat | -| sensor_msgs/PointCloud2 | gz.msgs.PointCloudPacked | -| std_msgs/Bool | gz.msgs.Boolean | -| std_msgs/ColorRGBA | gz.msgs.Color | -| std_msgs/Empty | gz.msgs.Empty | -| std_msgs/Float32 | gz.msgs.Float | -| std_msgs/Float64 | gz.msgs.Double | -| std_msgs/Header | gz.msgs.Header | -| std_msgs/Int32 | gz.msgs.Int32 | -| std_msgs/String | gz.msgs.StringMsg | -| std_msgs/UInt32 | gz.msgs.UInt32 | -| tf2_msgs/TFMessage | gz.msgs.Pose_V | -| trajectory_msgs/JointTrajectory | gz.msgs.JointTrajectory | +| ROS type | Gazebo Transport Type | +| --------------------------------------- | :----------------------------- | +| builtin_interfaces/Time | gz.msgs.Time | +| geometry_msgs/Point | gz.msgs.Vector3d | +| geometry_msgs/Pose | gz.msgs.Pose | +| geometry_msgs/msg/PoseArray | gz.msgs.Pose_V | +| geometry_msgs/PoseStamped | gz.msgs.Pose | +| geometry_msgs/PoseWithCovariance | gz.msgs.PoseWithCovariance | +| geometry_msgs/PoseWithCovarianceStamped | gz.msgs.PoseWithCovariance | +| geometry_msgs/Quaternion | gz.msgs.Quaternion | +| geometry_msgs/Transform | gz.msgs.Pose | +| geometry_msgs/TransformStamped | gz.msgs.Pose | +| geometry_msgs/Twist | gz.msgs.Twist | +| geometry_msgs/TwistWithCovariance | gz.msgs.TwistWithCovariance | +| geometry_msgs/Vector3 | gz.msgs.Vector3d | +| geometry_msgs/Wrench | gz.msgs.Wrench | +| gps_msgs/GPSFix | gz.msgs.NavSat | +| nav_msgs/Odometry | gz.msgs.Odometry | +| nav_msgs/Odometry | gz.msgs.OdometryWithCovariance | +| rcl_interfaces/ParameterValue | gz.msgs.Any | +| ros_gz_interfaces/Contact | gz.msgs.Contact | +| ros_gz_interfaces/Contacts | gz.msgs.Contacts | +| ros_gz_interfaces/Dataframe | gz.msgs.Dataframe | +| ros_gz_interfaces/Entity | gz.msgs.Entity | +| ros_gz_interfaces/msg/Float32Array | gz.msgs.Float_V | +| ros_gz_interfaces/GuiCamera | gz.msgs.GUICamera | +| ros_gz_interfaces/JointWrench | gz.msgs.JointWrench | +| ros_gz_interfaces/Light | gz.msgs.Light | +| ros_gz_interfaces/ParamVec | gz.msgs.Param | +| ros_gz_interfaces/ParamVec | gz.msgs.Param_V | +| ros_gz_interfaces/StringVec | gz.msgs.StringMsg_V | +| ros_gz_interfaces/TrackVisual | gz.msgs.TrackVisual | +| ros_gz_interfaces/VideoRecord | gz.msgs.VideoRecord | +| rosgraph_msgs/Clock | gz.msgs.Clock | +| sensor_msgs/BatteryState | gz.msgs.BatteryState | +| sensor_msgs/CameraInfo | gz.msgs.CameraInfo | +| sensor_msgs/FluidPressure | gz.msgs.FluidPressure | +| sensor_msgs/Image | gz.msgs.Image | +| sensor_msgs/Imu | gz.msgs.IMU | +| sensor_msgs/JointState | gz.msgs.Model | +| sensor_msgs/Joy | gz.msgs.Joy | +| sensor_msgs/LaserScan | gz.msgs.LaserScan | +| sensor_msgs/MagneticField | gz.msgs.Magnetometer | +| sensor_msgs/NavSatFix | gz.msgs.NavSat | +| sensor_msgs/PointCloud2 | gz.msgs.PointCloudPacked | +| std_msgs/Bool | gz.msgs.Boolean | +| std_msgs/ColorRGBA | gz.msgs.Color | +| std_msgs/Empty | gz.msgs.Empty | +| std_msgs/Float32 | gz.msgs.Float | +| std_msgs/Float64 | gz.msgs.Double | +| std_msgs/Header | gz.msgs.Header | +| std_msgs/Int32 | gz.msgs.Int32 | +| std_msgs/String | gz.msgs.StringMsg | +| std_msgs/UInt32 | gz.msgs.UInt32 | +| tf2_msgs/TFMessage | gz.msgs.Pose_V | +| trajectory_msgs/JointTrajectory | gz.msgs.JointTrajectory | And the following for services: 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 719e8b26..5d75ceb2 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 @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -109,6 +110,18 @@ convert_ros_to_gz( const geometry_msgs::msg::PoseWithCovariance & ros_msg, gz::msgs::PoseWithCovariance & gz_msg); +template<> +void +convert_gz_to_ros( + const gz::msgs::PoseWithCovariance & gz_msg, + geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg); + +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg, + gz::msgs::PoseWithCovariance & gz_msg); + template<> void convert_gz_to_ros( diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 3597b09b..5b9a9814 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -33,6 +33,7 @@ Mapping('PoseArray', 'Pose_V'), Mapping('PoseStamped', 'Pose'), Mapping('PoseWithCovariance', 'PoseWithCovariance'), + Mapping('PoseWithCovarianceStamped', 'PoseWithCovariance'), Mapping('Quaternion', 'Quaternion'), Mapping('Transform', 'Pose'), Mapping('TransformStamped', 'Pose'), diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp index d4fc29eb..4a446417 100644 --- a/ros_gz_bridge/src/convert/geometry_msgs.cpp +++ b/ros_gz_bridge/src/convert/geometry_msgs.cpp @@ -165,6 +165,27 @@ convert_gz_to_ros( } } +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg, + gz::msgs::PoseWithCovariance & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_pose()->mutable_header())); + convert_ros_to_gz(ros_msg.pose, gz_msg); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::PoseWithCovariance & gz_msg, + geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg) +{ + convert_gz_to_ros(gz_msg.pose().header(), ros_msg.header); + convert_gz_to_ros(gz_msg, ros_msg.pose); +} + + 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 981e9684..7fc73259 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -323,6 +323,7 @@ void createTestMsg(gz::msgs::PoseWithCovariance & _msg) { createTestMsg(*_msg.mutable_pose()->mutable_position()); createTestMsg(*_msg.mutable_pose()->mutable_orientation()); + createTestMsg(*_msg.mutable_pose()->mutable_header()); for (int i = 0; i < 36; i++) { _msg.mutable_covariance()->add_data(i); } diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 7a534749..7d22e76e 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -343,6 +343,18 @@ void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->pose)); + compareTestMsg(std::make_shared(_msg->header)); +} + void createTestMsg(geometry_msgs::msg::PoseStamped & _msg) { createTestMsg(_msg.header); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index ec543bb3..db22aa7a 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -245,6 +246,18 @@ void createTestMsg(geometry_msgs::msg::PoseWithCovariance & _msg); /// \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::PoseWithCovarianceStamped & _msg); + +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(geometry_msgs::msg::PoseWithCovarianceStamped & _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::PoseStamped & _msg);