HOTFIX/ROS-382: os_driver fails when raw option is enabled (#384)

* Fix os_driver fails when RAW option is enabled * Replace lifecycle_publisher with regular publisher for the os_pcap
ouster-lidar · Oct 8, 2024 · 78c96fa · 78c96fa
1 parent cd97df2
commit 78c96fa
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 14 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -2,9 +2,11 @@
 Changelog
 =========
 
-ouster_ros v0.13.1
+ouster_ros v0.13.2
 ==================
 * [BUGFIX]: Make sure to initialize the sensor with launch file parameters.
+* [BUGFIX]: ``os_driver`` failed when RAW option is used.
+
 
 ouster_ros v0.13.0
 ==================

diff --git 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.1</version>
+  <version>0.13.2</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp
@@ -200,21 +200,23 @@ class OusterDriver : public OusterSensor {
                 static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
 
         publish_raw = impl::check_token(tokens, "RAW");
+        if (publish_raw)
+            OusterSensor::create_publishers();
     }
 
     virtual void on_lidar_packet_msg(const LidarPacket& lidar_packet) override {
         if (lidar_packet_handler)
             lidar_packet_handler(lidar_packet);
-        
+
         if (publish_raw)
             OusterSensor::on_lidar_packet_msg(lidar_packet);
     }
 
     virtual void on_imu_packet_msg(const ImuPacket& imu_packet) override {
         if (imu_packet_handler)
             imu_pub->publish(imu_packet_handler(imu_packet));
-        
-        if(publish_raw)
+
+        if (publish_raw)
             OusterSensor::on_imu_packet_msg(imu_packet);
     }
 

diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp
@@ -75,8 +75,6 @@ class OusterPcap : public OusterSensorNodeBase {
         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_read_thread();
         return LifecycleNodeInterface::CallbackReturn::SUCCESS;
@@ -278,10 +276,8 @@ class OusterPcap : public OusterSensorNodeBase {
     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;
+    rclcpp::Publisher<ouster_sensor_msgs::msg::PacketMsg>::SharedPtr lidar_packet_pub;
+    rclcpp::Publisher<ouster_sensor_msgs::msg::PacketMsg>::SharedPtr imu_packet_pub;
 
     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
@@ -769,11 +769,10 @@ void OusterSensor::create_publishers() {
 
 void OusterSensor::allocate_buffers() {
     auto& pf = sensor::get_format(info);
-
-    lidar_packet_msg.buf.resize(pf.lidar_packet_size);
     lidar_packet.buf.resize(pf.lidar_packet_size);
+    lidar_packet_msg.buf.resize(pf.lidar_packet_size);
     imu_packet.buf.resize(pf.imu_packet_size);
-    imu_packet.buf.resize(pf.imu_packet_size);
+    imu_packet_msg.buf.resize(pf.imu_packet_size);
 }
 
 bool OusterSensor::init_id_changed(const sensor::packet_format& pf,