fix: Use the stable packet data copy for callback processing in Ouste…

…rDriver 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

@@ -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 {

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>

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,15 +141,21 @@ 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;
     std::string active_config;
     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