From 0363daff80bbb96431dc1ced2bb5976e98be8e6a Mon Sep 17 00:00:00 2001
From: Ussama Naal <606033+Samahu@users.noreply.github.com>
Date: Tue, 2 May 2023 12:23:05 -0700
Subject: [PATCH] SW-4859: enable having multiple components of same-type under
 same process (#108)

* Remove the use of static vars within components
* Resolve conflicts and update changelog and version
* Fix a typo 'instance'
---
 CHANGELOG.rst                     |  2 ++
 ouster-ros/package.xml            |  2 +-
 ouster-ros/src/os_cloud_node.cpp  | 25 ++++++++++++++++++-------
 ouster-ros/src/os_sensor_node.cpp |  8 +++++++-
 4 files changed, 28 insertions(+), 9 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index f6964420..7b6fdbdf 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -24,6 +24,8 @@ ouster_ros(2)
 * make specifying metadata file optional during record and replay modes as of package version 8.1
 * replace ``tf_prefix`` from ``os_cloud`` with ``sensor_frame``, ``lidar_frame`` and ``imu_frame``
   launch parameters.
+* bugfix: fixed an issue that prevents running multiple instances of the sensor and cloud components
+  in the same process.
 
 ouster_client
 -------------
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
index 1ffd719f..e79b8980 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.8.2</version>
+  <version>0.8.3</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 3af44b97..f4267857 100644
--- a/ouster-ros/src/os_cloud_node.cpp
+++ b/ouster-ros/src/os_cloud_node.cpp
@@ -181,9 +181,8 @@ class OusterCloud : public OusterProcessingNodeBase {
     void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud,
                       sensor_msgs::msg::PointCloud2& cloud) {
         // TODO: remove the staging step in the future
-        static pcl::PCLPointCloud2 pcl_pc2;
-        pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
-        pcl_conversions::moveFromPCL(pcl_pc2, cloud);
+        pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2);
+        pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud);
     }
 
     void convert_scan_to_pointcloud_publish(uint64_t scan_ts,
@@ -295,13 +294,18 @@ class OusterCloud : public OusterProcessingNodeBase {
     void lidar_handler_ros_time(const PacketMsg::ConstPtr& packet) {
         auto packet_receive_time = rclcpp::Clock(RCL_ROS_TIME).now();
         const uint8_t* packet_buf = packet->buf.data();
-        static auto frame_ts = extrapolate_frame_ts(
-            packet_buf, packet_receive_time);  // first point cloud time
+        if (!lidar_handler_ros_time_frame_ts_initialized) {
+            lidar_handler_ros_time_frame_ts = extrapolate_frame_ts(
+                packet_buf, packet_receive_time);  // first point cloud time
+            lidar_handler_ros_time_frame_ts_initialized = true;
+        }
         if (!(*scan_batcher)(packet_buf, *lidar_scan)) return;
         auto scan_ts = compute_scan_ts(lidar_scan->timestamp());
-        convert_scan_to_pointcloud_publish(scan_ts, frame_ts);
+        convert_scan_to_pointcloud_publish(scan_ts,
+                                           lidar_handler_ros_time_frame_ts);
         // set time for next point cloud msg
-        frame_ts = extrapolate_frame_ts(packet_buf, packet_receive_time);
+        lidar_handler_ros_time_frame_ts =
+            extrapolate_frame_ts(packet_buf, packet_receive_time);
     }
 
     void imu_handler(const PacketMsg::ConstSharedPtr packet) {
@@ -352,6 +356,13 @@ class OusterCloud : public OusterProcessingNodeBase {
         compute_scan_ts;
     double scan_col_ts_spacing_ns;  // interval or spacing between columns of a
                                     // scan
+
+    pcl::PCLPointCloud2
+        staging_pcl_pc2;  // a buffer used for staging during the conversion
+                          // from a PCL point cloud to a ros point cloud message
+
+    bool lidar_handler_ros_time_frame_ts_initialized = false;
+    rclcpp::Time lidar_handler_ros_time_frame_ts;
 };
 
 }  // namespace ouster_ros
diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp
index ffe3c716..0d4aa8e7 100644
--- a/ouster-ros/src/os_sensor_node.cpp
+++ b/ouster-ros/src/os_sensor_node.cpp
@@ -697,7 +697,10 @@ class OusterSensor : public OusterSensorNodeBase {
     bool init_id_changed(const sensor::packet_format& pf,
                          const uint8_t* lidar_buf) {
         uint32_t current_init_id = pf.init_id(lidar_buf);
-        static uint32_t last_init_id = current_init_id + 1;
+        if (!last_init_id_initialized) {
+            last_init_id = current_init_id + 1;
+            last_init_id_initialized = true;
+        }
         if (reset_last_init_id && last_init_id != current_init_id) {
             last_init_id = current_init_id;
             reset_last_init_id = false;
@@ -819,6 +822,9 @@ class OusterSensor : public OusterSensorNodeBase {
     bool reset_last_init_id = true;
     std::atomic<bool> reset_in_progress = {false};
 
+    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;