[backport humble] Added Altimeter msg bridging (#413)

ros_gz_bridge/README.md

@@ -32,30 +32,31 @@ 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              |
 | ros_gz_interfaces/msg/JointWrench    | ignition::msgs::JointWrench            |
 | 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             |
 | sensor_msgs/msg/FluidPressure        | ignition::msgs::FluidPressure          |
 | 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.
 

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 <ignition/msgs/altimeter.pb.h>
 #include <ignition/msgs/entity.pb.h>
 #include <ignition/msgs/joint_wrench.pb.h>
 #include <ignition/msgs/contact.pb.h>
@@ -31,6 +32,7 @@
 #include <ignition/msgs/world_control.pb.h>
 
 // ROS 2 messages
+#include <ros_gz_interfaces/msg/altimeter.hpp>
 #include <ros_gz_interfaces/msg/entity.hpp>
 #include <ros_gz_interfaces/msg/joint_wrench.hpp>
 #include <ros_gz_interfaces/msg/contact.hpp>
@@ -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(

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'),

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(

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -247,6 +247,24 @@ void compareTestMsg(const std::shared_ptr<ignition::msgs::Vector3d> & _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<ignition::msgs::Altimeter> & _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<ignition::msgs::Header>(_msg->header()));
+}
 void createTestMsg(ignition::msgs::Param & _msg)
 {
   createTestMsg(*_msg.mutable_header());

ros_gz_bridge/test/utils/gz_test_msg.hpp

@@ -16,6 +16,7 @@
 #define UTILS__GZ_TEST_MSG_HPP_
 
 #include <ignition/msgs/actuators.pb.h>
+#include <ignition/msgs/altimeter.pb.h>
 #include <ignition/msgs/any.pb.h>
 #include <ignition/msgs/axis.pb.h>
 #include <ignition/msgs/battery_state.pb.h>
@@ -258,6 +259,14 @@ void createTestMsg(ignition::msgs::JointWrench & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<ignition::msgs::JointWrench> & _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<ignition::msgs::Altimeter> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(ignition::msgs::Joy & _msg);

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -700,6 +700,26 @@ void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::JointWrench> &
   compareTestMsg(std::make_shared<geometry_msgs::msg::Wrench>(_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<ros_gz_interfaces::msg::Altimeter> & _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;

ros_gz_bridge/test/utils/ros_test_msg.hpp

@@ -44,6 +44,7 @@
 #include <geometry_msgs/msg/wrench.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #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/gui_camera.hpp>
 #include <ros_gz_interfaces/msg/joint_wrench.hpp>
@@ -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<ros_gz_interfaces::msg::JointWrench> & _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<ros_gz_interfaces::msg::Altimeter> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(ros_gz_interfaces::msg::Light & _msg);

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"

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