From 3b7094325070af79dcc6b6356470d5a830f74795 Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Wed, 7 Aug 2024 16:48:43 -0700
Subject: [PATCH 1/8] Port the pcap replay to ros2-foxy

---
 ouster-ros/CMakeLists.txt                |  46 ++-
 ouster-ros/launch/replay_pcap.launch.xml |  84 ++++++
 ouster-ros/src/os_pcap_node.cpp          | 347 +++++++++++++++++++++++
 3 files changed, 467 insertions(+), 10 deletions(-)
 create mode 100644 ouster-ros/launch/replay_pcap.launch.xml
 create mode 100644 ouster-ros/src/os_pcap_node.cpp

diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt
index 1da19df6..afadfb16 100644
--- a/ouster-ros/CMakeLists.txt
+++ b/ouster-ros/CMakeLists.txt
@@ -31,9 +31,11 @@ endif()
 option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON)
 
 set(_ouster_ros_INCLUDE_DIRS
-  include
-  ouster-sdk/ouster_client/include
-  ouster-sdk/ouster_client/include/optional-lite
+  "include;ouster-sdk/ouster_client/include;ouster-sdk/ouster_client/include/optional-lite"
+  "include;"
+  "ouster-sdk/ouster_client/include;"
+  "ouster-sdk/ouster_client/include/optional-lite;"
+  "ouster-sdk/ouster_pcap/include"
 )
 
 # ==== Libraries ====
@@ -55,6 +57,11 @@ include_directories(${_ouster_ros_INCLUDE_DIRS})
 # use only MPL-licensed parts of eigen
 add_definitions(-DEIGEN_MPL2_ONLY)
 
+set(OUSTER_TARGET_LINKS ouster_client)
+if (BUILD_PCAP)
+  list(APPEND OUSTER_TARGET_LINKS ouster_pcap)
+endif()
+
 add_library(ouster_ros_library SHARED
   src/os_ros.cpp
 )
@@ -78,7 +85,7 @@ target_link_libraries(ouster_ros_library
     ouster_build
     pcl_common
   # PRIVATE (unsupported)
-  -Wl,--whole-archive ouster_client -Wl,--no-whole-archive
+  -Wl,--whole-archive ${OUSTER_TARGET_LINKS} -Wl,--no-whole-archive
 )
 
 # helper method to construct ouster-ros components
@@ -165,6 +172,18 @@ rclcpp_components_register_node(os_driver_component
 )
 
 
+if (BUILD_PCAP)
+# ==== os_replay_component ====
+create_ros2_component(os_pcap_component
+  "src/os_sensor_node_base.cpp;src/os_pcap_node.cpp"
+  ""
+)
+rclcpp_components_register_node(os_pcap_component
+  PLUGIN "ouster_ros::OusterPcap"
+  EXECUTABLE os_pcap
+)
+endif()
+
 # ==== Test ====
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
@@ -191,14 +210,21 @@ endif()
 
 
 # ==== Install ====
+set(OUSTER_INSTALL_TARGETS
+  ouster_ros_library
+  os_sensor_component
+  os_replay_component
+  os_cloud_component
+  os_image_component
+  os_driver_component
+)
+if (BUILD_PCAP)
+  list(APPEND OUSTER_INSTALL_TARGETS os_pcap_component)
+endif()
+
 install(
   TARGETS
-    ouster_ros_library
-    os_sensor_component
-    os_replay_component
-    os_cloud_component
-    os_image_component
-    os_driver_component
+    ${OUSTER_INSTALL_TARGETS}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml
new file mode 100644
index 00000000..fadfddcd
--- /dev/null
+++ b/ouster-ros/launch/replay_pcap.launch.xml
@@ -0,0 +1,84 @@
+<launch>
+
+  <arg name="ouster_ns" default="ouster"
+    description="Override the default namespace of all ouster nodes"/>
+  <!-- TODO: revisit the proper behaviour of allowing users override the default timestamp_mode during replay -->
+  <arg name="timestamp_mode" default=""
+    description="method used to timestamp measurements; possible values: {
+    TIME_FROM_INTERNAL_OSC,
+    TIME_FROM_SYNC_PULSE_IN,
+    TIME_FROM_PTP_1588,
+    TIME_FROM_ROS_TIME
+    }"/>
+   <arg name="ptp_utc_tai_offset" default="-37.0"
+    description="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
+  <arg name="metadata" description="path to write metadata file when receiving sensor data"/>
+  <arg name="pcap_file" description="file name to use for the recorded bag file"/>
+  <arg name="viz" default="true"
+    description="whether to run a rviz"/>
+  <arg name="rviz_config" default="$(find-pkg-share ouster_ros)/config/viz-reliable.rviz"
+    description="optional rviz config file"/>
+
+  <arg name="sensor_frame" default="os_sensor"
+    description="sets name of choice for the sensor_frame tf frame, value can not be empty"/>
+  <arg name="lidar_frame" default="os_lidar"
+    description="sets name of choice for the os_lidar tf frame, value can not be empty"/>
+  <arg name="imu_frame" default="os_imu"
+      description="sets name of choice for the os_imu tf frame, value can not be empty"/>
+  <arg name="point_cloud_frame" default=""
+    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."/>
+
+  <let name="_use_metadata_file" value="$(eval '\'$(var metadata)\' != \'\'')"/>
+
+  <arg name="use_system_default_qos" default="true"
+    description="Use the default system QoS settings"/>
+
+  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" description="
+    The IMG flag here is not supported and does not affect anything,
+    to disable image topics you would need to omit the os_image node
+    from the launch file"/>
+
+  <arg name="scan_ring" default="0" description="
+    use this parameter in conjunction with the SCAN flag
+    and choose a value the range [0, sensor_beams_count)"/>
+
+  <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">
+      <param name="metadata" value="$(var metadata)"/>
+      <param name="pcap_file" value="$(var pcap_file)"/>
+      <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+    </node>
+    <node pkg="ouster_ros" exec="os_cloud" name="os_cloud" output="screen">
+      <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="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)"/>
+      <param name="proc_mask" value="$(var proc_mask)"/>
+      <param name="scan_ring" value="$(var scan_ring)"/>
+    </node>
+    <node pkg="ouster_ros" exec="os_image" name="os_image" output="screen">
+      <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+    </node>
+  </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)"/>
+  </include>
+
+</launch>
diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp
new file mode 100644
index 00000000..f04841fd
--- /dev/null
+++ b/ouster-ros/src/os_pcap_node.cpp
@@ -0,0 +1,347 @@
+/**
+ * Copyright (c) 2018-2023, Ouster, Inc.
+ * All rights reserved.
+ *
+ * @file os_pcap_node.cpp
+ * @brief This node handles replaying publishing pcap files
+ *
+ */
+
+// prevent clang-format from altering the location of "ouster_ros/ros.h", the
+// header file needs to be the first include due to PCL_NO_PRECOMPILE flag
+// clang-format off
+#include "ouster_ros/os_ros.h"
+// clang-format on
+
+#include "ouster_sensor_msgs/msg/packet_msg.h"
+#include "ouster_ros/os_sensor_node_base.h"
+#include "ouster_ros/visibility_control.h"
+
+#include "thread_safe_ring_buffer.h"
+#include <ouster/os_pcap.h>
+
+namespace sensor = ouster::sensor;
+using ouster::sensor_utils::PcapReader;
+
+using ouster_sensor_msgs::msg::PacketMsg;
+
+namespace ouster_ros {
+
+class OusterPcap : public OusterSensorNodeBase {
+   public:
+    OUSTER_ROS_PUBLIC
+    explicit OusterPcap(const rclcpp::NodeOptions& options)
+        : OusterSensorNodeBase("os_pcap", options) {
+        declare_parameters();
+    }
+
+    LifecycleNodeInterface::CallbackReturn on_configure(
+        const rclcpp_lifecycle::State&) {
+        RCLCPP_INFO(get_logger(), "on_configure() is called.");
+
+        try {
+            auto meta_file = get_meta_file();
+            auto pcap_file = get_pcap_file();
+            create_metadata_publisher();
+            load_metadata_from_file(meta_file);
+            open_pcap(pcap_file);
+            publish_metadata();
+            create_get_metadata_service();
+            RCLCPP_INFO(get_logger(), "Running in replay mode");
+        } catch (const std::exception& ex) {
+            RCLCPP_ERROR_STREAM(
+                get_logger(),
+                "exception thrown while configuring the sensor, details: "
+                    << ex.what());
+            // TODO: return ERROR on fatal errors, FAILURE otherwise
+            return LifecycleNodeInterface::CallbackReturn::ERROR;
+        }
+
+        return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+    }
+
+    LifecycleNodeInterface::CallbackReturn on_activate(
+        const rclcpp_lifecycle::State& state) {
+        RCLCPP_DEBUG(get_logger(), "on_activate() is called.");
+        LifecycleNode::on_activate(state);
+        create_publishers();
+        if (imu_packet_pub) imu_packet_pub->on_activate();
+        if (lidar_packet_pub) lidar_packet_pub->on_activate();
+        allocate_buffers();
+        start_packet_processing_threads();
+        start_packet_read_thread();
+        return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+    }
+
+    LifecycleNodeInterface::CallbackReturn on_error(
+        const rclcpp_lifecycle::State&) {
+        RCLCPP_DEBUG(get_logger(), "on_error() is called.");
+        // Always return failure for now
+        return LifecycleNodeInterface::CallbackReturn::FAILURE;
+    }
+
+    LifecycleNodeInterface::CallbackReturn on_deactivate(
+        const rclcpp_lifecycle::State& state) {
+        RCLCPP_DEBUG(get_logger(), "on_deactivate() is called.");
+        LifecycleNode::on_deactivate(state);
+        return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+    }
+
+    LifecycleNodeInterface::CallbackReturn on_cleanup(
+        const rclcpp_lifecycle::State&) {
+        RCLCPP_DEBUG(get_logger(), "on_cleanup() is called.");
+        cleanup();
+        return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+    }
+
+    LifecycleNodeInterface::CallbackReturn on_shutdown(
+        const rclcpp_lifecycle::State& state) {
+        RCLCPP_DEBUG(get_logger(), "on_shutdown() is called.");
+
+        if (state.label() == "unconfigured") {
+            // nothing to do, return success
+            return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+        }
+
+        // whether state was 'active' or 'inactive' do cleanup
+        try {
+            cleanup();
+        } catch (const std::exception& ex) {
+            RCLCPP_ERROR_STREAM(
+                get_logger(),
+                "exception thrown durng cleanup, details: " << ex.what());
+            return LifecycleNodeInterface::CallbackReturn::ERROR;
+        }
+
+        return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+    }
+
+   private:
+
+    void cleanup() {
+        get_metadata_srv.reset();
+    }
+
+    void declare_parameters() {
+        declare_parameter("metadata");
+        declare_parameter("pcap_file");
+        declare_parameter("use_system_default_qos", false);
+    }
+
+    std::string get_meta_file() const {
+        auto meta_file = get_parameter("metadata").as_string();
+        if (!is_arg_set(meta_file)) {
+            RCLCPP_ERROR(get_logger(),
+                         "Must specify metadata file in replay mode");
+            throw std::runtime_error("metadata no specificed");
+        }
+        return meta_file;
+    }
+
+    std::string get_pcap_file() const {
+        auto pcap_file = get_parameter("pcap_file").as_string();
+        if (!is_arg_set(pcap_file)) {
+            RCLCPP_ERROR(get_logger(),
+                         "Must specify pcap file in replay mode");
+            throw std::runtime_error("pcap_file param not set");
+        }
+        return pcap_file;
+    }
+
+    void load_metadata_from_file(const std::string& meta_file) {
+        try {
+            cached_metadata = read_text_file(meta_file);
+            info = sensor::parse_metadata(cached_metadata);
+            display_lidar_info(info);
+        } catch (const std::runtime_error& e) {
+            cached_metadata.clear();
+            RCLCPP_ERROR_STREAM(
+                get_logger(),
+                "Error when running in replay mode: " << e.what());
+        }
+    }
+
+    void allocate_buffers() {
+        auto& pf = sensor::get_format(info);
+
+        lidar_packet.buf.resize(pf.lidar_packet_size);
+        // TODO: gauge necessary queue size for lidar packets
+        lidar_packets =
+            std::make_unique<ThreadSafeRingBuffer>(pf.lidar_packet_size, 1024);
+
+        imu_packet.buf.resize(pf.imu_packet_size);
+        // TODO: gauge necessary queue size for lidar packets
+        imu_packets =
+            std::make_unique<ThreadSafeRingBuffer>(pf.imu_packet_size, 1024);
+    }
+
+    void create_publishers() {
+    bool use_system_default_qos =
+        get_parameter("use_system_default_qos").as_bool();
+        rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS();
+        rclcpp::QoS sensor_data_qos = rclcpp::SensorDataQoS();
+        auto selected_qos =
+            use_system_default_qos ? system_default_qos : sensor_data_qos;
+        lidar_packet_pub =
+            create_publisher<PacketMsg>("lidar_packets", selected_qos);
+        imu_packet_pub = create_publisher<PacketMsg>("imu_packets", selected_qos);
+    }
+
+    void open_pcap(const std::string& pcap_file) {
+        pcap.reset(new PcapReader(pcap_file));
+    }
+
+    void start_packet_read_thread() {
+        packet_read_active = true;
+        packet_read_thread = std::make_unique<std::thread>([this]() {
+            auto& pf = sensor::get_format(info);
+            while (packet_read_active) {
+                read_packets(*pcap, pf);
+            }
+            RCLCPP_DEBUG(get_logger(),
+                         "packet_read_thread done.");
+        });
+    }
+
+    void stop_packet_read_thread() {
+        RCLCPP_DEBUG(get_logger(), "packet_read_thread stopping.");
+        if (packet_read_thread->joinable()) {
+            packet_read_active = false;
+            packet_read_thread->join();
+        }
+    }
+
+    void start_packet_processing_threads() {
+        imu_packets_processing_thread_active = true;
+        imu_packets_processing_thread = std::make_unique<std::thread>([this]() {
+            while (imu_packets_processing_thread_active) {
+                imu_packets->read([this](const uint8_t* buffer) {
+                    on_imu_packet_msg(buffer);
+                });
+            }
+            RCLCPP_DEBUG(get_logger(), "imu_packets_processing_thread done.");
+        });
+
+        lidar_packets_processing_thread_active = true;
+        lidar_packets_processing_thread =
+            std::make_unique<std::thread>([this]() {
+                while (lidar_packets_processing_thread_active) {
+                    lidar_packets->read([this](const uint8_t* buffer) {
+                        on_lidar_packet_msg(buffer);
+                    });
+                }
+
+                RCLCPP_DEBUG(get_logger(),
+                             "lidar_packets_processing_thread done.");
+            });
+    }
+
+    void stop_packet_processing_threads() {
+        RCLCPP_DEBUG(get_logger(), "stopping packet processing threads.");
+
+        if (imu_packets_processing_thread->joinable()) {
+            imu_packets_processing_thread_active = false;
+            imu_packets_processing_thread->join();
+        }
+
+        if (lidar_packets_processing_thread->joinable()) {
+            lidar_packets_processing_thread_active = false;
+            lidar_packets_processing_thread->join();
+        }
+    }
+
+    void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) {
+        // copying the data from queue buffer into the message buffer
+        // this can be avoided by constructing an abstraction where
+        // OusterSensor has its own RingBuffer of PacketMsg but for
+        // now we are focusing on optimizing the code for OusterDriver
+        std::memcpy(lidar_packet.buf.data(), raw_lidar_packet,
+                    lidar_packet.buf.size());
+        lidar_packet_pub->publish(lidar_packet);
+    }
+
+    void on_imu_packet_msg(const uint8_t* raw_imu_packet) {
+        // copying the data from queue buffer into the message buffer
+        // this can be avoided by constructing an abstraction where
+        // OusterSensor has its own RingBuffer of PacketMsg but for
+        // now we are focusing on optimizing the code for OusterDriver
+        std::memcpy(imu_packet.buf.data(), raw_imu_packet,
+                    imu_packet.buf.size());
+        imu_packet_pub->publish(imu_packet);
+    }
+
+    void read_packets(PcapReader& pcap, const sensor::packet_format& pf) {
+        size_t payload_size = pcap.next_packet();
+        auto packet_info = pcap.current_info();
+        while (payload_size) {
+            auto start = std::chrono::high_resolution_clock::now();
+            if (packet_info.dst_port == info.config.udp_port_imu) {
+                imu_packets->write_overwrite(
+                    [this, &pcap, &pf, &packet_info](uint8_t* buffer) {
+                        std::memcpy(buffer, pcap.current_data(),
+                                    pf.imu_packet_size);
+                    });
+            } else if (packet_info.dst_port == info.config.udp_port_lidar) {
+                lidar_packets->write_overwrite(
+                    [this, &pcap, &pf, &packet_info](uint8_t* buffer) {
+                        std::memcpy(buffer, pcap.current_data(),
+                                    pf.lidar_packet_size);
+                    });
+            } else {
+                std::cout << "unknown packet" << std::endl;
+            }
+            auto prev_packet_ts = packet_info.timestamp;
+            payload_size = pcap.next_packet();
+            packet_info = pcap.current_info();
+            auto curr_packet_ts = packet_info.timestamp;
+            auto end = std::chrono::high_resolution_clock::now();
+            auto dt = (curr_packet_ts - prev_packet_ts) - (end - start);
+            std::this_thread::sleep_for(dt);  // pace packet generation
+        }
+    }
+
+   private:
+    std::shared_ptr<PcapReader> pcap;
+    ouster_sensor_msgs::msg::PacketMsg lidar_packet;
+    ouster_sensor_msgs::msg::PacketMsg imu_packet;
+    rclcpp_lifecycle::LifecyclePublisher<ouster_sensor_msgs::msg::PacketMsg>::SharedPtr
+        lidar_packet_pub;
+    rclcpp_lifecycle::LifecyclePublisher<ouster_sensor_msgs::msg::PacketMsg>::SharedPtr
+        imu_packet_pub;
+
+    std::unique_ptr<ThreadSafeRingBuffer> lidar_packets;
+    std::unique_ptr<ThreadSafeRingBuffer> imu_packets;
+
+    std::atomic<bool> packet_read_active = {false};
+    std::unique_ptr<std::thread> packet_read_thread;
+
+    std::atomic<bool> imu_packets_processing_thread_active = {false};
+    std::unique_ptr<std::thread> imu_packets_processing_thread;
+
+    std::atomic<bool> lidar_packets_processing_thread_active = {false};
+    std::unique_ptr<std::thread> lidar_packets_processing_thread;
+
+    bool force_sensor_reinit = false;
+    bool reset_last_init_id = true;
+
+    bool last_init_id_initialized = false;
+    uint32_t last_init_id;
+
+    // TODO: add as a ros parameter
+    const int max_poll_client_error_count = 10;
+    int poll_client_error_count = 0;
+    // TODO: add as a ros parameter
+    const int max_read_lidar_packet_errors = 60;
+    int read_lidar_packet_errors = 0;
+    // TODO: add as a ros parameter
+    const int max_read_imu_packet_errors = 60;
+    int read_imu_packet_errors = 0;
+
+    bool pause = false;
+};
+
+}  // namespace ouster_ros
+
+#include <rclcpp_components/register_node_macro.hpp>
+
+RCLCPP_COMPONENTS_REGISTER_NODE(ouster_ros::OusterPcap)

From 781b6038bed007b52690f9bc07186b2af40f3782 Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Wed, 7 Aug 2024 17:24:47 -0700
Subject: [PATCH 2/8] Update README.md and CHANGELOG

---
 CHANGELOG.rst          |  2 ++
 README.md              | 13 ++++++++++++-
 ouster-ros/package.xml |  2 +-
 3 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 2c4373d7..e1ba1270 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -11,6 +11,8 @@ Changelog
 * [BUGFIX]: Implement lock free ring buffer with throttling to reduce partial frames
 * add support for FUSA udp profile ``FUSA_RNG15_RFL8_NIR8_DUAL``.
 * [BREAKING]: Set xyz values of individual points in the PointCloud to NaNs when range is zero.
+* Added support to replay pcap format direclty from ouster-ros. The feature needs to be enabled
+  explicitly by turning on the ``BUILD_PCAP`` cmake option and having ``libpcap-dev`` installed.
 
 ouster_ros v0.12.0
 ==================
diff --git a/README.md b/README.md
index 74230a77..69a4cf49 100644
--- a/README.md
+++ b/README.md
@@ -25,6 +25,7 @@
       - [Sensor Mode](#sensor-mode)
       - [Recording Mode](#recording-mode)
       - [Replay Mode](#replay-mode)
+        - [PCAP Replay Mode](#pcap-replay-mode)
       - [Multicast Mode (experimental)](#multicast-mode-experimental)
     - [Invoking Services](#invoking-services)
       - [GetMetadata](#getmetadata)
@@ -100,7 +101,8 @@ sudo apt install -y         \
 > You may choose a different _ssl_ backend for the _curl_ library such as `libcurl4-gnutls-dev` or
 > `libcurl4-nss-dev`
 
-
+> **Note**  
+> To use the PCAP replay mode you need to have `libpcap-dev` installed
 
 ### Windows
 TBD
@@ -190,6 +192,15 @@ ros2 launch ouster_ros replay.launch.xml    \
     metadata:=<json file name>              # optional if bag file has /metadata topic
 ```
 
+##### PCAP Replay Mode
+> Note
+> To use this feature you need to compile the driver with `BUILD_PCAP` option enabled
+```bash
+ros2 launch ouster_ros replay_pcap.launch.xml   \
+    pcap_file:=<path to ouster pcap file>       \
+    metadata:=<json file name>              # required
+```
+
 #### Multicast Mode (experimental)
 The multicast launch mode supports configuring the sensor to broadcast lidar packets from the same
 sensor (live) to multiple active clients. You initiate this mode by using `sensor_mtp.launch.xml`
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
index 240e69dd..258523bc 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.12.4</version>
+  <version>0.12.5</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

From 33ac5bb644e0a197e45760d3fde078a8c11a4fd3 Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Thu, 15 Aug 2024 08:50:06 -0700
Subject: [PATCH 3/8] Add time update

---
 .../launch/replay.independent.launch.xml       |  2 ++
 ouster-ros/src/os_pcap_node.cpp                | 18 ------------------
 2 files changed, 2 insertions(+), 18 deletions(-)

diff --git a/ouster-ros/launch/replay.independent.launch.xml b/ouster-ros/launch/replay.independent.launch.xml
index cf3b4525..4e9b21d9 100644
--- a/ouster-ros/launch/replay.independent.launch.xml
+++ b/ouster-ros/launch/replay.independent.launch.xml
@@ -1,5 +1,7 @@
 <launch>
 
+  <set_parameter name="use_sim_time" value="true" />
+
   <arg name="ouster_ns" default="ouster"
     description="Override the default namespace of all ouster nodes"/>
   <!-- TODO: revisit the proper behaviour of allowing users override the default timestamp_mode during replay -->
diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp
index f04841fd..74def1ef 100644
--- a/ouster-ros/src/os_pcap_node.cpp
+++ b/ouster-ros/src/os_pcap_node.cpp
@@ -320,24 +320,6 @@ class OusterPcap : public OusterSensorNodeBase {
 
     std::atomic<bool> lidar_packets_processing_thread_active = {false};
     std::unique_ptr<std::thread> lidar_packets_processing_thread;
-
-    bool force_sensor_reinit = false;
-    bool reset_last_init_id = true;
-
-    bool last_init_id_initialized = false;
-    uint32_t last_init_id;
-
-    // TODO: add as a ros parameter
-    const int max_poll_client_error_count = 10;
-    int poll_client_error_count = 0;
-    // TODO: add as a ros parameter
-    const int max_read_lidar_packet_errors = 60;
-    int read_lidar_packet_errors = 0;
-    // TODO: add as a ros parameter
-    const int max_read_imu_packet_errors = 60;
-    int read_imu_packet_errors = 0;
-
-    bool pause = false;
 };
 
 }  // namespace ouster_ros

From 7b21aa478bcf3d324f71a0f3ccfd86ace37fcf4b Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Thu, 15 Aug 2024 08:55:26 -0700
Subject: [PATCH 4/8] The actual add time update + choose defaults

---
 ouster-ros/launch/replay_pcap.launch.xml |  6 ++++--
 ouster-ros/src/os_pcap_node.cpp          | 24 ++++++++++++++++++++++--
 2 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml
index fadfddcd..aae177d0 100644
--- a/ouster-ros/launch/replay_pcap.launch.xml
+++ b/ouster-ros/launch/replay_pcap.launch.xml
@@ -1,9 +1,11 @@
 <launch>
 
+  <!-- NOTE: pcap replay node does not implement clock-->
+  <set_parameter name="use_sim_time" value="false" />
+
   <arg name="ouster_ns" default="ouster"
     description="Override the default namespace of all ouster nodes"/>
-  <!-- TODO: revisit the proper behaviour of allowing users override the default timestamp_mode during replay -->
-  <arg name="timestamp_mode" default=""
+  <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC"
     description="method used to timestamp measurements; possible values: {
     TIME_FROM_INTERNAL_OSC,
     TIME_FROM_SYNC_PULSE_IN,
diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp
index 74def1ef..0c73c52b 100644
--- a/ouster-ros/src/os_pcap_node.cpp
+++ b/ouster-ros/src/os_pcap_node.cpp
@@ -13,6 +13,11 @@
 #include "ouster_ros/os_ros.h"
 // clang-format on
 
+#include <string>
+#include <thread>
+#include <chrono>
+#include <iomanip>
+
 #include "ouster_sensor_msgs/msg/packet_msg.h"
 #include "ouster_ros/os_sensor_node_base.h"
 #include "ouster_ros/visibility_control.h"
@@ -22,6 +27,7 @@
 
 namespace sensor = ouster::sensor;
 using ouster::sensor_utils::PcapReader;
+using namespace std::chrono;
 
 using ouster_sensor_msgs::msg::PacketMsg;
 
@@ -273,8 +279,13 @@ class OusterPcap : public OusterSensorNodeBase {
     void read_packets(PcapReader& pcap, const sensor::packet_format& pf) {
         size_t payload_size = pcap.next_packet();
         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);
+
         while (payload_size) {
-            auto start = std::chrono::high_resolution_clock::now();
+            auto start = high_resolution_clock::now();
             if (packet_info.dst_port == info.config.udp_port_imu) {
                 imu_packets->write_overwrite(
                     [this, &pcap, &pf, &packet_info](uint8_t* buffer) {
@@ -294,9 +305,18 @@ class OusterPcap : public OusterSensorNodeBase {
             payload_size = pcap.next_packet();
             packet_info = pcap.current_info();
             auto curr_packet_ts = packet_info.timestamp;
-            auto end = std::chrono::high_resolution_clock::now();
+            auto end = high_resolution_clock::now();
             auto dt = (curr_packet_ts - prev_packet_ts) - (end - start);
             std::this_thread::sleep_for(dt);  // pace packet generation
+
+            if (curr_packet_ts - last_update > UPDATE_PERIOD) {
+                last_update = curr_packet_ts;
+                std::cout << "\rtime passed: "
+                    << std::fixed << std::setprecision(3)
+                    << (curr_packet_ts - file_start).count() / 1e6f
+                    << " s" << std::flush
+                    << std::endl;   // This seem to be required for ROS2
+            }
         }
     }
 

From 6f1aa3eaad5ce77f54eb0600fb96e849606e2826 Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Thu, 15 Aug 2024 09:08:42 -0700
Subject: [PATCH 5/8] Add missing point_type parameter

---
 ouster-ros/launch/replay.independent.launch.xml | 10 ++++++++++
 ouster-ros/launch/replay_pcap.launch.xml        | 10 ++++++++++
 2 files changed, 20 insertions(+)

diff --git a/ouster-ros/launch/replay.independent.launch.xml b/ouster-ros/launch/replay.independent.launch.xml
index 4e9b21d9..cb0c44d1 100644
--- a/ouster-ros/launch/replay.independent.launch.xml
+++ b/ouster-ros/launch/replay.independent.launch.xml
@@ -48,6 +48,15 @@
     use this parameter in conjunction with the SCAN flag
     and choose a value the range [0, sensor_beams_count)"/>
 
+  <arg name="point_type" default="original" description="point type for the generated point cloud;
+   available options: {
+    original,
+    native,
+    xyz,
+    xyzi,
+    xyzir
+    }"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_replay" name="os_replay" output="screen">
@@ -63,6 +72,7 @@
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>
+      <param name="point_type" value="$(var point_type)"/>
     </node>
     <node pkg="ouster_ros" exec="os_image" name="os_image" output="screen">
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml
index aae177d0..35340119 100644
--- a/ouster-ros/launch/replay_pcap.launch.xml
+++ b/ouster-ros/launch/replay_pcap.launch.xml
@@ -46,6 +46,15 @@
     use this parameter in conjunction with the SCAN flag
     and choose a value the range [0, sensor_beams_count)"/>
 
+  <arg name="point_type" default="xyz" description="point type for the generated point cloud;
+   available options: {
+    original,
+    native,
+    xyz,
+    xyzi,
+    xyzir
+    }"/>
+
   <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">
@@ -63,6 +72,7 @@
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>
+      <param name="point_type" value="$(var point_type)"/>
     </node>
     <node pkg="ouster_ros" exec="os_image" name="os_image" output="screen">
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>

From 292a5c637773634ce900bc266a14f6ab4ade4b9a Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Thu, 15 Aug 2024 10:19:54 -0700
Subject: [PATCH 6/8] Update log message printing

---
 ouster-ros/src/os_pcap_node.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp
index 0c73c52b..dfe8a5be 100644
--- a/ouster-ros/src/os_pcap_node.cpp
+++ b/ouster-ros/src/os_pcap_node.cpp
@@ -300,6 +300,9 @@ class OusterPcap : public OusterSensorNodeBase {
                     });
             } else {
                 std::cout << "unknown packet" << std::endl;
+                RCLCPP_WARN_STREAM_THROTTLE(
+                    get_logger(), *get_clock(), 1,
+                    "unknown packet /w port" << packet_info.dst_port);
             }
             auto prev_packet_ts = packet_info.timestamp;
             payload_size = pcap.next_packet();

From 97ce1236436c5831c78383f2dcf7f7726cac9b61 Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Thu, 15 Aug 2024 10:22:13 -0700
Subject: [PATCH 7/8] Add space

---
 ouster-ros/src/os_pcap_node.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp
index dfe8a5be..7e856b9d 100644
--- a/ouster-ros/src/os_pcap_node.cpp
+++ b/ouster-ros/src/os_pcap_node.cpp
@@ -302,7 +302,7 @@ class OusterPcap : public OusterSensorNodeBase {
                 std::cout << "unknown packet" << std::endl;
                 RCLCPP_WARN_STREAM_THROTTLE(
                     get_logger(), *get_clock(), 1,
-                    "unknown packet /w port" << packet_info.dst_port);
+                    "unknown packet /w port: " << packet_info.dst_port);
             }
             auto prev_packet_ts = packet_info.timestamp;
             payload_size = pcap.next_packet();

From cc8a9f02fd7b7560bb9d0d6aed3451b2277f349d Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Thu, 15 Aug 2024 10:48:34 -0700
Subject: [PATCH 8/8] Remove duplicate includes

---
 ouster-ros/CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt
index afadfb16..220b0c72 100644
--- a/ouster-ros/CMakeLists.txt
+++ b/ouster-ros/CMakeLists.txt
@@ -31,7 +31,6 @@ endif()
 option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON)
 
 set(_ouster_ros_INCLUDE_DIRS
-  "include;ouster-sdk/ouster_client/include;ouster-sdk/ouster_client/include/optional-lite"
   "include;"
   "ouster-sdk/ouster_client/include;"
   "ouster-sdk/ouster_client/include/optional-lite;"