From 455aff27934f6b1c1ce92d12e0b3b6ba0e978476 Mon Sep 17 00:00:00 2001
From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com>
Date: Mon, 4 Mar 2024 21:02:00 +0100
Subject: [PATCH] fix: Use the stable packet data copy for callback processing
 in OusterDriver as well.

* Utilizing the PacketMsg data structure to have the pointed-to buffer data transferred immediately once received and use the stable copy for further processing, instead of a reference to potentially mutable underlying data.
---
 ouster-ros/src/os_driver_node.cpp |  8 ++++----
 ouster-ros/src/os_sensor_node.cpp | 10 ++++++++--
 ouster-ros/src/os_sensor_node.h   | 14 ++++++++++----
 3 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp
index 49971f5c..c0be8d4e 100644
--- a/ouster-ros/src/os_driver_node.cpp
+++ b/ouster-ros/src/os_driver_node.cpp
@@ -186,13 +186,13 @@ class OusterDriver : public OusterSensor {
                 static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
     }
 
-    virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override {
-        if (lidar_packet_handler) lidar_packet_handler(raw_lidar_packet);
+    void process_lidar_packet() override {
+        if (lidar_packet_handler) lidar_packet_handler(lidar_packet.buf.data());
     }
 
-    virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override {
+    void process_imu_packet() override {
         if (imu_packet_handler)
-            imu_pub->publish(imu_packet_handler(raw_imu_packet));
+            imu_pub->publish(imu_packet_handler(imu_packet.buf.data()));
     }
 
     virtual void cleanup() override {
diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp
index 3c76d6cf..06fec23d 100644
--- a/ouster-ros/src/os_sensor_node.cpp
+++ b/ouster-ros/src/os_sensor_node.cpp
@@ -840,7 +840,7 @@ void OusterSensor::on_lidar_packet_msg(const uint8_t* raw_lidar_packet) {
     // 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);
+    process_lidar_packet();
 }
 
 void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) {
@@ -849,9 +849,15 @@ void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) {
     // 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);
+    process_imu_packet();
 }
 
+void OusterSensor::process_lidar_packet() {
+  lidar_packet_pub->publish(lidar_packet);
+}
+
+void OusterSensor::process_imu_packet() { imu_packet_pub->publish(imu_packet); }
+
 }  // namespace ouster_ros
 
 #include <rclcpp_components/register_node_macro.hpp>
diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h
index 0715a2d2..18aa3160 100644
--- a/ouster-ros/src/os_sensor_node.h
+++ b/ouster-ros/src/os_sensor_node.h
@@ -61,9 +61,9 @@ class OusterSensor : public OusterSensorNodeBase {
 
     virtual void create_publishers();
 
-    virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet);
+    virtual void process_lidar_packet();
 
-    virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet);
+    virtual void process_imu_packet();
 
     virtual void cleanup();
 
@@ -141,6 +141,14 @@ class OusterSensor : public OusterSensorNodeBase {
 
     void stop_packet_processing_threads();
 
+    void on_lidar_packet_msg(const uint8_t* raw_lidar_packet);
+
+    void on_imu_packet_msg(const uint8_t* raw_imu_packet);
+
+   protected:
+    ouster_sensor_msgs::msg::PacketMsg lidar_packet;
+    ouster_sensor_msgs::msg::PacketMsg imu_packet;
+
    private:
     std::string sensor_hostname;
     std::string staged_config;
@@ -148,8 +156,6 @@ class OusterSensor : public OusterSensorNodeBase {
     std::string mtp_dest;
     bool mtp_main;
     std::shared_ptr<sensor::client> sensor_client;
-    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