SW-4859: enable having multiple components of same type under same pr…

…ocess (#109)

* Remove the use of static vars within sensor and cloud components
* Resolve conflicts and update changelog and verion
* Fix a typo

CHANGELOG.rst

@@ -27,6 +27,8 @@ ouster_ros(2)
 * bugfix: remove ``--clock`` option when playing bag files in ros-foxy.
 * 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
 -------------

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

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

ouster-ros/src/os_sensor_node.cpp

@@ -699,7 +699,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;
@@ -821,6 +824,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;