Add support for gz.msgs.EntityWrench (base branch: ros2) (#573) (#574)

Signed-off-by: Victor T. N. <[email protected]> (cherry picked from commit f9afb69) Co-authored-by: Victor T. Noppeney <[email protected]>
gazebosim · Jul 16, 2024 · b25e9ee · b25e9ee
1 parent c1072de
commit b25e9ee
Show file tree

Hide file tree

Showing 11 changed files with 102 additions and 0 deletions.
diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md
@@ -34,6 +34,7 @@ The following message types can be bridged for topics:
 | 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/EntityWrench             | gz.msgs.EntityWrench                |
 | 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                 |

diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp
@@ -18,6 +18,7 @@
 // Gazebo Msgs
 #include <gz/msgs/altimeter.pb.h>
 #include <gz/msgs/entity.pb.h>
+#include <gz/msgs/entity_wrench.pb.h>
 #include <gz/msgs/joint_wrench.pb.h>
 #include <gz/msgs/contact.pb.h>
 #include <gz/msgs/contacts.pb.h>
@@ -37,6 +38,7 @@
 // ROS 2 messages
 #include <ros_gz_interfaces/msg/altimeter.hpp>
 #include <ros_gz_interfaces/msg/entity.hpp>
+#include <ros_gz_interfaces/msg/entity_wrench.hpp>
 #include <ros_gz_interfaces/msg/joint_wrench.hpp>
 #include <ros_gz_interfaces/msg/contact.hpp>
 #include <ros_gz_interfaces/msg/contacts.hpp>
@@ -95,6 +97,18 @@ convert_gz_to_ros(
   const gz::msgs::Entity & gz_msg,
   ros_gz_interfaces::msg::Entity & ros_msg);
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::EntityWrench & ros_msg,
+  gz::msgs::EntityWrench & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::EntityWrench & gz_msg,
+  ros_gz_interfaces::msg::EntityWrench & 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
@@ -64,6 +64,7 @@
         Mapping('Contacts', 'Contacts'),
         Mapping('Dataframe', 'Dataframe'),
         Mapping('Entity', 'Entity'),
+        Mapping('EntityWrench', 'EntityWrench'),
         Mapping('Float32Array', 'Float_V'),
         Mapping('GuiCamera', 'GUICamera'),
         Mapping('JointWrench', 'JointWrench'),

diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp
@@ -139,6 +139,28 @@ convert_gz_to_ros(
   }
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::EntityWrench & ros_msg,
+  gz::msgs::EntityWrench & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+  convert_ros_to_gz(ros_msg.entity, (*gz_msg.mutable_entity()));
+  convert_ros_to_gz(ros_msg.wrench, (*gz_msg.mutable_wrench()));
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::EntityWrench & gz_msg,
+  ros_gz_interfaces::msg::EntityWrench & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  convert_gz_to_ros(gz_msg.entity(), ros_msg.entity);
+  convert_gz_to_ros(gz_msg.wrench(), ros_msg.wrench);
+}
+
 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
@@ -526,6 +526,30 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::Entity> & _msg)
   EXPECT_EQ(expected_msg.type(), _msg->type());
 }
 
+void createTestMsg(gz::msgs::EntityWrench & _msg)
+{
+  gz::msgs::Header header_msg;
+  gz::msgs::Entity entity_msg;
+  gz::msgs::Wrench wrench_msg;
+
+  createTestMsg(header_msg);
+  createTestMsg(entity_msg);
+  createTestMsg(wrench_msg);
+
+  _msg.mutable_header()->CopyFrom(header_msg);
+  _msg.mutable_entity()->CopyFrom(entity_msg);
+  _msg.mutable_wrench()->CopyFrom(wrench_msg);
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::EntityWrench> & _msg)
+{
+  gz::msgs::EntityWrench expected_msg;
+  createTestMsg(expected_msg);
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+  compareTestMsg(std::make_shared<gz::msgs::Entity>(_msg->entity()));
+  compareTestMsg(std::make_shared<gz::msgs::Wrench>(_msg->wrench()));
+}
+
 void createTestMsg(gz::msgs::Contact & _msg)
 {
   gz::msgs::Entity collision1;

diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp
@@ -29,6 +29,7 @@
 #include <gz/msgs/double.pb.h>
 #include <gz/msgs/empty.pb.h>
 #include <gz/msgs/entity.pb.h>
+#include <gz/msgs/entity_wrench.pb.h>
 #include <gz/msgs/dataframe.pb.h>
 #include <gz/msgs/float.pb.h>
 #include <gz/msgs/fluid_pressure.pb.h>
@@ -292,6 +293,14 @@ void createTestMsg(gz::msgs::Entity & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<gz::msgs::Entity> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::EntityWrench & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gz::msgs::EntityWrench> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(gz::msgs::Contact & _msg);

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp
@@ -867,6 +867,23 @@ void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::Entity> & _msg
   EXPECT_EQ(expected_msg.type, _msg->type);
 }
 
+void createTestMsg(ros_gz_interfaces::msg::EntityWrench & _msg)
+{
+  createTestMsg(_msg.header);
+  createTestMsg(_msg.entity);
+  createTestMsg(_msg.wrench);
+}
+
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::EntityWrench> & _msg)
+{
+  ros_gz_interfaces::msg::EntityWrench expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<std_msgs::msg::Header>(_msg->header));
+  compareTestMsg(std::make_shared<ros_gz_interfaces::msg::Entity>(_msg->entity));
+  compareTestMsg(std::make_shared<geometry_msgs::msg::Wrench>(_msg->wrench));
+}
+
 void createTestMsg(ros_gz_interfaces::msg::Contact & _msg)
 {
   createTestMsg(_msg.collision1);

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp
@@ -50,6 +50,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <ros_gz_interfaces/msg/altimeter.hpp>
 #include <ros_gz_interfaces/msg/entity.hpp>
+#include <ros_gz_interfaces/msg/entity_wrench.hpp>
 #include <ros_gz_interfaces/msg/gui_camera.hpp>
 #include <ros_gz_interfaces/msg/joint_wrench.hpp>
 #include <ros_gz_interfaces/msg/contact.hpp>
@@ -425,6 +426,14 @@ void createTestMsg(ros_gz_interfaces::msg::Entity & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::Entity> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(ros_gz_interfaces::msg::EntityWrench & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::EntityWrench> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(ros_gz_interfaces::msg::Contact & _msg);

diff --git a/ros_gz_interfaces/CMakeLists.txt b/ros_gz_interfaces/CMakeLists.txt
@@ -23,6 +23,7 @@ set(msg_files
   "msg/Dataframe.msg"
   "msg/Entity.msg"
   "msg/EntityFactory.msg"
+  "msg/EntityWrench.msg"
   "msg/Float32Array.msg"
   "msg/GuiCamera.msg"
   "msg/JointWrench.msg"

diff --git a/ros_gz_interfaces/README.md b/ros_gz_interfaces/README.md
@@ -8,6 +8,7 @@ This package currently contains some Gazebo-specific ROS message and service dat
 * [Contacts](msg/Contacts.msg): related to [ignition::msgs::Contacts](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/contacts.proto). A list of contacts.
 * [Entity](msg/Entity.msg): related to [ignition::msgs::Entity](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/entity.proto). Entity of Gazebo Sim.
 * [EntityFactory](msg/EntityFactory.msg): related to [ignition::msgs::EntityFactory](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/entity_factory.proto). Message to create a new entity.
+* [EntityWrench](msg/EntityWrench.msg): related to [ignition::msgs::EntityWrench](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/gz/msgs/entity_wrench.proto). Wrench to be applied to a specified Entity of Gazebo Sim.
 * [Light](msg/Light.msg): related to [ignition::msgs::Light](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/light.proto). Light info in Gazebo Sim.
 * [WorldControl](msg/WorldControl.msg): related to [ignition::msgs::WorldControl](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/world_control.proto). Message to control world of Gazebo Sim.
 * [WorldReset](msg/WorldReset.msg): related to [ignition::msgs::WorldReset](https://github.com/gazebosim/gz-msgs/blob/ign-msgs7/proto/ignition/msgs/world_reset.proto). Reset time and model of simulation.

diff --git a/ros_gz_interfaces/msg/EntityWrench.msg b/ros_gz_interfaces/msg/EntityWrench.msg
@@ -0,0 +1,3 @@
+std_msgs/Header header            # Time stamp
+ros_gz_interfaces/Entity entity   # Entity
+geometry_msgs/Wrench wrench       # Wrench to be applied to entity