feat: override_timestamps_with_wall_time parameter

Stamp all outgoing headers with the wall time if parameter `override_timestamps_with_wall_time` is set to true. Signed-off-by: Rein Appeldoorn <[email protected]>
gazebosim · Jun 19, 2024 · a656c0f · a656c0f
1 parent 4c64bbe
commit a656c0f
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 5 deletions.
diff --git a/ros_gz_bridge/src/bridge_handle_gz_to_ros.cpp b/ros_gz_bridge/src/bridge_handle_gz_to_ros.cpp
@@ -70,7 +70,9 @@ void BridgeHandleGzToRos::StartSubscriber()
     this->gz_node_,
     this->config_.gz_topic_name,
     this->config_.subscriber_queue_size,
-    this->ros_publisher_);
+    this->ros_publisher_,
+    this->ros_node_->declare_parameter<bool>(
+      "override_timestamps_with_wall_time", false));
 
   this->gz_subscriber_ = this->gz_node_;
 }

diff --git a/ros_gz_bridge/src/factory.hpp b/ros_gz_bridge/src/factory.hpp
@@ -28,6 +28,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
 {
 
@@ -104,12 +114,13 @@ 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)
   {
     std::function<void(const GZ_T &)> subCb =
       [this, ros_pub](const GZ_T & _msg)
       {
-        this->gz_callback(_msg, ros_pub);
+        this->gz_callback(_msg, ros_pub, override_timestamps_with_wall_time);
       };
 
     // Ignore messages that are published from this bridge.
@@ -139,10 +150,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) {

diff --git a/ros_gz_bridge/src/factory_interface.hpp b/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