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

Signed-off-by: Victor T. N. <[email protected]> (cherry picked from commit f9afb69) # Conflicts: # ros_gz_bridge/README.md # ros_gz_bridge/test/utils/gz_test_msg.hpp
gazebosim · Jul 1, 2024 · fafea1c · fafea1c
1 parent 02ea7f6
commit fafea1c
Show file tree

Hide file tree

Showing 11 changed files with 175 additions and 0 deletions.
diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md
@@ -5,6 +5,7 @@ between ROS and Gazebo Transport.
 
 The following message types can be bridged for topics:
 
+<<<<<<< HEAD
 | ROS type                                    | Gazebo type                                 |
 |---------------------------------------------|:-------------------------------------------:|
 | builtin_interfaces/msg/Time                 | ignition::msgs::Time                        |
@@ -66,6 +67,75 @@ The following message types can be bridged for topics:
 | trajectory_msgs/msg/JointTrajectory         | ignition::msgs::JointTrajectory             |
 | vision_msgs/msg/Detection3D                 | ignition::msgs::AnnotatedOriented3DBox      |
 | vision_msgs/msg/Detection3DArray            | ignition::msgs::AnnotatedOriented3DBox_V    |
+=======
+| ROS type                                       | Gazebo Transport Type               |
+| ---------------------------------------------- | :------------------------------:    |
+| actuator_msgs/msg/Actuators                    | gz.msgs.Actuators                   |
+| builtin_interfaces/msg/Time                    | gz.msgs.Time                        |
+| geometry_msgs/msg/Point                        | gz.msgs.Vector3d                    |
+| geometry_msgs/msg/Pose                         | gz.msgs.Pose                        |
+| geometry_msgs/msg/PoseArray                    | gz.msgs.Pose_V                      |
+| geometry_msgs/msg/PoseStamped                  | gz.msgs.Pose                        |
+| geometry_msgs/msg/PoseWithCovariance           | gz.msgs.PoseWithCovariance          |
+| geometry_msgs/msg/PoseWithCovarianceStamped    | gz.msgs.PoseWithCovariance          |
+| geometry_msgs/msg/Quaternion                   | gz.msgs.Quaternion                  |
+| geometry_msgs/msg/Transform                    | gz.msgs.Pose                        |
+| geometry_msgs/msg/TransformStamped             | gz.msgs.Pose                        |
+| geometry_msgs/msg/Twist                        | gz.msgs.Twist                       |
+| geometry_msgs/msg/TwistStamped                 | gz.msgs.Twist                       |
+| geometry_msgs/msg/TwistWithCovariance          | gz.msgs.TwistWithCovariance         |
+| geometry_msgs/msg/TwistWithCovarianceStamped   | gz.msgs.TwistWithCovariance         |
+| geometry_msgs/msg/Vector3                      | gz.msgs.Vector3d                    |
+| geometry_msgs/msg/Wrench                       | gz.msgs.Wrench                      |
+| geometry_msgs/msg/WrenchStamped                | gz.msgs.Wrench                      |
+| gps_msgs/msg/GPSFix                            | gz.msgs.NavSat                      |
+| nav_msgs/msg/Odometry                          | gz.msgs.Odometry                    |
+| nav_msgs/msg/Odometry                          | gz.msgs.OdometryWithCovariance      |
+| rcl_interfaces/msg/ParameterValue              | gz.msgs.Any                         |
+| ros_gz_interfaces/msg/Altimeter                | gz.msgs.Altimeter                   |
+| ros_gz_interfaces/msg/Contact                  | gz.msgs.Contact                     |
+| 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                 |
+| ros_gz_interfaces/msg/Light                    | gz.msgs.Light                       |
+| ros_gz_interfaces/msg/ParamVec                 | gz.msgs.Param                       |
+| ros_gz_interfaces/msg/ParamVec                 | gz.msgs.Param_V                     |
+| ros_gz_interfaces/msg/SensorNoise              | gz.msgs.SensorNoise                 |
+| ros_gz_interfaces/msg/StringVec                | gz.msgs.StringMsg_V                 |
+| ros_gz_interfaces/msg/TrackVisual              | gz.msgs.TrackVisual                 |
+| ros_gz_interfaces/msg/VideoRecord              | gz.msgs.VideoRecord                 |
+| rosgraph_msgs/msg/Clock                        | gz.msgs.Clock                       |
+| sensor_msgs/msg/BatteryState                   | gz.msgs.BatteryState                |
+| sensor_msgs/msg/CameraInfo                     | gz.msgs.CameraInfo                  |
+| sensor_msgs/msg/FluidPressure                  | gz.msgs.FluidPressure               |
+| sensor_msgs/msg/Image                          | gz.msgs.Image                       |
+| sensor_msgs/msg/Imu                            | gz.msgs.IMU                         |
+| sensor_msgs/msg/JointState                     | gz.msgs.Model                       |
+| sensor_msgs/msg/Joy                            | gz.msgs.Joy                         |
+| sensor_msgs/msg/LaserScan                      | gz.msgs.LaserScan                   |
+| sensor_msgs/msg/MagneticField                  | gz.msgs.Magnetometer                |
+| sensor_msgs/msg/NavSatFix                      | gz.msgs.NavSat                      |
+| sensor_msgs/msg/PointCloud2                    | gz.msgs.PointCloudPacked            |
+| std_msgs/msg/Bool                              | gz.msgs.Boolean                     |
+| std_msgs/msg/ColorRGBA                         | gz.msgs.Color                       |
+| std_msgs/msg/Empty                             | gz.msgs.Empty                       |
+| std_msgs/msg/Float32                           | gz.msgs.Float                       |
+| std_msgs/msg/Float64                           | gz.msgs.Double                      |
+| std_msgs/msg/Header                            | gz.msgs.Header                      |
+| std_msgs/msg/Int32                             | gz.msgs.Int32                       |
+| std_msgs/msg/String                            | gz.msgs.StringMsg                   |
+| std_msgs/msg/UInt32                            | gz.msgs.UInt32                      |
+| tf2_msgs/msg/TFMessage                         | gz.msgs.Pose_V                      |
+| trajectory_msgs/msg/JointTrajectory            | gz.msgs.JointTrajectory             |
+| vision_msgs/msg/Detection2D                    | gz.msgs.AnnotatedAxisAligned2DBox   |
+| vision_msgs/msg/Detection2DArray               | gz.msgs.AnnotatedAxisAligned2DBox_V |
+| vision_msgs/msg/Detection3D                    | gz::msgs::AnnotatedOriented3DBox    |
+| vision_msgs/msg/Detection3DArray               | gz::msgs::AnnotatedOriented3DBox_V  |
+>>>>>>> f9afb69 (Add support for gz.msgs.EntityWrench (base branch: ros2) (#573))
 
 And the following for services:
 

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>
@@ -35,6 +36,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>
@@ -102,6 +104,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
@@ -62,6 +62,7 @@
         Mapping('Contact', 'Contact'),
         Mapping('Contacts', 'Contacts'),
         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
@@ -525,6 +525,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,11 @@
 #include <gz/msgs/double.pb.h>
 #include <gz/msgs/empty.pb.h>
 #include <gz/msgs/entity.pb.h>
+<<<<<<< HEAD
+=======
+#include <gz/msgs/entity_wrench.pb.h>
+#include <gz/msgs/dataframe.pb.h>
+>>>>>>> f9afb69 (Add support for gz.msgs.EntityWrench (base branch: ros2) (#573))
 #include <gz/msgs/float.pb.h>
 #include <gz/msgs/float_v.pb.h>
 #include <gz/msgs/fluid_pressure.pb.h>
@@ -298,6 +303,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
@@ -857,6 +857,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
@@ -49,6 +49,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>
@@ -418,6 +419,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