From f20043832675a9fd302a48ecd81a77cadd1176d5 Mon Sep 17 00:00:00 2001
From: Ussama Naal <606033+Samahu@users.noreply.github.com>
Date: Wed, 30 Oct 2024 13:46:47 -0700
Subject: [PATCH] ROS-389 [rolling/humble/iron/jazzy]:
 replay-improvments-and-fixes (#393)

Remap metadata topic + Support loop capability in pcap replay + Add play_delay & play_rate for replay
Added a launch file parameter pub_static_tf to disable sensor transforms broadcast
---------
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
---
 CHANGELOG.rst                                 |  4 +
 ouster-ros/config/driver_params.yaml          |  6 +-
 .../config/os_sensor_cloud_image_params.yaml  |  1 +
 .../include/ouster_ros/os_sensor_node_base.h  | 21 ++++-
 ouster-ros/launch/record.composite.launch.xml |  5 ++
 ouster-ros/launch/replay.composite.launch.xml | 14 +++-
 ouster-ros/launch/replay_pcap.launch.xml      | 26 +++---
 ouster-ros/launch/sensor.composite.launch.xml |  5 ++
 .../launch/sensor.independent.launch.xml      |  5 ++
 ouster-ros/launch/sensor_mtp.launch.xml       |  5 ++
 ouster-ros/package.xml                        |  2 +-
 ouster-ros/src/os_cloud_node.cpp              |  4 +-
 ouster-ros/src/os_driver_node.cpp             |  4 +-
 ouster-ros/src/os_pcap_node.cpp               | 40 ++++++---
 ouster-ros/src/os_sensor_node.cpp             | 74 +----------------
 ouster-ros/src/os_sensor_node.h               | 13 ---
 ouster-ros/src/os_sensor_node_base.cpp        | 81 ++++++++++++++++++-
 .../src/os_static_transforms_broadcaster.h    |  4 +
 18 files changed, 197 insertions(+), 117 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 135d4bed..143dd7f9 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -5,6 +5,10 @@ Changelog
 [unreleased]
 ============
 * [BUGFIX]: correctly align timestamps to the generated point cloud.
+* Added support to enable **loop** for pcap replay + other replay config.
+* Added a new launch file parameter ``pub_static_tf`` that allows users to turn off the braodcast
+  of sensor TF transforms.
+
 
 ouster_ros v0.13.2
 ==================
diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml
index 8f6f5b40..05a0c217 100644
--- a/ouster-ros/config/driver_params.yaml
+++ b/ouster-ros/config/driver_params.yaml
@@ -56,7 +56,11 @@ ouster/os_driver:
     # point_cloud_frame[optional]: which frame of reference to use when
     # generating PointCloud2 or LaserScan messages, select between the values of
     # lidar_frame and sensor_frame.
-    point_cloud_frame: os_lidar 
+    point_cloud_frame: os_lidar
+    # pub_static_tf[optional]: when this flag is set to True, the driver will
+    # broadcast the TF transforms for the imu/sensor/lidar frames. Prevent the
+    # driver from broadcasting TF transforms by setting this parameter to False.
+    pub_static_tf: true
     # proc_mask[optional]: use any combination of the 4 flags IMG, PCL, IMU and
     # SCAN to enable or disable their respective messages.
     proc_mask: IMU|PCL|SCAN|IMG|RAW
diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml
index 52296c1d..593053f8 100644
--- a/ouster-ros/config/os_sensor_cloud_image_params.yaml
+++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml
@@ -27,6 +27,7 @@ ouster/os_cloud:
     lidar_frame: os_lidar
     imu_frame: os_imu
     point_cloud_frame: os_lidar
+    pub_static_tf: true
     timestamp_mode: ''  # this value needs to match os_sensor/timestamp_mode
     ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588
     proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options
diff --git a/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/ouster-ros/include/ouster_ros/os_sensor_node_base.h
index cb303a48..2bafe696 100644
--- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h
+++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h
@@ -7,21 +7,23 @@
  *
  */
 
-#include <ouster/types.h>
+#include <chrono>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <lifecycle_msgs/srv/change_state.hpp>
 #include <std_msgs/msg/string.hpp>
 
 #include "ouster_sensor_msgs/srv/get_metadata.hpp"
 
+#include <ouster/types.h>
+
 namespace ouster_ros {
 
 class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
    protected:
     explicit OusterSensorNodeBase(const std::string& name,
-                                  const rclcpp::NodeOptions& options)
-        : rclcpp_lifecycle::LifecycleNode(name, options) {}
+                                  const rclcpp::NodeOptions& options);
 
    protected:
     bool is_arg_set(const std::string& arg) const {
@@ -41,7 +43,20 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
     static bool write_text_to_file(const std::string& file_path,
                                    const std::string& text);
 
+    static std::string transition_id_to_string(uint8_t transition_id);
+
+    template <typename CallbackT, typename... CallbackT_Args>
+    bool change_state(std::uint8_t transition_id, CallbackT callback,
+                      CallbackT_Args... callback_args,
+                      std::chrono::seconds time_out = std::chrono::seconds{3});
+
+    void execute_transitions_sequence(std::vector<uint8_t> transitions_sequence,
+                                      size_t at);
+
+
    protected:
+    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> change_state_client;
+
     ouster::sensor::sensor_info info;
     rclcpp::Service<ouster_sensor_msgs::srv::GetMetadata>::SharedPtr get_metadata_srv;
     std::string cached_metadata;
diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml
index 0908ada5..d89d3c1f 100644
--- a/ouster-ros/launch/record.composite.launch.xml
+++ b/ouster-ros/launch/record.composite.launch.xml
@@ -55,6 +55,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <arg name="use_system_default_qos" default="true"
     description="Use the default system QoS settings"/>
@@ -125,6 +129,7 @@
         <param name="lidar_frame" value="$(var lidar_frame)"/>
         <param name="imu_frame" value="$(var imu_frame)"/>
         <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+        <param name="pub_static_tf" value="$(var pub_static_tf)"/>
         <param name="timestamp_mode" value="$(var timestamp_mode)"/>
         <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml
index a78da49a..2d0a99a5 100644
--- a/ouster-ros/launch/replay.composite.launch.xml
+++ b/ouster-ros/launch/replay.composite.launch.xml
@@ -3,6 +3,9 @@
   <set_parameter name="use_sim_time" value="true" />
 
   <arg name="loop" default="false" description="request loop playback"/>
+  <arg name="play_delay" default="0" description="playback start delay in seconds"/>
+  <arg name="play_rate" default="1.0"/>
+
   <let if="$(var loop)" name="_loop" value="--loop"/>
   <let unless="$(var loop)" name="_loop" value=" "/>
 
@@ -37,6 +40,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <let name="_use_metadata_file" value="$(eval '\'$(var metadata)\' != \'\'')"/>
 
@@ -77,12 +84,14 @@
     </node>
     <node_container pkg="rclcpp_components" exec="component_container_mt" name="os_container" output="screen" namespace="">
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterCloud" name="os_cloud">
+        <remap from="/os_node/metadata" to="/ouster/metadata"/>
         <remap from="/os_node/imu_packets" to="/ouster/imu_packets"/>
         <remap from="/os_node/lidar_packets" to="/ouster/lidar_packets"/>
         <param name="sensor_frame" value="$(var sensor_frame)"/>
         <param name="lidar_frame" value="$(var lidar_frame)"/>
         <param name="imu_frame" value="$(var imu_frame)"/>
         <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+        <param name="pub_static_tf" value="$(var pub_static_tf)"/>
         <param name="timestamp_mode" value="$(var timestamp_mode)"/>
         <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
@@ -95,6 +104,7 @@
         <param name="max_range" value="$(var max_range)"/>
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
+        <remap from="/os_node/metadata" to="/ouster/metadata"/>
         <remap from="/os_node/lidar_packets" to="/ouster/lidar_packets"/>
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
         <param name="proc_mask" value="$(var proc_mask)"/>
@@ -119,9 +129,9 @@
   <let name="_use_bag_file_name" value="$(eval '\'$(var bag_file)\' != \'b\'')"/>
 
   <executable if="$(var _use_bag_file_name)" output="screen"
-    launch-prefix="bash -c 'sleep 3; $0 $@'"
+    launch-prefix="bash -c 'sleep $(var play_delay); $0 $@'"
     cmd="ros2 bag play $(var bag_file) --clock $(var _loop)
-      --qos-profile-overrides-path
+      --rate $(var play_rate) --qos-profile-overrides-path
       $(find-pkg-share ouster_ros)/config/metadata-qos-override.yaml"/>
 
 </launch>
diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml
index bb758771..9c5a7f3b 100644
--- a/ouster-ros/launch/replay_pcap.launch.xml
+++ b/ouster-ros/launch/replay_pcap.launch.xml
@@ -1,8 +1,13 @@
 <launch>
 
-  <!-- NOTE: pcap replay node does not implement clock-->
+  <!-- NOTE: pcap replay node does not implement clock -->
   <set_parameter name="use_sim_time" value="false" />
 
+  <arg name="loop" default="false" description="request loop playback"/>
+  <arg name="play_delay" default="0" description="playback start delay in seconds"/>
+  <arg name="progress_update_freq" default="1.0"
+    description="playback preogress update frequency per second"/>
+
   <arg name="ouster_ns" default="ouster"
     description="Override the default namespace of all ouster nodes"/>
   <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC"
@@ -31,6 +36,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <let name="_use_metadata_file" value="$(eval '\'$(var metadata)\' != \'\'')"/>
 
@@ -65,9 +74,12 @@
 
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
-    <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_pcap" name="os_pcap" output="screen">
+    <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_pcap" name="os_pcap"
+        launch-prefix="bash -c 'sleep $(var play_delay); $0 $@'" output="screen">
       <param name="metadata" value="$(var metadata)"/>
       <param name="pcap_file" value="$(var pcap_file)"/>
+      <param name="loop" value="$(var loop)"/>
+      <param name="progress_update_freq" value="$(var progress_update_freq)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
     </node>
     <node_container pkg="rclcpp_components" exec="component_container_mt" name="os_container" output="screen" namespace="">
@@ -76,6 +88,7 @@
         <param name="lidar_frame" value="$(var lidar_frame)"/>
         <param name="imu_frame" value="$(var imu_frame)"/>
         <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+        <param name="pub_static_tf" value="$(var pub_static_tf)"/>
         <param name="timestamp_mode" value="$(var timestamp_mode)"/>
         <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
@@ -94,15 +107,6 @@
     </node_container>
   </group>
 
-  <!-- HACK: configure and activate the replay node via a process execute since state
-    transition is currently not availabe through launch.xml format -->
-  <executable if="$(var _use_metadata_file)"
-    cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_pcap configure"
-    launch-prefix="bash -c 'sleep 0; $0 $@'" output="screen"/>
-  <executable if="$(var _use_metadata_file)"
-    cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_pcap activate"
-    launch-prefix="bash -c 'sleep 1; $0 $@'" output="screen"/>
-
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>
diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml
index 8d1c40f6..7ffae269 100644
--- a/ouster-ros/launch/sensor.composite.launch.xml
+++ b/ouster-ros/launch/sensor.composite.launch.xml
@@ -53,6 +53,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <arg name="use_system_default_qos" default="false"
     description="Use the default system QoS settings"/>
@@ -119,6 +123,7 @@
       <param name="lidar_frame" value="$(var lidar_frame)"/>
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+      <param name="pub_static_tf" value="$(var pub_static_tf)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
       <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml
index 21cbeac1..41625bdb 100644
--- a/ouster-ros/launch/sensor.independent.launch.xml
+++ b/ouster-ros/launch/sensor.independent.launch.xml
@@ -53,6 +53,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <arg name="use_system_default_qos" default="false"
     description="Use the default system QoS settings"/>
@@ -122,6 +126,7 @@
       <param name="lidar_frame" value="$(var lidar_frame)"/>
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+      <param name="pub_static_tf" value="$(var pub_static_tf)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
       <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml
index ab721f82..387aec86 100644
--- a/ouster-ros/launch/sensor_mtp.launch.xml
+++ b/ouster-ros/launch/sensor_mtp.launch.xml
@@ -61,6 +61,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <arg name="use_system_default_qos" default="false"
     description="Use the default system QoS settings"/>
@@ -117,6 +121,7 @@
       <param name="lidar_frame" value="$(var lidar_frame)"/>
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+      <param name="pub_static_tf" value="$(var pub_static_tf)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
       <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
index a2a1bd15..825707e9 100644
--- a/ouster-ros/package.xml
+++ b/ouster-ros/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.13.3</version>
+  <version>0.13.4</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>
diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp
index b28ddc72..1732e781 100644
--- a/ouster-ros/src/os_cloud_node.cpp
+++ b/ouster-ros/src/os_cloud_node.cpp
@@ -72,7 +72,9 @@ class OusterCloud : public OusterProcessingNodeBase {
         RCLCPP_INFO(get_logger(),
                     "OusterCloud: retrieved new sensor metadata!");
         info = sensor::parse_metadata(metadata_msg->data);
-        tf_bcast.broadcast_transforms(info);
+        if (tf_bcast.publish_static_tf()) {
+            tf_bcast.broadcast_transforms(info);
+        }
         create_publishers_subscriptions(info);
     }
 
diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp
index 0a198804..8ad3b99a 100644
--- a/ouster-ros/src/os_driver_node.cpp
+++ b/ouster-ros/src/os_driver_node.cpp
@@ -53,7 +53,9 @@ class OusterDriver : public OusterSensor {
 
     virtual void on_metadata_updated(const sensor::sensor_info& info) override {
         OusterSensor::on_metadata_updated(info);
-        tf_bcast.broadcast_transforms(info);
+        if (tf_bcast.publish_static_tf()) {
+          tf_bcast.broadcast_transforms(info);
+        }
     }
 
     virtual void create_publishers() override {
diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp
index 0b458c1c..2c4adb84 100644
--- a/ouster-ros/src/os_pcap_node.cpp
+++ b/ouster-ros/src/os_pcap_node.cpp
@@ -18,16 +18,18 @@
 #include <chrono>
 #include <iomanip>
 
+#include <lifecycle_msgs/msg/transition.hpp>
+#include <lifecycle_msgs/srv/change_state.hpp>
 #include "ouster_sensor_msgs/msg/packet_msg.h"
 #include "ouster_ros/os_sensor_node_base.h"
 #include "ouster_ros/visibility_control.h"
 
 #include <ouster/os_pcap.h>
 
+using namespace std::chrono;
+using namespace std::chrono_literals;
 namespace sensor = ouster::sensor;
 using ouster::sensor_utils::PcapReader;
-using namespace std::chrono;
-
 using ouster_sensor_msgs::msg::PacketMsg;
 
 namespace ouster_ros {
@@ -36,8 +38,19 @@ class OusterPcap : public OusterSensorNodeBase {
    public:
     OUSTER_ROS_PUBLIC
     explicit OusterPcap(const rclcpp::NodeOptions& options)
-        : OusterSensorNodeBase("os_pcap", options) {
+        : OusterSensorNodeBase("os_pcap", options)
+    {
         declare_parameters();
+        bool auto_start = get_parameter("auto_start").as_bool();
+
+        if (auto_start) {
+            RCLCPP_INFO(get_logger(), "auto start requested");
+            auto request_transitions = std::vector<uint8_t>{
+                lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE,
+                lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE};
+            execute_transitions_sequence(request_transitions, 0);
+            RCLCPP_INFO(get_logger(), "auto start initiated");
+        }
     }
 
     ~OusterPcap() override {
@@ -52,6 +65,10 @@ class OusterPcap : public OusterSensorNodeBase {
         try {
             auto meta_file = get_meta_file();
             auto pcap_file = get_pcap_file();
+            loop = get_parameter("loop").as_bool();
+            progress_update_freq = get_parameter("progress_update_freq").as_double();
+            if (progress_update_freq < 0.001)
+                progress_update_freq = 0.001;
             create_metadata_pub();
             load_metadata_from_file(meta_file);
             open_pcap(pcap_file);
@@ -131,8 +148,11 @@ class OusterPcap : public OusterSensorNodeBase {
     }
 
     void declare_parameters() {
+        declare_parameter("auto_start", true);
         declare_parameter<std::string>("metadata");
         declare_parameter<std::string>("pcap_file");
+        declare_parameter("loop", false);
+        declare_parameter("progress_update_freq", 1.0);
         declare_parameter("use_system_default_qos", false);
     }
 
@@ -195,11 +215,13 @@ class OusterPcap : public OusterSensorNodeBase {
         packet_read_active = true;
         packet_read_thread = std::make_unique<std::thread>([this]() {
             auto& pf = sensor::get_format(info);
-            while (packet_read_active) {
+            do {
                 read_packets(*pcap, pf);
-            }
+                pcap->reset();
+            } while(rclcpp::ok() && packet_read_active && loop);
             RCLCPP_DEBUG(get_logger(),
                          "packet_read_thread done.");
+            rclcpp::shutdown();
         });
     }
 
@@ -236,10 +258,9 @@ class OusterPcap : public OusterSensorNodeBase {
         auto packet_info = pcap.current_info();
         auto file_start = packet_info.timestamp;
         auto last_update = file_start;
-        using namespace std::chrono_literals;
-        const auto UPDATE_PERIOD = duration_cast<microseconds>(1s);
+        const auto UPDATE_PERIOD = duration_cast<microseconds>(1s / progress_update_freq);
 
-        while (rclcpp::ok() && payload_size) {
+        while (rclcpp::ok() && packet_read_active && payload_size) {
             auto start = high_resolution_clock::now();
             if (packet_info.dst_port == info.config.udp_port_imu) {
                 std::memcpy(imu_packet.buf.data(), pcap.current_data(),
@@ -278,7 +299,8 @@ class OusterPcap : public OusterSensorNodeBase {
     ouster_sensor_msgs::msg::PacketMsg imu_packet;
     rclcpp::Publisher<ouster_sensor_msgs::msg::PacketMsg>::SharedPtr lidar_packet_pub;
     rclcpp::Publisher<ouster_sensor_msgs::msg::PacketMsg>::SharedPtr imu_packet_pub;
-
+    bool loop;
+    double progress_update_freq;
     std::atomic<bool> packet_read_active = {false};
     std::unique_ptr<std::thread> packet_read_thread;
 };
diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp
index 9426aaff..ed7e86f0 100644
--- a/ouster-ros/src/os_sensor_node.cpp
+++ b/ouster-ros/src/os_sensor_node.cpp
@@ -22,7 +22,6 @@ using ouster_sensor_msgs::srv::SetConfig;
 
 using std::to_string;
 using namespace std::chrono_literals;
-using namespace std::string_literals;
 
 using sensor::LidarPacket;
 using sensor::ImuPacket;
@@ -31,9 +30,7 @@ namespace ouster_ros {
 
 OusterSensor::OusterSensor(const std::string& name,
                            const rclcpp::NodeOptions& options)
-    : OusterSensorNodeBase(name, options),
-      change_state_client{
-          create_client<ChangeState>(get_name() + "/change_state"s)} {
+    : OusterSensorNodeBase(name, options) {
     declare_parameters();
     staged_config = parse_config_from_ros_parameters();
     attempt_reconnect = get_parameter("attempt_reconnect").as_bool();
@@ -292,75 +289,6 @@ void OusterSensor::save_metadata() {
     }
 }
 
-std::string OusterSensor::transition_id_to_string(uint8_t transition_id) {
-    switch (transition_id) {
-        case lifecycle_msgs::msg::Transition::TRANSITION_CREATE:
-            return "create"s;
-        case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE:
-            return "configure"s;
-        case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP:
-            return "cleanup"s;
-        case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE:
-            return "activate";
-        case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE:
-            return "deactivate"s;
-        case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY:
-            return "destroy"s;
-        default:
-            return "unknown"s;
-    }
-}
-
-template <typename CallbackT, typename... CallbackT_Args>
-bool OusterSensor::change_state(std::uint8_t transition_id, CallbackT callback,
-                                CallbackT_Args... callback_args,
-                                std::chrono::seconds time_out) {
-    if (!change_state_client->wait_for_service(time_out)) {
-        RCLCPP_ERROR_STREAM(
-            get_logger(), "Service " << change_state_client->get_service_name()
-                                     << "is not available.");
-        return false;
-    }
-
-    auto request = std::make_shared<ChangeState::Request>();
-    request->transition.id = transition_id;
-    // send an async request to perform the transition
-    change_state_client->async_send_request(
-        request, [this, callback,
-                  callback_args...](rclcpp::Client<ChangeState>::SharedFuture ff) {
-            callback(callback_args..., ff.get()->success);
-        });
-    return true;
-}
-
-void OusterSensor::execute_transitions_sequence(
-    std::vector<uint8_t> transitions_sequence, size_t at) {
-    assert(at < transitions_sequence.size() &&
-           "at index exceeds the number of transitions");
-    auto transition_id = transitions_sequence[at];
-    RCLCPP_DEBUG_STREAM(
-        get_logger(), "transition: [" << transition_id_to_string(transition_id)
-                                      << "] started");
-    change_state(transition_id, [this, transitions_sequence, at](bool success) {
-        if (success) {
-            RCLCPP_DEBUG_STREAM(
-                get_logger(),
-                "transition: [" << transition_id_to_string(transitions_sequence[at])
-                                << "] completed");
-            if (at < transitions_sequence.size() - 1) {
-                execute_transitions_sequence(transitions_sequence, at + 1);
-            } else {
-                RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed");
-            }
-        } else {
-            RCLCPP_DEBUG_STREAM(
-                get_logger(),
-                "transition: [" << transition_id_to_string(transitions_sequence[at])
-                                << "] failed");
-        }
-    });
-}
-
 // param init_id_reset is overriden to true when force_reinit is true
 void OusterSensor::reset_sensor(bool force_reinit, bool init_id_reset) {
     if (!sensor_connection_active) {
diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h
index fc2049cf..9db67b89 100644
--- a/ouster-ros/src/os_sensor_node.h
+++ b/ouster-ros/src/os_sensor_node.h
@@ -18,8 +18,6 @@
 #include <vector>
 
 #include <std_srvs/srv/empty.hpp>
-#include <lifecycle_msgs/msg/transition.hpp>
-#include <lifecycle_msgs/srv/change_state.hpp>
 #include "ouster_sensor_msgs/msg/packet_msg.hpp"
 #include "ouster_sensor_msgs/srv/get_config.hpp"
 #include "ouster_sensor_msgs/srv/set_config.hpp"
@@ -28,7 +26,6 @@
 
 
 namespace sensor = ouster::sensor;
-using lifecycle_msgs::srv::ChangeState;
 using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
 
 namespace ouster_ros {
@@ -77,15 +74,6 @@ class OusterSensor : public OusterSensorNodeBase {
 
     void save_metadata();
 
-    static std::string transition_id_to_string(uint8_t transition_id);
-    template <typename CallbackT, typename... CallbackT_Args>
-    bool change_state(std::uint8_t transition_id, CallbackT callback,
-                      CallbackT_Args... callback_args,
-                      std::chrono::seconds time_out = std::chrono::seconds{3});
-
-    void execute_transitions_sequence(std::vector<uint8_t> transitions_sequence,
-                                      size_t at);
-
     // param init_id_reset is overriden to true when force_reinit is true
     void reset_sensor(bool force_reinit, bool init_id_reset = false);
 
@@ -152,7 +140,6 @@ class OusterSensor : public OusterSensorNodeBase {
     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_srv;
     rclcpp::Service<ouster_sensor_msgs::srv::GetConfig>::SharedPtr get_config_srv;
     rclcpp::Service<ouster_sensor_msgs::srv::SetConfig>::SharedPtr set_config_srv;
-    std::shared_ptr<rclcpp::Client<ChangeState>> change_state_client;
 
     std::atomic<bool> sensor_connection_active = {false};
     std::unique_ptr<std::thread> sensor_connection_thread;
diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp
index a0dadedd..817d3001 100644
--- a/ouster-ros/src/os_sensor_node_base.cpp
+++ b/ouster-ros/src/os_sensor_node_base.cpp
@@ -7,17 +7,25 @@
  */
 
 #include "ouster_ros/os_sensor_node_base.h"
-
 #include <ouster/impl/build.h>
-
 #include <fstream>
+#include <lifecycle_msgs/msg/transition.hpp>
 
+using namespace std::string_literals;
+using lifecycle_msgs::srv::ChangeState;
 namespace sensor = ouster::sensor;
 using ouster_sensor_msgs::srv::GetMetadata;
 using sensor::UDPProfileLidar;
 
 namespace ouster_ros {
 
+OusterSensorNodeBase::OusterSensorNodeBase(
+    const std::string& name, const rclcpp::NodeOptions& options)
+: rclcpp_lifecycle::LifecycleNode(name, options),
+  change_state_client{create_client<ChangeState>(name + "/change_state"s)} {
+}
+
+
 void OusterSensorNodeBase::create_get_metadata_service() {
     get_metadata_srv = create_service<GetMetadata>(
         "get_metadata",
@@ -74,4 +82,73 @@ bool OusterSensorNodeBase::write_text_to_file(const std::string& file_path,
     return true;
 }
 
+std::string OusterSensorNodeBase::transition_id_to_string(uint8_t transition_id) {
+    switch (transition_id) {
+        case lifecycle_msgs::msg::Transition::TRANSITION_CREATE:
+            return "create"s;
+        case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE:
+            return "configure"s;
+        case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP:
+            return "cleanup"s;
+        case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE:
+            return "activate"s;
+        case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE:
+            return "deactivate"s;
+        case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY:
+            return "destroy"s;
+        default:
+            return "unknown"s;
+    }
+}
+
+template <typename CallbackT, typename... CallbackT_Args>
+bool OusterSensorNodeBase::change_state(std::uint8_t transition_id, CallbackT callback,
+                                CallbackT_Args... callback_args,
+                                std::chrono::seconds time_out) {
+    if (!change_state_client->wait_for_service(time_out)) {
+        RCLCPP_ERROR_STREAM(
+            get_logger(), "Service " << change_state_client->get_service_name()
+                                    << "is not available.");
+        return false;
+    }
+
+    auto request = std::make_shared<ChangeState::Request>();
+    request->transition.id = transition_id;
+    // send an async request to perform the transition
+    change_state_client->async_send_request(
+        request, [this, callback,
+                callback_args...](rclcpp::Client<ChangeState>::SharedFuture ff) {
+            callback(callback_args..., ff.get()->success);
+        });
+    return true;
+}
+
+void OusterSensorNodeBase::execute_transitions_sequence(
+    std::vector<uint8_t> transitions_sequence, size_t at) {
+    assert(at < transitions_sequence.size() &&
+        "at index exceeds the number of transitions");
+    auto transition_id = transitions_sequence[at];
+    RCLCPP_DEBUG_STREAM(
+        get_logger(), "transition: [" << transition_id_to_string(transition_id)
+                                    << "] started");
+    change_state(transition_id, [this, transitions_sequence, at](bool success) {
+        if (success) {
+            RCLCPP_DEBUG_STREAM(
+                get_logger(),
+                "transition: [" << transition_id_to_string(transitions_sequence[at])
+                                << "] completed");
+            if (at < transitions_sequence.size() - 1) {
+                execute_transitions_sequence(transitions_sequence, at + 1);
+            } else {
+                RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed");
+            }
+        } else {
+            RCLCPP_DEBUG_STREAM(
+                get_logger(),
+                "transition: [" << transition_id_to_string(transitions_sequence[at])
+                                << "] failed");
+        }
+    });
+}
+
 }  // namespace ouster_ros
diff --git a/ouster-ros/src/os_static_transforms_broadcaster.h b/ouster-ros/src/os_static_transforms_broadcaster.h
index 760b5cda..911df513 100644
--- a/ouster-ros/src/os_static_transforms_broadcaster.h
+++ b/ouster-ros/src/os_static_transforms_broadcaster.h
@@ -23,6 +23,7 @@ class OusterStaticTransformsBroadcaster {
         node->declare_parameter("lidar_frame", "os_lidar");
         node->declare_parameter("imu_frame", "os_imu");
         node->declare_parameter("point_cloud_frame", "");
+        node->declare_parameter("pub_static_tf", true);
     }
 
     void parse_parameters() {
@@ -31,6 +32,7 @@ class OusterStaticTransformsBroadcaster {
         imu_frame = node->get_parameter("imu_frame").as_string();
         point_cloud_frame =
             node->get_parameter("point_cloud_frame").as_string();
+        pub_static_tf = node->get_parameter("pub_static_tf").as_bool();
 
         // validate point_cloud_frame
         if (point_cloud_frame.empty()) {
@@ -65,6 +67,7 @@ class OusterStaticTransformsBroadcaster {
     bool apply_lidar_to_sensor_transform() const {
         return point_cloud_frame == sensor_frame;
     }
+    bool publish_static_tf() const { return pub_static_tf; }
 
    private:
     NodeT* node;
@@ -73,6 +76,7 @@ class OusterStaticTransformsBroadcaster {
     std::string lidar_frame;
     std::string sensor_frame;
     std::string point_cloud_frame;
+    bool pub_static_tf;
 };
 
 }  // namespace ouster_ros