diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 57289234..2a53c08c 100644 --- a/ros_gz_bridge/README.md +++ b/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. diff --git a/ros_gz_bridge/src/bridge_handle.cpp b/ros_gz_bridge/src/bridge_handle.cpp index c7851503..eb8b1f90 100644 --- a/ros_gz_bridge/src/bridge_handle.cpp +++ b/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; diff --git a/ros_gz_bridge/src/bridge_handle.hpp b/ros_gz_bridge/src/bridge_handle.hpp index d1751fb9..777496ac 100644 --- a/ros_gz_bridge/src/bridge_handle.hpp +++ b/ros_gz_bridge/src/bridge_handle.hpp @@ -101,6 +101,9 @@ class BridgeHandle /// \brief Typed factory used to create publishers/subscribers std::shared_ptr 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 diff --git a/ros_gz_bridge/src/bridge_handle_gz_to_ros.cpp b/ros_gz_bridge/src/bridge_handle_gz_to_ros.cpp index ddc292f8..ce90c875 100644 --- 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,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_; } diff --git a/ros_gz_bridge/src/factory.hpp b/ros_gz_bridge/src/factory.hpp index ec00af7b..8261fbfb 100644 --- a/ros_gz_bridge/src/factory.hpp +++ b/ros_gz_bridge/src/factory.hpp @@ -15,9 +15,11 @@ #ifndef FACTORY_HPP_ #define FACTORY_HPP_ +#include #include #include #include +#include #include @@ -27,6 +29,16 @@ #include "factory_interface.hpp" +template +struct has_header : std::false_type +{ +}; + +template +struct has_header>: std::true_type +{ +}; + namespace ros_gz_bridge { @@ -103,8 +115,10 @@ class Factory : public FactoryInterface std::shared_ptr 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 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 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::value) { + if (override_timestamps_with_wall_time) { + auto now = std::chrono::system_clock::now().time_since_epoch(); + auto ns = + std::chrono::duration_cast(now).count(); + ros_msg.header.stamp.sec = ns / 1e9; + ros_msg.header.stamp.nanosec = ns - ros_msg.header.stamp.sec * 1e9; + } + } std::shared_ptr> pub = std::dynamic_pointer_cast>(ros_pub); if (pub != nullptr) { diff --git a/ros_gz_bridge/src/factory_interface.hpp b/ros_gz_bridge/src/factory_interface.hpp index b760bcd8..3ea64ee8 100644 --- 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 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 diff --git a/ros_gz_bridge/src/ros_gz_bridge.cpp b/ros_gz_bridge/src/ros_gz_bridge.cpp index 46a86d98..c3752d95 100644 --- a/ros_gz_bridge/src/ros_gz_bridge.cpp +++ b/ros_gz_bridge/src/ros_gz_bridge.cpp @@ -33,6 +33,7 @@ RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options) this->declare_parameter("subscription_heartbeat", 1000); this->declare_parameter("config_file", ""); this->declare_parameter("expand_gz_topic_names", false); + this->declare_parameter("override_timestamps_with_wall_time", false); int heartbeat; this->get_parameter("subscription_heartbeat", heartbeat);