diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index f89837c2..c6644292 100644 --- 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 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..e484ac83 100644 --- 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 #include #include +<<<<<<< HEAD +======= +#include +#include +>>>>>>> f9afb69 (Add support for gz.msgs.EntityWrench (base branch: ros2) (#573)) #include #include #include @@ -298,6 +303,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