diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 26bf94f1a6..70fb60c4de 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -175,6 +175,10 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '--use-sim-time', action='store_true', default=False, help='Use simulation time.' ) + parser.add_argument( + '--repeated-transient-local', action='store_true', default=False, + help='Repeat transient local messages at the start of each new bag file.' + ) self._subparser = parser def main(self, *, args): # noqa: D102 diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index a387ab0998..82c7b1bde3 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -48,6 +48,7 @@ struct RecordOptions bool ignore_leaf_topics = false; bool start_paused = false; bool use_sim_time = false; + bool repeated_transient_local = false; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/recorder.hpp b/rosbag2_transport/include/rosbag2_transport/recorder.hpp index 07783c2f88..181210dbbb 100644 --- a/rosbag2_transport/include/rosbag2_transport/recorder.hpp +++ b/rosbag2_transport/include/rosbag2_transport/recorder.hpp @@ -153,6 +153,8 @@ class Recorder : public rclcpp::Node std::atomic stop_discovery_; std::future discovery_future_; std::unordered_map> subscriptions_; + std::map, + std::shared_ptr> transient_local_messages_; std::unordered_set topics_warned_about_incompatibility_; std::string serialization_format_; std::unordered_map topic_qos_profile_overrides_; diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 1fd029fd6a..76bb10fa54 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -191,6 +191,13 @@ void Recorder::event_publisher_thread_main() message.closed_file = bag_split_info_.closed_file; message.opened_file = bag_split_info_.opened_file; split_event_pub_->publish(message); + if (writer_ && record_options_.repeated_transient_local) { + for (const auto & message : transient_local_messages_) { + writer_->write( + message.second, message.first.first, message.first.second, + this->get_clock()->now()); + } + } } should_exit = event_publisher_thread_should_exit_; @@ -318,9 +325,14 @@ Recorder::create_subscription( topic_name, topic_type, qos, - [this, topic_name, topic_type](std::shared_ptr message) { + [this, topic_name, topic_type, qos](std::shared_ptr message) { if (!paused_.load()) { writer_->write(message, topic_name, topic_type, this->get_clock()->now()); + if (record_options_.repeated_transient_local && + qos.get_rmw_qos_profile().durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + { + transient_local_messages_.insert_or_assign({topic_name, topic_type}, message); + } } }); return subscription;