diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index f89837c2..c2f7ce3d 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -41,6 +41,7 @@ The following message types can be bridged for topics: | ros_gz_interfaces/msg/Contacts | ignition::msgs::Contacts | | ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | | ros_gz_interfaces/msg/Entity | ignition::msgs::Entity | +| ros_gz_interfaces/msg/EntityWrench | ignition::msgs::EntityWrench | | ros_gz_interfaces/msg/Float32Array | ignition::msgs::Float_V | | ros_gz_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | | ros_gz_interfaces/msg/JointWrench | ignition::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 136c8d9b..756ff2a8 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 @@ -35,6 +36,7 @@ // ROS 2 messages #include #include +#include #include #include #include @@ -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 index d17fb56d..d5923ffa 100644 --- 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 index 0d619955..57688b48 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 71a651b3..a966b8f7 100644 --- 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 & _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 ad063b62..c815a46d 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 @@ -298,6 +299,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 76e80d29..fcba33f4 100644 --- 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 & _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 6bc395cb..5fea2758 100644 --- 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 #include #include +#include #include #include #include @@ -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 & _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