From 68288d4c2ffa5b5d6a2c8f355086be82a2f4a935 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 21 Jun 2023 09:52:43 +0200 Subject: [PATCH] [backport Iron] Added Altimeter msg bridging (#413) (#414) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aditya Pande Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Aditya Pande --- ros_gz_bridge/README.md | 15 ++++++------ .../convert/ros_gz_interfaces.hpp | 14 +++++++++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 1 + .../src/convert/ros_gz_interfaces.cpp | 24 +++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 18 ++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 9 +++++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 20 ++++++++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 +++++++ ros_gz_interfaces/CMakeLists.txt | 1 + ros_gz_interfaces/msg/Altimeter.msg | 13 ++++++++++ 10 files changed, 117 insertions(+), 7 deletions(-) create mode 100644 ros_gz_interfaces/msg/Altimeter.msg diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 0b1082ad..b60cfc1b 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -32,10 +32,11 @@ The following message types can be bridged for topics: | geometry_msgs/msg/TwistWithCovariance| ignition::msgs::TwistWithCovariance | | nav_msgs/msg/Odometry | ignition::msgs::Odometry | | nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | -| rcl_interfaces/msg/ParameterValue | ignition::msgs::Any | +| rcl_interfaces/msg/ParameterValue | ignition::msgs::Any | +| ros_gz_interfaces/msg/Altimeter | ignition.msgs.Altimeter | | ros_gz_interfaces/msg/Contact | ignition::msgs::Contact | | ros_gz_interfaces/msg/Contacts | ignition::msgs::Contacts | -| ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | +| ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | | ros_gz_interfaces/msg/Entity | ignition::msgs::Entity | | ros_gz_interfaces/msg/Float32Array | ignition::msgs::Float_V | | ros_gz_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | @@ -43,8 +44,8 @@ The following message types can be bridged for topics: | ros_gz_interfaces/msg/Light | ignition::msgs::Light | | ros_gz_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | | ros_gz_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | -| ros_gz_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | -| ros_gz_interfaces/msg/WorldControl | ignition::msgs::WorldControl | +| ros_gz_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | +| ros_gz_interfaces/msg/WorldControl | ignition::msgs::WorldControl | | rosgraph_msgs/msg/Clock | ignition::msgs::Clock | | sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | | sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | @@ -52,10 +53,10 @@ The following message types can be bridged for topics: | sensor_msgs/msg/Imu | ignition::msgs::IMU | | sensor_msgs/msg/Image | ignition::msgs::Image | | sensor_msgs/msg/JointState | ignition::msgs::Model | -| sensor_msgs/msg/Joy | ignition::msgs::Joy | +| sensor_msgs/msg/Joy | ignition::msgs::Joy | | sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | | sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | -| sensor_msgs/msg/NavSatFix | ignition::msgs::NavSat | +| sensor_msgs/msg/NavSatFix | ignition::msgs::NavSat | | sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | | tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | | trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory | @@ -64,7 +65,7 @@ And the following for services: | ROS type | Gazebo request | Gazebo response | |--------------------------------------|:--------------------------:| --------------------- | -| ros_gz_interfaces/srv/ControlWorld | ignition.msgs.WorldControl | ignition.msgs.Boolean | +| ros_gz_interfaces/srv/ControlWorld | ignition.msgs.WorldControl | ignition.msgs.Boolean | Run `ros2 run ros_gz_bridge parameter_bridge -h` for instructions. 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 9a71beb9..18d7f1d4 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 @@ -16,6 +16,7 @@ #define ROS_GZ_BRIDGE__CONVERT__ROS_GZ_INTERFACES_HPP_ // Gazebo Msgs +#include #include #include #include @@ -31,6 +32,7 @@ #include // ROS 2 messages +#include #include #include #include @@ -69,6 +71,18 @@ convert_gz_to_ros( const ignition::msgs::JointWrench & gz_msg, ros_gz_interfaces::msg::JointWrench & ros_msg); +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::Altimeter & ros_msg, + ignition::msgs::Altimeter & gz_msg); + +template<> +void +convert_gz_to_ros( + const ignition::msgs::Altimeter & gz_msg, + ros_gz_interfaces::msg::Altimeter & 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 3968ed6e..8c601e60 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -53,6 +53,7 @@ Mapping('ParameterValue', 'Any'), ], 'ros_gz_interfaces': [ + Mapping('Altimeter', 'Altimeter'), Mapping('Contact', 'Contact'), Mapping('Contacts', 'Contacts'), Mapping('Entity', 'Entity'), diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 6081f68b..fc6e6c32 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -47,6 +47,30 @@ convert_gz_to_ros( convert_gz_to_ros(gz_msg.body_2_wrench(), ros_msg.body_2_wrench); } +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::Altimeter & ros_msg, + ignition::msgs::Altimeter & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + gz_msg.set_vertical_position(ros_msg.vertical_position); + gz_msg.set_vertical_velocity(ros_msg.vertical_velocity); + gz_msg.set_vertical_reference(ros_msg.vertical_reference); +} + +template<> +void +convert_gz_to_ros( + const ignition::msgs::Altimeter & gz_msg, + ros_gz_interfaces::msg::Altimeter & ros_msg) +{ + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + ros_msg.vertical_position = gz_msg.vertical_position(); + ros_msg.vertical_velocity = gz_msg.vertical_velocity(); + ros_msg.vertical_reference = gz_msg.vertical_reference(); +} + 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 9ac737cf..b0bf97d5 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -247,6 +247,24 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_EQ(expected_msg.z(), _msg->z()); } +void createTestMsg(ignition::msgs::Altimeter & _msg) +{ + createTestMsg(*_msg.mutable_header()); + _msg.set_vertical_position(100); + _msg.set_vertical_velocity(200); + _msg.set_vertical_reference(300); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + ignition::msgs::Altimeter expected_msg; + createTestMsg(expected_msg); + + EXPECT_EQ(expected_msg.vertical_position(), _msg->vertical_position()); + EXPECT_EQ(expected_msg.vertical_velocity(), _msg->vertical_velocity()); + EXPECT_EQ(expected_msg.vertical_reference(), _msg->vertical_reference()); + compareTestMsg(std::make_shared(_msg->header())); +} void createTestMsg(ignition::msgs::Param & _msg) { createTestMsg(*_msg.mutable_header()); diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index 6ae8b41f..fd25d289 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -16,6 +16,7 @@ #define UTILS__GZ_TEST_MSG_HPP_ #include +#include #include #include #include @@ -258,6 +259,14 @@ void createTestMsg(ignition::msgs::JointWrench & _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(ignition::msgs::Altimeter & _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(ignition::msgs::Joy & _msg); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 9f2d880a..d1bcdd9c 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -700,6 +700,26 @@ void compareTestMsg(const std::shared_ptr & compareTestMsg(std::make_shared(_msg->body_2_wrench)); } +void createTestMsg(ros_gz_interfaces::msg::Altimeter & _msg) +{ + createTestMsg(_msg.header); + _msg.vertical_position = 100; + _msg.vertical_velocity = 200; + _msg.vertical_reference = 300; +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + ros_gz_interfaces::msg::Altimeter expected_msg; + createTestMsg(expected_msg); + + EXPECT_EQ(expected_msg.vertical_position, _msg->vertical_position); + EXPECT_EQ(expected_msg.vertical_velocity, _msg->vertical_velocity); + EXPECT_EQ(expected_msg.vertical_reference, _msg->vertical_reference); + + compareTestMsg(_msg->header); +} + void createTestMsg(ros_gz_interfaces::msg::Entity & _msg) { _msg.id = 1; diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index 5220d916..697e1079 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -348,6 +349,14 @@ void createTestMsg(ros_gz_interfaces::msg::JointWrench & _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::Altimeter & _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::Light & _msg); diff --git a/ros_gz_interfaces/CMakeLists.txt b/ros_gz_interfaces/CMakeLists.txt index 255736cf..ae656dd5 100644 --- a/ros_gz_interfaces/CMakeLists.txt +++ b/ros_gz_interfaces/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) set(msg_files + "msg/Altimeter.msg" "msg/Contact.msg" "msg/Contacts.msg" "msg/Dataframe.msg" diff --git a/ros_gz_interfaces/msg/Altimeter.msg b/ros_gz_interfaces/msg/Altimeter.msg new file mode 100644 index 00000000..5f820c2b --- /dev/null +++ b/ros_gz_interfaces/msg/Altimeter.msg @@ -0,0 +1,13 @@ +# A message for Altimeter readings. + +# Optional header data. +std_msgs/Header header + +# Vertical position data, in meters. +float64 vertical_position + +# Vertical velocity data, in meters/second. +float64 vertical_velocity + +# Vertical reference. +float64 vertical_reference