diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 4aa0b1b79a..3c9aae665f 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -514,16 +514,34 @@ std::shared_ptr RecorderImpl::create_subscription( const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos) { - auto subscription = node->create_generic_subscription( + std::function, + const rclcpp::MessageInfo &)> recvFun; + + if (record_options_.use_sim_time) { + return node->create_generic_subscription( + topic_name, + topic_type, + qos, + [this, topic_name, topic_type](std::shared_ptr message) { + if (!paused_.load()) { + writer_->write( + message, topic_name, topic_type, node->now()); + } + }); + } + + return node->create_generic_subscription( topic_name, topic_type, qos, - [this, topic_name, topic_type](std::shared_ptr message) { + [this, topic_name, topic_type](std::shared_ptr message, + const rclcpp::MessageInfo & mi) { if (!paused_.load()) { - writer_->write(message, topic_name, topic_type, node->get_clock()->now()); + writer_->write( + message, topic_name, topic_type, + rclcpp::Time(mi.get_rmw_message_info().received_timestamp)); } }); - return subscription; } std::vector RecorderImpl::offered_qos_profiles_for_topic(