diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 8decf383..08743a66 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -18,6 +18,7 @@ The following message types can be bridged for topics: | 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 | 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 c71a4973..edee9b44 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 @@ -187,6 +188,18 @@ convert_gz_to_ros( const ignition::msgs::Wrench & gz_msg, geometry_msgs::msg::Wrench & ros_msg); +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::WrenchStamped & ros_msg, + ignition::msgs::Wrench & gz_msg); + +template<> +void +convert_gz_to_ros( + const ignition::msgs::Wrench & gz_msg, + geometry_msgs::msg::WrenchStamped & ros_msg); + } // namespace ros_gz_bridge #endif // ROS_GZ_BRIDGE__CONVERT__GEOMETRY_MSGS_HPP_ diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 5befb5d9..3b3197e7 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -39,6 +39,7 @@ Mapping('Twist', 'Twist'), Mapping('TwistWithCovariance', 'TwistWithCovariance'), Mapping('Wrench', 'Wrench'), + Mapping('WrenchStamped', 'Wrench'), Mapping('Vector3', 'Vector3d'), ], 'nav_msgs': [ diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp index 2e1710db..fd2ad3d3 100644 --- a/ros_gz_bridge/src/convert/geometry_msgs.cpp +++ b/ros_gz_bridge/src/convert/geometry_msgs.cpp @@ -306,4 +306,26 @@ convert_gz_to_ros( convert_gz_to_ros(gz_msg.torque(), ros_msg.torque); } +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::WrenchStamped & ros_msg, + ignition::msgs::Wrench & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + convert_ros_to_gz(ros_msg.wrench.force, (*gz_msg.mutable_force())); + convert_ros_to_gz(ros_msg.wrench.torque, (*gz_msg.mutable_torque())); +} + +template<> +void +convert_gz_to_ros( + const ignition::msgs::Wrench & gz_msg, + geometry_msgs::msg::WrenchStamped & ros_msg) +{ + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + convert_gz_to_ros(gz_msg.force(), ros_msg.wrench.force); + convert_gz_to_ros(gz_msg.torque(), ros_msg.wrench.torque); +} + } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index b3833471..e0c5bceb 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -446,6 +446,19 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->torque)); } +void createTestMsg(geometry_msgs::msg::WrenchStamped & _msg) +{ + createTestMsg(_msg.header); + createTestMsg(_msg.wrench.force); + createTestMsg(_msg.wrench.torque); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->header)); + compareTestMsg(std::make_shared(_msg->wrench.force)); + compareTestMsg(std::make_shared(_msg->wrench.torque)); +} void createTestMsg(ros_gz_interfaces::msg::Light & _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 32dfd8a2..8d14135d 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -298,6 +299,14 @@ void createTestMsg(geometry_msgs::msg::Wrench & _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::WrenchStamped & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// tf2_msgs /// \brief Create a message used for testing.