Added new topics to record rosbag2 dataset

physical_ai_interfaces/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package physical_ai_interfaces
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.7.3 (2026-01-19)
+------------------
+* None
+
 0.7.2 (2025-12-01)
 ------------------
 * None

physical_ai_interfaces/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>physical_ai_interfaces</name>
-  <version>0.7.2</version>
+  <version>0.7.3</version>
   <description>
     ROS 2 interfaces for Physical AI tools
   </description>

physical_ai_manager/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package physical_ai_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.7.3 (2026-01-19)
+------------------
+* None
+
 0.7.2 (2025-12-01)
 ------------------
 * None

physical_ai_manager/package.json

@@ -1,6 +1,6 @@
 {
   "name": "physical_ai_manager",
-  "version": "0.7.2",
+  "version": "0.7.3",
   "description": "Web UI for Physical AI Platform",
   "author": "Kiwoong Park <pkw@robotis.com>",
   "contributors": [

physical_ai_server/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package physical_ai_server
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.7.3 (2026-01-19)
+------------------
+* Modified to record /tf, /robot_description, /camera_info topics in rosbag2
+* Contributors: Dongyun Kim
+
 0.7.2 (2025-12-01)
 ------------------
 * Fixed an issue where the task_index was being merged based on the first episode when merging episodes in the *.parquet data.

physical_ai_server/config/ffw_bg2_rev4_config.yaml

@@ -25,6 +25,16 @@ physical_ai_server:
         - leader_head
         - leader_lift
 
+      rosbag_extra_topic_list:
+        - /tf
+        - /tf_static
+        - /robot_description
+        - /zed/zed_node/rgb/camera_info
+        - /zed/zed_node/left/camera_info
+        - /zed/zed_node/right/camera_info
+        - /camera_left/camera_left/color/camera_info
+        - /camera_right/camera_right/color/camera_info
+
       joint_order:
         leader_left:
           - arm_l_joint1

physical_ai_server/config/ffw_sg2_rev1_config.yaml

@@ -28,6 +28,18 @@ physical_ai_server:
         - leader_lift
         - leader_mobile
 
+      rosbag_extra_topic_list:
+        - /tf
+        - /tf_static
+        - /robot_description
+        - /odom
+        - /cmd_vel
+        - /zed/zed_node/rgb/camera_info
+        - /zed/zed_node/left/camera_info
+        - /zed/zed_node/right/camera_info
+        - /camera_left/camera_left/color/camera_info
+        - /camera_right/camera_right/color/camera_info
+
       joint_order:
         leader_left:
           - arm_l_joint1

physical_ai_server/config/omx_f_config.yaml

@@ -17,6 +17,11 @@ physical_ai_server:
       joint_list:
         - leader
 
+      rosbag_extra_topic_list:
+        - /tf
+        - /tf_static
+        - /robot_description
+
       joint_order:
         leader:
           - joint1

physical_ai_server/config/omy_f3m_config.yaml

@@ -15,6 +15,11 @@ physical_ai_server:
       joint_list:
         - leader
 
+      rosbag_extra_topic_list:
+        - /tf
+        - /tf_static
+        - /robot_description
+
       joint_order:
         leader:
           - joint1

physical_ai_server/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>physical_ai_server</name>
-  <version>0.7.2</version>
+  <version>0.7.3</version>
   <description>
     ROS 2 package for Open Platform AI Kit integration
   </description>

physical_ai_server/setup.py

@@ -14,7 +14,7 @@
 
 setup(
     name=package_name,
-    version='0.7.2',
+    version='0.7.3',
     packages=find_packages(),
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),

physical_ai_tools/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package physical_ai_tools
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.7.3 (2026-01-19)
+------------------
+* Modified to record /tf, /robot_description, /camera_info topics in rosbag2
+* Contributors: Dongyun Kim
+
 0.7.2 (2025-12-01)
 ------------------
 * Fixed an issue where the task_index was being merged based on the first episode when merging episodes in the *.parquet data.

physical_ai_tools/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>physical_ai_tools</name>
-  <version>0.7.2</version>
+  <version>0.7.3</version>
   <description>
     physical_ai_tools meta ROS 2 package
   </description>

rosbag_recorder/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package rosbag_recorder
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.7.3 (2026-01-19)
+------------------
+* Modified to record /tf, /robot_description, /camera_info topics in rosbag2
+* Contributors: Dongyun Kim
+
 0.7.2 (2025-12-01)
 ------------------
 * None

rosbag_recorder/include/rosbag_recorder/service_bag_recorder.hpp

@@ -58,12 +58,26 @@ class ServiceBagRecorder : public rclcpp::Node
     const std::map<std::string, std::vector<std::string>> & names_and_types);
   void delete_bag_directory(const std::string & bag_uri);
   void create_subscriptions();
+  rclcpp::QoS get_qos_for_topic(const std::string & topic);
+  void flush_latched_messages();
 
   rclcpp::Service<rosbag_recorder::srv::SendCommand>::SharedPtr send_command_srv_;
 
   std::vector<rclcpp::GenericSubscription::SharedPtr> subscriptions_;
   std::unique_ptr<rosbag2_cpp::Writer> writer_;
   std::unordered_map<std::string, std::string> type_for_topic_;
+
+  // Cache of latched topic names to avoid repeated publisher info lookups
+  std::unordered_set<std::string> latched_topics_;
+
+  // Buffer for latched messages received before recording starts
+  struct BufferedMessage
+  {
+    std::shared_ptr<rclcpp::SerializedMessage> msg;
+    rclcpp::Time timestamp;
+  };
+  std::unordered_map<std::string, BufferedMessage> latched_message_buffer_;
+
   bool is_recording_ {false};
   std::string current_bag_uri_;
   std::vector<std::string> topics_to_record_ {};

rosbag_recorder/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rosbag_recorder</name>
-  <version>0.7.2</version>
+  <version>0.7.3</version>
   <description>
     ROS 2 package to record rosbag dataset
   </description>

rosbag_recorder/src/service_bag_recorder.cpp

@@ -116,6 +116,8 @@ void ServiceBagRecorder::handle_prepare(const std::vector<std::string> & topics)
       type_for_topic_[topic] = type;
     }
 
+    // Create subscriptions early to avoid data loss
+    // Latched messages will be buffered until recording starts
     create_subscriptions();
 
     RCLCPP_INFO(
@@ -180,7 +182,9 @@ void ServiceBagRecorder::handle_start(const std::string & uri)
     throw std::runtime_error(std::string("Failed to start recording: ") + e.what());
   }
 
+  // Set recording flag and flush buffered latched messages
   is_recording_ = true;
+  flush_latched_messages();
 
   RCLCPP_INFO(
     this->get_logger(), "Recording started: uri=%s topics=%zu",
@@ -306,39 +310,109 @@ void ServiceBagRecorder::create_subscriptions()
   RCLCPP_INFO(this->get_logger(), "Creating subscriptions");
 
   subscriptions_.clear();
+  latched_topics_.clear();
 
   // Create generic subscriptions for all topics
   for (const auto & [topic, type] : type_for_topic_) {
     auto options = rclcpp::SubscriptionOptions();
+    auto qos = get_qos_for_topic(topic);
+
+    // Cache whether this topic is latched to avoid repeated publisher lookups
+    if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
+      latched_topics_.insert(topic);
+    }
+
     auto sub = this->create_generic_subscription(
       topic,
       type,
-      rclcpp::QoS(100),
+      qos,
       [this, topic](std::shared_ptr<rclcpp::SerializedMessage> serialized_msg) {
         this->handle_serialized_message(topic, serialized_msg);
       },
       options);
     subscriptions_.push_back(sub);
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Subscribed to topic: %s",
+      topic.c_str());
   }
 }
 
-void ServiceBagRecorder::handle_serialized_message(
-  const std::string & topic,
-  const std::shared_ptr<rclcpp::SerializedMessage> & serialized_msg)
+rclcpp::QoS ServiceBagRecorder::get_qos_for_topic(
+  const std::string & topic)
 {
-  std::scoped_lock<std::mutex> lock(mutex_);
+  // Get publisher info to determine QoS settings
+  auto publishers_info = this->get_publishers_info_by_topic(topic);
+
+  if (!publishers_info.empty()) {
+    // Check if any publisher uses TRANSIENT_LOCAL durability
+    for (const auto & pub_info : publishers_info) {
+      if (pub_info.qos_profile().durability() == rclcpp::DurabilityPolicy::TransientLocal) {
+        RCLCPP_INFO(
+          this->get_logger(),
+          "Topic '%s' uses TRANSIENT_LOCAL durability, using matching QoS",
+          topic.c_str());
+        // Use smaller queue size (10) for latched topics as they typically publish once
+        // and rely on durability rather than queue depth
+        return rclcpp::QoS(rclcpp::KeepLast(10))
+               .reliable()
+               .transient_local();
+      }
+    }
+  }
+
+  // Use default QoS for other topics
+  return rclcpp::QoS(100);
+}
 
-  if (!is_recording_ || !writer_) {
+void ServiceBagRecorder::flush_latched_messages()
+{
+  if (latched_message_buffer_.empty()) {
     return;
   }
 
+  RCLCPP_INFO(
+    this->get_logger(),
+    "Flushing %zu buffered latched messages",
+    latched_message_buffer_.size());
+
+  for (const auto & [topic, buffered] : latched_message_buffer_) {
+    if (writer_) {
+      const auto it = type_for_topic_.find(topic);
+      if (it != type_for_topic_.end()) {
+        writer_->write(buffered.msg, topic, it->second, buffered.timestamp);
+        RCLCPP_DEBUG(this->get_logger(), "Flushed latched message from: %s", topic.c_str());
+      }
+    }
+  }
+
+  latched_message_buffer_.clear();
+}
+
+void ServiceBagRecorder::handle_serialized_message(
+  const std::string & topic,
+  const std::shared_ptr<rclcpp::SerializedMessage> & serialized_msg)
+{
+  std::scoped_lock<std::mutex> lock(mutex_);
 
   const auto it = type_for_topic_.find(topic);
   if (it == type_for_topic_.end()) {
     return;
   }
   const std::string & type = it->second;
-  writer_->write(serialized_msg, topic, type, this->now());
+
+  // If recording, write directly to bag
+  if (is_recording_ && writer_) {
+    writer_->write(serialized_msg, topic, type, this->now());
+    return;
+  }
+
+  // If not recording yet but this is a latched topic, buffer it
+  // Use cached latched topic status to avoid repeated publisher info lookups
+  if (!is_recording_ && latched_topics_.count(topic)) {
+    latched_message_buffer_[topic] = {serialized_msg, this->now()};
+    RCLCPP_DEBUG(this->get_logger(), "Buffered latched message from topic: %s", topic.c_str());
+  }
 }
 
 int main(int argc, char ** argv)