From c2f271f4b40a3849fecb4d3257c604900f0434fa Mon Sep 17 00:00:00 2001 From: "Victor T. Noppeney" Date: Mon, 1 Jul 2024 08:43:38 -0300 Subject: [PATCH] Add support for gz.msgs.EntityWrench (base branch: ros2) (#573) Signed-off-by: Victor T. N. (cherry picked from commit f9afb69d1163633dd978024bb7271a28cf7b551a) --- ros_gz_bridge/README.md | 1 + .../convert/ros_gz_interfaces.hpp | 14 +++++++++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 1 + .../src/convert/ros_gz_interfaces.cpp | 22 +++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 24 +++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 9 +++++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 17 +++++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 +++++++ ros_gz_interfaces/CMakeLists.txt | 1 + ros_gz_interfaces/README.md | 1 + ros_gz_interfaces/msg/EntityWrench.msg | 3 +++ 11 files changed, 102 insertions(+) create mode 100644 ros_gz_interfaces/msg/EntityWrench.msg diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 98792d92..57289234 100644 --- 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 index 9b17d0cf..b49f02d7 100644 --- 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 #include +#include #include #include #include @@ -37,6 +38,7 @@ // ROS 2 messages #include #include +#include #include #include #include @@ -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 index 2414586f..203e33d1 100644 --- 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 index ae4665e5..eb923b34 100644 --- 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 index cbaff583..b13a06b1 100644 --- 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 & _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 & _msg) +{ + gz::msgs::EntityWrench expected_msg; + createTestMsg(expected_msg); + compareTestMsg(std::make_shared(_msg->header())); + compareTestMsg(std::make_shared(_msg->entity())); + compareTestMsg(std::make_shared(_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 index 0f9b712a..cff1a0af 100644 --- 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 #include #include +#include #include #include #include @@ -292,6 +293,14 @@ void createTestMsg(gz::msgs::Entity & _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(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 & _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 index 42afcb25..42f9b90b 100644 --- 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 & _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 & _msg) +{ + ros_gz_interfaces::msg::EntityWrench expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header)); + compareTestMsg(std::make_shared(_msg->entity)); + compareTestMsg(std::make_shared(_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 index 43c127e9..68211305 100644 --- 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 #include #include +#include #include #include #include @@ -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 & _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 & _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 index 5f3b312e..9f3d7a1f 100644 --- 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 index 88ed27d7..610cb7e3 100644 --- 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 new file mode 100644 index 00000000..c1a03987 --- /dev/null +++ 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 \ No newline at end of file