Stamp all outgoing headers with the wall time if parameter override_t…

…imestamps_with_wall_time is set to true (#562)

Signed-off-by: Rein Appeldoorn <[email protected]>
(cherry picked from commit 6daea2c)

# Conflicts:
#	ros_gz_bridge/src/factory.hpp

ros_gz_bridge/README.md

@@ -90,6 +90,9 @@ You should create an unidirectional `/clock` bridge:
 ros2 run ros_gz_bridge parameter_bridge /clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock
 ```
 
+An alternative set-up can be using the bridge with the `override_timestamps_with_wall_time` ros parameter set to `true` (default=`false`). In this set-up,
+all header timestamps of the outgoing messages will be stamped with the wall time. This can be useful when the simulator has to communicate with an external system that requires wall times.
+
 ## Example 1a: Gazebo Transport talker and ROS 2 listener
 
 Start the parameter bridge which will watch the specified topics.

ros_gz_bridge/src/bridge_handle.cpp

@@ -30,6 +30,8 @@ BridgeHandle::BridgeHandle(
   config_(config),
   factory_(get_factory(config.ros_type_name, config.gz_type_name))
 {
+  ros_node_->get_parameter("override_timestamps_with_wall_time",
+    override_timestamps_with_wall_time_);
 }
 
 BridgeHandle::~BridgeHandle() = default;

ros_gz_bridge/src/bridge_handle.hpp

@@ -101,6 +101,9 @@ class BridgeHandle
 
   /// \brief Typed factory used to create publishers/subscribers
   std::shared_ptr<FactoryInterface> factory_;
+
+  /// \brief Override the header.stamp field of the outgoing messages with the wall time
+  bool override_timestamps_with_wall_time_ = false;
 };
 
 }  // namespace ros_gz_bridge

ros_gz_bridge/src/bridge_handle_gz_to_ros.cpp

@@ -70,7 +70,8 @@ void BridgeHandleGzToRos::StartSubscriber()
     this->gz_node_,
     this->config_.gz_topic_name,
     this->config_.subscriber_queue_size,
-    this->ros_publisher_);
+    this->ros_publisher_,
+    override_timestamps_with_wall_time_);
 
   this->gz_subscriber_ = this->gz_node_;
 }

ros_gz_bridge/src/factory.hpp

@@ -15,9 +15,11 @@
 #ifndef FACTORY_HPP_
 #define FACTORY_HPP_
 
+#include <chrono>
 #include <functional>
 #include <memory>
 #include <string>
+#include <type_traits>
 
 #include <gz/transport/Node.hh>
 
@@ -27,6 +29,16 @@
 
 #include "factory_interface.hpp"
 
+template<class T, class = void>
+struct has_header : std::false_type
+{
+};
+
+template<class T>
+struct has_header<T, std::void_t<decltype(T::header)>>: std::true_type
+{
+};
+
 namespace ros_gz_bridge
 {
 
@@ -103,8 +115,10 @@ class Factory : public FactoryInterface
     std::shared_ptr<gz::transport::Node> node,
     const std::string & topic_name,
     size_t /*queue_size*/,
-    rclcpp::PublisherBase::SharedPtr ros_pub)
+    rclcpp::PublisherBase::SharedPtr ros_pub,
+    bool override_timestamps_with_wall_time)
   {
+<<<<<<< HEAD
     std::function<void(const GZ_T &,
       const gz::transport::MessageInfo &)> subCb =
       [this, ros_pub](const GZ_T & _msg, const gz::transport::MessageInfo & _info)
@@ -113,6 +127,12 @@ class Factory : public FactoryInterface
         if (!_info.IntraProcess()) {
           this->gz_callback(_msg, ros_pub);
         }
+=======
+    std::function<void(const GZ_T &)> subCb =
+      [this, ros_pub, override_timestamps_with_wall_time](const GZ_T & _msg)
+      {
+        this->gz_callback(_msg, ros_pub, override_timestamps_with_wall_time);
+>>>>>>> 6daea2c (Stamp all outgoing headers with the wall time if parameter override_timestamps_with_wall_time is set to true (#562))
       };
 
     node->Subscribe(topic_name, subCb);
@@ -139,10 +159,20 @@ class Factory : public FactoryInterface
   static
   void gz_callback(
     const GZ_T & gz_msg,
-    rclcpp::PublisherBase::SharedPtr ros_pub)
+    rclcpp::PublisherBase::SharedPtr ros_pub,
+    bool override_timestamps_with_wall_time)
   {
     ROS_T ros_msg;
     convert_gz_to_ros(gz_msg, ros_msg);
+    if constexpr (has_header<ROS_T>::value) {
+      if (override_timestamps_with_wall_time) {
+        auto now = std::chrono::system_clock::now().time_since_epoch();
+        auto ns =
+          std::chrono::duration_cast<std::chrono::nanoseconds>(now).count();
+        ros_msg.header.stamp.sec = ns / 1e9;
+        ros_msg.header.stamp.nanosec = ns - ros_msg.header.stamp.sec * 1e9;
+      }
+    }
     std::shared_ptr<rclcpp::Publisher<ROS_T>> pub =
       std::dynamic_pointer_cast<rclcpp::Publisher<ROS_T>>(ros_pub);
     if (pub != nullptr) {

ros_gz_bridge/src/factory_interface.hpp

@@ -60,7 +60,8 @@ class FactoryInterface
     std::shared_ptr<gz::transport::Node> node,
     const std::string & topic_name,
     size_t queue_size,
-    rclcpp::PublisherBase::SharedPtr ros_pub) = 0;
+    rclcpp::PublisherBase::SharedPtr ros_pub,
+    bool override_timestamps_with_wall_time) = 0;
 };
 
 }  // namespace ros_gz_bridge

ros_gz_bridge/src/ros_gz_bridge.cpp

@@ -33,6 +33,7 @@ RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options)
   this->declare_parameter<int>("subscription_heartbeat", 1000);
   this->declare_parameter<std::string>("config_file", "");
   this->declare_parameter<bool>("expand_gz_topic_names", false);
+  this->declare_parameter<bool>("override_timestamps_with_wall_time", false);
 
   int heartbeat;
   this->get_parameter("subscription_heartbeat", heartbeat);