feat: add repeated transient local option

Signed-off-by: Daisuke Nishimatsu <[email protected]>

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

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

rosbag2_transport/include/rosbag2_transport/recorder.hpp

@@ -16,6 +16,7 @@
 #define ROSBAG2_TRANSPORT__RECORDER_HPP_
 
 #include <future>
+#include <map>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -153,6 +154,8 @@ class Recorder : public rclcpp::Node
   std::atomic<bool> stop_discovery_;
   std::future<void> discovery_future_;
   std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericSubscription>> subscriptions_;
+  std::map<std::pair<std::string, std::string>,
+    std::shared_ptr<const rclcpp::SerializedMessage>> transient_local_messages_;
   std::unordered_set<std::string> topics_warned_about_incompatibility_;
   std::string serialization_format_;
   std::unordered_map<std::string, rclcpp::QoS> topic_qos_profile_overrides_;

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<const rclcpp::SerializedMessage> message) {
+    [this, topic_name, topic_type, qos](std::shared_ptr<const rclcpp::SerializedMessage> 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;