Introduce WrenchStamped into bridge (#327)

Signed-off-by: Lovro <lovro.ivanov@gmail.com>
gazebosim · Feb 8, 2023 · 8a5f768 · 8a5f768
1 parent 74dcf50
commit 8a5f768
Show file tree

Hide file tree

Showing 6 changed files with 59 additions and 0 deletions.
diff --git 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
@@ -36,6 +36,7 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
 
 #include <ros_gz_bridge/convert_decl.hpp>
 
@@ -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
@@ -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
@@ -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
@@ -446,6 +446,19 @@ void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::Wrench> & _msg)
   compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_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<geometry_msgs::msg::WrenchStamped> & _msg)
+{
+  compareTestMsg(std::make_shared<std_msgs::msg::Header>(_msg->header));
+  compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_msg->wrench.force));
+  compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_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
@@ -41,6 +41,7 @@
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <ros_gz_interfaces/msg/entity.hpp>
 #include <ros_gz_interfaces/msg/gui_camera.hpp>
@@ -298,6 +299,14 @@ void createTestMsg(geometry_msgs::msg::Wrench & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::Wrench> & _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<geometry_msgs::msg::WrenchStamped> & _msg);
+
 /// tf2_msgs
 
 /// \brief Create a message used for testing.