Added WorldStatistics message support for the ros_gz_bridge (backport #841) (#847)

Signed-off-by: Alejandro Hernandez Cordero <ahcorde@gmail.com>
Co-authored-by: Andreas Loeffler <73336148+andreas-loeffler@users.noreply.github.com>
Co-authored-by: Alejandro Hernandez Cordero <ahcorde@gmail.com>

Author: 3 people

ros_gz_bridge/README.md

@@ -46,11 +46,13 @@ The following message types can be bridged for topics:
 | 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/LogPlaybackStatistics | ignition.msgs.LogPlaybackStatistics         |
 | ros_gz_interfaces/msg/SensorNoise           | ignition::msgs::SensorNoise                 |
 | 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/WorldStatistics       | ignition.msgs.WorldStatistics               |
 | rosgraph_msgs/msg/Clock                     | ignition::msgs::Clock                       |
 | sensor_msgs/msg/BatteryState                | ignition::msgs::BatteryState                |
 | sensor_msgs/msg/CameraInfo                  | ignition::msgs::CameraInfo                  |

ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp

@@ -26,13 +26,15 @@
 #include <gz/msgs/float_v.pb.h>
 #include <gz/msgs/gui_camera.pb.h>
 #include <gz/msgs/light.pb.h>
+#include <gz/msgs/log_playback_stats.pb.h>
 #include <gz/msgs/param.pb.h>
 #include <gz/msgs/param_v.pb.h>
 #include <gz/msgs/sensor_noise.pb.h>
 #include <gz/msgs/stringmsg_v.pb.h>
 #include <gz/msgs/track_visual.pb.h>
 #include <gz/msgs/video_record.pb.h>
 #include <gz/msgs/world_control.pb.h>
+#include <gz/msgs/world_stats.pb.h>
 
 // ROS 2 messages
 #include <ros_gz_interfaces/msg/altimeter.hpp>
@@ -45,12 +47,14 @@
 #include <ros_gz_interfaces/msg/float32_array.hpp>
 #include <ros_gz_interfaces/msg/gui_camera.hpp>
 #include <ros_gz_interfaces/msg/light.hpp>
+#include <ros_gz_interfaces/msg/log_playback_statistics.hpp>
 #include <ros_gz_interfaces/msg/param_vec.hpp>
 #include <ros_gz_interfaces/msg/sensor_noise.hpp>
 #include <ros_gz_interfaces/msg/string_vec.hpp>
 #include <ros_gz_interfaces/msg/track_visual.hpp>
 #include <ros_gz_interfaces/msg/video_record.hpp>
 #include <ros_gz_interfaces/msg/world_control.hpp>
+#include <ros_gz_interfaces/msg/world_statistics.hpp>
 
 // Required for HAVE_DATAFRAME definition
 #include <ros_gz_bridge/ros_gz_bridge.hpp>
@@ -309,6 +313,18 @@ convert_gz_to_ros(
   const gz::msgs::WorldReset & gz_msg,
   ros_gz_interfaces::msg::WorldReset & ros_msg);
 
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::WorldStatistics & gz_msg,
+  ros_gz_interfaces::msg::WorldStatistics & ros_msg);
+
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::WorldStatistics & ros_msg,
+  gz::msgs::WorldStatistics & gz_msg);
+
 template<>
 void
 convert_ros_to_gz(
@@ -320,6 +336,19 @@ void
 convert_gz_to_ros(
   const gz::msgs::Float_V & gz_msg,
   ros_gz_interfaces::msg::Float32Array & ros_msg);
+
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::LogPlaybackStatistics & ros_msg,
+  gz::msgs::LogPlaybackStatistics & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::LogPlaybackStatistics & gz_msg,
+  ros_gz_interfaces::msg::LogPlaybackStatistics & ros_msg);
+
 }  // namespace ros_gz_bridge
 
 #endif  // ROS_GZ_BRIDGE__CONVERT__ROS_GZ_INTERFACES_HPP_

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -68,12 +68,14 @@
         Mapping('GuiCamera', 'GUICamera'),
         Mapping('JointWrench', 'JointWrench'),
         Mapping('Light', 'Light'),
+        Mapping('LogPlaybackStatistics', 'LogPlaybackStatistics'),
         Mapping('ParamVec', 'Param'),
         Mapping('ParamVec', 'Param_V'),
         Mapping('SensorNoise', 'SensorNoise'),
         Mapping('StringVec', 'StringMsg_V'),
         Mapping('TrackVisual', 'TrackVisual'),
         Mapping('VideoRecord', 'VideoRecord'),
+        Mapping('WorldStatistics', 'WorldStatistics'),
     ],
     'rosgraph_msgs': [
         Mapping('Clock', 'Clock'),

ros_gz_bridge/src/convert/ros_gz_interfaces.cpp

@@ -796,6 +796,43 @@ convert_gz_to_ros(
   ros_msg.model_only = gz_msg.model_only();
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::WorldStatistics & ros_msg,
+  gz::msgs::WorldStatistics & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, *gz_msg.mutable_header());
+  convert_ros_to_gz(ros_msg.sim_time, *gz_msg.mutable_sim_time());
+  convert_ros_to_gz(ros_msg.pause_time, *gz_msg.mutable_pause_time());
+  convert_ros_to_gz(ros_msg.real_time, *gz_msg.mutable_real_time());
+  gz_msg.set_paused(ros_msg.paused);
+  gz_msg.set_iterations(ros_msg.iterations);
+  gz_msg.set_model_count(ros_msg.model_count);
+  convert_ros_to_gz(ros_msg.log_playback_statistics, *gz_msg.mutable_log_playback_stats());
+  gz_msg.set_real_time_factor(ros_msg.real_time_factor);
+  convert_ros_to_gz(ros_msg.step_size, *gz_msg.mutable_step_size());
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::WorldStatistics & gz_msg,
+  ros_gz_interfaces::msg::WorldStatistics & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  convert_gz_to_ros(gz_msg.sim_time(), ros_msg.sim_time);
+  convert_gz_to_ros(gz_msg.pause_time(), ros_msg.pause_time);
+  convert_gz_to_ros(gz_msg.real_time(), ros_msg.real_time);
+  ros_msg.paused = gz_msg.paused();
+  ros_msg.iterations = gz_msg.iterations();
+  ros_msg.model_count = gz_msg.model_count();
+  convert_gz_to_ros(gz_msg.log_playback_stats(), ros_msg.log_playback_statistics);
+  ros_msg.real_time_factor = gz_msg.real_time_factor();
+  convert_gz_to_ros(gz_msg.step_size(), ros_msg.step_size);
+}
+
+
 template<>
 void
 convert_ros_to_gz(
@@ -819,4 +856,27 @@ convert_gz_to_ros(
     ros_msg.data.push_back(p);
   }
 }
+
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::LogPlaybackStatistics & ros_msg,
+  gz::msgs::LogPlaybackStatistics & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, *gz_msg.mutable_header());
+  convert_ros_to_gz(ros_msg.start_time, *gz_msg.mutable_start_time());
+  convert_ros_to_gz(ros_msg.end_time, *gz_msg.mutable_end_time());
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::LogPlaybackStatistics & gz_msg,
+  ros_gz_interfaces::msg::LogPlaybackStatistics & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  convert_gz_to_ros(gz_msg.start_time(), ros_msg.start_time);
+  convert_gz_to_ros(gz_msg.end_time(), ros_msg.end_time);
+}
+
 }  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -1746,5 +1746,82 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedOriented3DBox_V> &
   }
 }
 
+void createTestMsg(gz::msgs::LogPlaybackStatistics & _msg)
+{
+  createTestMsg(*_msg.mutable_header());
+
+  gz::msgs::Time start_time;
+  createTestMsg(start_time);
+  _msg.mutable_start_time()->CopyFrom(start_time);
+
+  gz::msgs::Time end_time;
+  createTestMsg(end_time);
+  _msg.mutable_end_time()->CopyFrom(end_time);
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::LogPlaybackStatistics> & _msg)
+{
+  gz::msgs::LogPlaybackStatistics expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+  compareTestMsg(std::make_shared<gz::msgs::Time>(_msg->start_time()));
+  compareTestMsg(std::make_shared<gz::msgs::Time>(_msg->end_time()));
+}
+
+void createTestMsg(gz::msgs::WorldStatistics & _msg)
+{
+  createTestMsg(*_msg.mutable_header());
+
+  gz::msgs::Time sim_time;
+  createTestMsg(sim_time);
+  _msg.mutable_sim_time()->CopyFrom(sim_time);
+
+  gz::msgs::Time pause_time;
+  createTestMsg(pause_time);
+  _msg.mutable_pause_time()->CopyFrom(pause_time);
+
+  gz::msgs::Time real_time;
+  createTestMsg(real_time);
+  _msg.mutable_real_time()->CopyFrom(real_time);
+
+  _msg.set_paused(false);
+  _msg.set_iterations(123);
+  _msg.set_model_count(10);
+
+  gz::msgs::LogPlaybackStatistics log_playback_stats_msg;
+  createTestMsg(log_playback_stats_msg);
+  _msg.mutable_log_playback_stats()->CopyFrom(log_playback_stats_msg);
+
+  _msg.set_real_time_factor(0.75);
+
+  gz::msgs::Time step_size;
+  createTestMsg(step_size);
+  _msg.mutable_step_size()->CopyFrom(step_size);
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::WorldStatistics> & _msg)
+{
+  gz::msgs::WorldStatistics expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+  compareTestMsg(std::make_shared<gz::msgs::Time>(_msg->sim_time()));
+  compareTestMsg(std::make_shared<gz::msgs::Time>(_msg->pause_time()));
+  compareTestMsg(std::make_shared<gz::msgs::Time>(_msg->real_time()));
+
+  EXPECT_EQ(expected_msg.paused(), _msg->paused());
+  EXPECT_EQ(expected_msg.iterations(), _msg->iterations());
+  EXPECT_EQ(expected_msg.model_count(), _msg->model_count());
+
+  compareTestMsg(
+    std::make_shared<gz::msgs::LogPlaybackStatistics>(_msg->log_playback_stats()));
+
+  EXPECT_FLOAT_EQ(expected_msg.real_time_factor(), _msg->real_time_factor());
+
+  compareTestMsg(
+    std::make_shared<gz::msgs::Time>(_msg->step_size()));
+}
+
 }  // namespace testing
 }  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/gz_test_msg.hpp

@@ -44,6 +44,7 @@
 #include <gz/msgs/joy.pb.h>
 #include <gz/msgs/laserscan.pb.h>
 #include <gz/msgs/light.pb.h>
+#include <gz/msgs/log_playback_stats.pb.h>
 #include <gz/msgs/magnetometer.pb.h>
 #include <gz/msgs/model.pb.h>
 #include <gz/msgs/navsat.pb.h>
@@ -68,6 +69,7 @@
 #include <gz/msgs/wrench.pb.h>
 #include <gz/msgs/annotated_axis_aligned_2d_box_v.pb.h>
 #include <gz/msgs/annotated_oriented_3d_box_v.pb.h>
+#include <gz/msgs/world_stats.pb.h>
 
 #include <memory>
 
@@ -560,6 +562,22 @@ void createTestMsg(gz::msgs::AnnotatedOriented3DBox_V & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedOriented3DBox_V> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::LogPlaybackStatistics & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gz::msgs::LogPlaybackStatistics> & _msg);
+
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::WorldStatistics & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gz::msgs::WorldStatistics> & _msg);
+
 }  // namespace testing
 }  // namespace ros_gz_bridge
 

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -1636,5 +1636,91 @@ void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection3DArray> &
   }
 }
 
+void createTestMsg(ros_gz_interfaces::msg::LogPlaybackStatistics & _msg)
+{
+  std_msgs::msg::Header header_msg;
+  createTestMsg(header_msg);
+  _msg.header = header_msg;
+
+  builtin_interfaces::msg::Time start_time;
+  createTestMsg(start_time);
+  _msg.start_time = start_time;
+
+  builtin_interfaces::msg::Time end_time;
+  createTestMsg(end_time);
+  _msg.end_time = end_time;
+}
+
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::LogPlaybackStatistics> & _msg)
+{
+  ros_gz_interfaces::msg::LogPlaybackStatistics expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<std_msgs::msg::Header>(_msg->header));
+  compareTestMsg(std::make_shared<builtin_interfaces::msg::Time>(_msg->start_time));
+  compareTestMsg(std::make_shared<builtin_interfaces::msg::Time>(_msg->end_time));
+}
+
+void createTestMsg(ros_gz_interfaces::msg::WorldStatistics & _msg)
+{
+  std_msgs::msg::Header header_msg;
+  createTestMsg(header_msg);
+  _msg.header = header_msg;
+
+  builtin_interfaces::msg::Time sim_time;
+  createTestMsg(sim_time);
+  _msg.sim_time = sim_time;
+
+  builtin_interfaces::msg::Time pause_time;
+  createTestMsg(pause_time);
+  _msg.pause_time = pause_time;
+
+  builtin_interfaces::msg::Time real_time;
+  createTestMsg(real_time);
+  _msg.real_time = real_time;
+
+  _msg.paused = false;
+
+  _msg.iterations = 123;
+
+  _msg.model_count = 10;
+
+  ros_gz_interfaces::msg::LogPlaybackStatistics log_playback_statistics;
+  createTestMsg(log_playback_statistics);
+  _msg.log_playback_statistics = log_playback_statistics;
+
+  _msg.real_time_factor = 0.75;
+
+  builtin_interfaces::msg::Time step_size;
+  createTestMsg(step_size);
+  _msg.step_size = step_size;
+}
+
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::WorldStatistics> & _msg)
+{
+  ros_gz_interfaces::msg::WorldStatistics expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<std_msgs::msg::Header>(_msg->header));
+
+  compareTestMsg(std::make_shared<builtin_interfaces::msg::Time>(_msg->sim_time));
+  compareTestMsg(std::make_shared<builtin_interfaces::msg::Time>(_msg->pause_time));
+  compareTestMsg(std::make_shared<builtin_interfaces::msg::Time>(_msg->real_time));
+
+  EXPECT_EQ(expected_msg.paused, _msg->paused);
+
+  EXPECT_EQ(expected_msg.iterations, _msg->iterations);
+
+  EXPECT_EQ(expected_msg.model_count, _msg->model_count);
+
+  compareTestMsg(
+    std::make_shared<ros_gz_interfaces::msg::LogPlaybackStatistics>(
+      _msg->log_playback_statistics));
+
+  EXPECT_FLOAT_EQ(expected_msg.real_time_factor, _msg->real_time_factor);
+
+  compareTestMsg(std::make_shared<builtin_interfaces::msg::Time>(_msg->step_size));
+}
+
 }  // namespace testing
 }  // namespace ros_gz_bridge