diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index e9a062ce..aff3b379 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -5,63 +5,64 @@ between ROS and Gazebo Transport. The following message types can be bridged for topics: -| ROS type | Gazebo type | -|--------------------------------------|:--------------------------------------:| -| builtin_interfaces/msg/Time | ignition::msgs::Time | -| std_msgs/msg/Bool | ignition::msgs::Boolean | -| std_msgs/msg/ColorRGBA | ignition::msgs::Color | -| std_msgs/msg/Empty | ignition::msgs::Empty | -| std_msgs/msg/Float32 | ignition::msgs::Float | -| std_msgs/msg/Float64 | ignition::msgs::Double | -| std_msgs/msg/Header | ignition::msgs::Header | -| std_msgs/msg/Int32 | ignition::msgs::Int32 | -| std_msgs/msg/UInt32 | ignition::msgs::UInt32 | -| std_msgs/msg/String | ignition::msgs::StringMsg | -| geometry_msgs/msg/Wrench | ignition::msgs::Wrench | -| geometry_msgs/msg/WrenchStamped | ignition::msgs::Wrench | -| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion | -| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d | -| geometry_msgs/msg/Point | ignition::msgs::Vector3d | -| geometry_msgs/msg/Pose | ignition::msgs::Pose | -| geometry_msgs/msg/PoseArray | ignition::msgs::Pose_V | -| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance | -| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose | -| 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 | -| rcl_interfaces/msg/ParameterValue | ignition::msgs::Any | -| ros_gz_interfaces/msg/Altimeter | ignition::msgs::Altimeter | -| ros_gz_interfaces/msg/Contact | ignition::msgs::Contact | -| ros_gz_interfaces/msg/Contacts | ignition::msgs::Contacts | -| ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | -| ros_gz_interfaces/msg/Entity | ignition::msgs::Entity | -| ros_gz_interfaces/msg/Float32Array | ignition::msgs::Float_V | -| ros_gz_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | -| ros_gz_interfaces/msg/JointWrench | ignition::msgs::JointWrench | -| ros_gz_interfaces/msg/Light | ignition::msgs::Light | -| ros_gz_interfaces/msg/SensorNoise | ignition::msgs::SensorNoise | -| ros_gz_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | -| ros_gz_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | -| ros_gz_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | -| ros_gz_interfaces/msg/WorldControl | ignition::msgs::WorldControl | -| rosgraph_msgs/msg/Clock | ignition::msgs::Clock | -| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | -| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | -| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure | -| sensor_msgs/msg/Imu | ignition::msgs::IMU | -| sensor_msgs/msg/Image | ignition::msgs::Image | -| sensor_msgs/msg/JointState | ignition::msgs::Model | -| sensor_msgs/msg/Joy | ignition::msgs::Joy | -| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | -| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | -| sensor_msgs/msg/NavSatFix | ignition::msgs::NavSat | -| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | -| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | -| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory | +| ROS type | Gazebo type | +|---------------------------------------------|:--------------------------------------:| +| builtin_interfaces/msg/Time | ignition::msgs::Time | +| std_msgs/msg/Bool | ignition::msgs::Boolean | +| std_msgs/msg/ColorRGBA | ignition::msgs::Color | +| std_msgs/msg/Empty | ignition::msgs::Empty | +| std_msgs/msg/Float32 | ignition::msgs::Float | +| std_msgs/msg/Float64 | ignition::msgs::Double | +| std_msgs/msg/Header | ignition::msgs::Header | +| std_msgs/msg/Int32 | ignition::msgs::Int32 | +| std_msgs/msg/UInt32 | ignition::msgs::UInt32 | +| std_msgs/msg/String | ignition::msgs::StringMsg | +| geometry_msgs/msg/Wrench | ignition::msgs::Wrench | +| geometry_msgs/msg/WrenchStamped | ignition::msgs::Wrench | +| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion | +| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d | +| geometry_msgs/msg/Point | ignition::msgs::Vector3d | +| geometry_msgs/msg/Pose | ignition::msgs::Pose | +| geometry_msgs/msg/PoseArray | ignition::msgs::Pose_V | +| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance | +| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose | +| 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 | +| geometry_msgs/msg/TwistWithCovarianceStamped| ignition::msgs::TwistWithCovariance | +| nav_msgs/msg/Odometry | ignition::msgs::Odometry | +| nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | +| rcl_interfaces/msg/ParameterValue | ignition::msgs::Any | +| ros_gz_interfaces/msg/Altimeter | ignition::msgs::Altimeter | +| ros_gz_interfaces/msg/Contact | ignition::msgs::Contact | +| ros_gz_interfaces/msg/Contacts | ignition::msgs::Contacts | +| ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | +| ros_gz_interfaces/msg/Entity | ignition::msgs::Entity | +| ros_gz_interfaces/msg/Float32Array | ignition::msgs::Float_V | +| ros_gz_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | +| ros_gz_interfaces/msg/JointWrench | ignition::msgs::JointWrench | +| ros_gz_interfaces/msg/Light | ignition::msgs::Light | +| ros_gz_interfaces/msg/SensorNoise | ignition::msgs::SensorNoise | +| ros_gz_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | +| ros_gz_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | +| ros_gz_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | +| ros_gz_interfaces/msg/WorldControl | ignition::msgs::WorldControl | +| rosgraph_msgs/msg/Clock | ignition::msgs::Clock | +| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | +| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | +| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure | +| sensor_msgs/msg/Imu | ignition::msgs::IMU | +| sensor_msgs/msg/Image | ignition::msgs::Image | +| sensor_msgs/msg/JointState | ignition::msgs::Model | +| sensor_msgs/msg/Joy | ignition::msgs::Joy | +| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | +| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | +| sensor_msgs/msg/NavSatFix | ignition::msgs::NavSat | +| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | +| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | +| trajectory_msgs/msg/JointTrajectory | ignition::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 43860e69..70537def 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 @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -189,6 +190,18 @@ convert_gz_to_ros( const gz::msgs::TwistWithCovariance & gz_msg, geometry_msgs::msg::TwistWithCovariance & ros_msg); +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg, + gz::msgs::TwistWithCovariance & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::TwistWithCovariance & gz_msg, + geometry_msgs::msg::TwistWithCovarianceStamped & 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 c8788c89..649fd6e3 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -42,6 +42,7 @@ Mapping('Twist', 'Twist'), Mapping('TwistStamped', 'Twist'), Mapping('TwistWithCovariance', 'TwistWithCovariance'), + Mapping('TwistWithCovarianceStamped', 'TwistWithCovariance'), Mapping('Wrench', 'Wrench'), Mapping('WrenchStamped', 'Wrench'), Mapping('Vector3', 'Vector3d'), diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp index 60ae1413..ad83ba6d 100644 --- a/ros_gz_bridge/src/convert/geometry_msgs.cpp +++ b/ros_gz_bridge/src/convert/geometry_msgs.cpp @@ -306,6 +306,26 @@ convert_gz_to_ros( } } +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg, + gz::msgs::TwistWithCovariance & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_twist()->mutable_header())); + convert_ros_to_gz(ros_msg.twist, gz_msg); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::TwistWithCovariance & gz_msg, + geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg) +{ + convert_gz_to_ros(gz_msg.twist().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 505ed922..b0bf4d2c 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -428,12 +428,15 @@ void createTestMsg(gz::msgs::TwistWithCovariance & _msg) { gz::msgs::Vector3d linear_msg; gz::msgs::Vector3d angular_msg; + gz::msgs::Header header_msg; createTestMsg(linear_msg); createTestMsg(angular_msg); + createTestMsg(header_msg); _msg.mutable_twist()->mutable_linear()->CopyFrom(linear_msg); _msg.mutable_twist()->mutable_angular()->CopyFrom(angular_msg); + _msg.mutable_twist()->mutable_header()->CopyFrom(header_msg); 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 6c30e98b..eea3a948 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -477,6 +477,18 @@ void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->twist)); + compareTestMsg(std::make_shared(_msg->header)); +} + void createTestMsg(geometry_msgs::msg::Wrench & _msg) { createTestMsg(_msg.force); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index d1e6caf2..589f4de1 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -314,6 +315,14 @@ void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _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::TwistWithCovarianceStamped & _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::Wrench & _msg);