Port the changes to ROS2

Samahu · Samahu · commit 557fc076e6c3 · 2025-02-03T16:14:43.000-08:00
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -5,12 +5,14 @@ Changelog
 [unreleased]
 ============
 * [BUGFIX]: correctly align timestamps to the generated point cloud.
-* Added support to enable **loop** for pcap replay + other replay config.
-* Added a new launch file parameter ``pub_static_tf`` that allows users to turn off the braodcast
+* [BUGFIX]: NEAR_IR data is not populated with data for organized point clouds that have no range.
+* Add support to enable **loop** for pcap replay + other replay config.
+* Add a new launch file parameter ``pub_static_tf`` that allows users to turn off the braodcast
   of sensor TF transforms.
-* Introduced a new topic ``/ouster/telemetry`` that publishes ``ouster_ros::Telemetry`` messages,
+* Introduce a new topic ``/ouster/telemetry`` that publishes ``ouster_ros::Telemetry`` messages,
   the topic can be turned on/off by including the token ``TLM`` in the flag ``proc_mask`` launch arg.
-[BUGFIX]: NEAR_IR data is not populated with data for organized point clouds that have no range.
+* Add a new launch file parameter ``min_scan_valid_columns_ratio`` to allow users to set the minimum
+  ratio of valid columns in a scan for it to be processed. Default value is ``0.0``.
 
 ouster_ros v0.13.2
 ==================
diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml
@@ -115,3 +115,6 @@ ouster/os_driver:
     min_range: 0.0
     # max_range[optional]: maximum lidar range to consider (meters).
     max_range: 1000.0
+    # min_scan_valid_columns_ratio[optional]: The minimum ratio of valid columns for
+    # processing the LidarScan [0, 1]. default is 0%
+    min_scan_valid_columns_ratio: 0.0
diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml
@@ -79,6 +79,16 @@
     xyzir
     }"/>
 
+  <arg name="organized" default="true"
+    description="generate an organzied point cloud"/>
+  <arg name="destagger" default="true"
+    description="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" default="0.0"
+    description="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" default="100000.0"
+    description="minimum lidar range to consider (meters)"/>
+
   <arg name="azimuth_window_start" default="0" description="azimuth window start;
     values range [0, 360000] millidegrees"/>
   <arg name="azimuth_window_end" default="360000" description="azimuth window end;
@@ -99,6 +109,9 @@
   <arg name="auto_start" default="true"
     description="automatically configure and activate the node"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    description="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node_container pkg="rclcpp_components" exec="component_container_mt" name="os_container" output="screen" namespace="">
@@ -136,6 +149,11 @@
         <param name="proc_mask" value="$(var proc_mask)"/>
         <param name="scan_ring" value="$(var scan_ring)"/>
         <param name="point_type" value="$(var point_type)"/>
+        <param name="organized" value="$(var organized)"/>
+        <param name="destagger" value="$(var destagger)"/>
+        <param name="min_range" value="$(var min_range)"/>
+        <param name="max_range" value="$(var max_range)"/>
+        <param name="min_scan_valid_columns_ratio" value="$(var min_scan_valid_columns_ratio)"/>
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml
@@ -76,6 +76,9 @@
   <arg name="max_range" default="100000.0"
     description="minimum lidar range to consider (meters)"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    description="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_replay" name="os_replay" output="screen">
@@ -102,6 +105,7 @@
         <param name="destagger" value="$(var destagger)"/>
         <param name="min_range" value="$(var min_range)"/>
         <param name="max_range" value="$(var max_range)"/>
+        <param name="min_scan_valid_columns_ratio" value="$(var min_scan_valid_columns_ratio)"/>
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
         <remap from="/os_node/metadata" to="/ouster/metadata"/>
diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml
@@ -72,6 +72,9 @@
   <arg name="max_range" default="10000.0"
     description="minimum lidar range to consider (meters)"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    description="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_pcap" name="os_pcap"
@@ -99,6 +102,7 @@
         <param name="destagger" value="$(var destagger)"/>
         <param name="min_range" value="$(var min_range)"/>
         <param name="max_range" value="$(var max_range)"/>
+        <param name="min_scan_valid_columns_ratio" value="$(var min_scan_valid_columns_ratio)"/>
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h
@@ -69,10 +69,12 @@ class LidarPacketHandler {
     LidarPacketHandler(const sensor::sensor_info& info,
                        const std::vector<LidarScanProcessor>& handlers,
                        const std::string& timestamp_mode,
-                       int64_t ptp_utc_tai_offset)
+                       int64_t ptp_utc_tai_offset,
+                       float min_scan_valid_columns_ratio)
         : ring_buffer(LIDAR_SCAN_COUNT),
           lidar_scan_handlers{handlers},
-          ptp_utc_tai_offset_(ptp_utc_tai_offset) {
+          ptp_utc_tai_offset_(ptp_utc_tai_offset),
+          min_scan_valid_columns_ratio_(min_scan_valid_columns_ratio) {
         // initialize lidar_scan processor and buffer
         scan_batcher = std::make_unique<ouster::ScanBatcher>(info);
 
@@ -130,6 +132,19 @@ class LidarPacketHandler {
                         *(mutexes[ring_buffer.write_head()]));
                     auto& lidar_scan = *lidar_scans[ring_buffer.write_head()];
                     result = lidar_handler(*this, pf, lidar_packet, lidar_scan);
+                    if (result) {
+                        // count the number of valid columns in the scan
+                        auto status = lidar_scan.status();
+                        size_t valid_cols = std::count_if(status.data(), status.data() + status.size(),
+                               [](const uint32_t s) { return (s & 0x01); });
+                        if (valid_cols < static_cast<size_t>(min_scan_valid_columns_ratio_ * status.size())) {
+                            RCLCPP_WARN_STREAM(rclcpp::get_logger(getName()), "number of valid columns per scan "
+                                << valid_cols << "/" << status.size()
+                                <<" which is below the ratio " << std::setprecision(4) << (100 * min_scan_valid_columns_ratio_)
+                                << "%, SKIPPING SCAN");
+                            result = false;
+                        }
+                    }
                 }
                 if (result) {
                     ring_buffer.write();
@@ -159,9 +174,11 @@ class LidarPacketHandler {
     static HandlerType create(
         const sensor::sensor_info& info,
         const std::vector<LidarScanProcessor>& handlers,
-        const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) {
+        const std::string& timestamp_mode, int64_t ptp_utc_tai_offset,
+        float min_scan_valid_columns_ratio) {
         auto handler = std::make_shared<LidarPacketHandler>(
-            info, handlers, timestamp_mode, ptp_utc_tai_offset);
+            info, handlers, timestamp_mode, ptp_utc_tai_offset,
+            min_scan_valid_columns_ratio);
         return [handler](const sensor::LidarPacket& lidar_packet) {
             if (handler->lidar_packet_accumlator(lidar_packet)) {
                 handler->ring_buffer_has_elements.notify_one();
@@ -339,7 +356,7 @@ class LidarPacketHandler {
    private:
     std::unique_ptr<ouster::ScanBatcher> scan_batcher;
     const int LIDAR_SCAN_COUNT = 10;
-    const double THROTTLE_PERCENT = 0.7;
+    const float THROTTLE_PERCENT = 0.7f;
     LockFreeRingBuffer ring_buffer;
     std::mutex ring_buffer_mutex;
     std::vector<std::unique_ptr<ouster::LidarScan>> lidar_scans;
@@ -368,6 +385,8 @@ class LidarPacketHandler {
     std::condition_variable ring_buffer_has_elements;
 
     int64_t ptp_utc_tai_offset_;
+
+    float min_scan_valid_columns_ratio_ = 0.0f;
 };
 
 }  // namespace ouster_ros
diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp
@@ -66,6 +66,7 @@ class OusterCloud : public OusterProcessingNodeBase {
         declare_parameter("min_range", 0.0);
         declare_parameter("max_range", 1000.0);
         declare_parameter("rows_step", 1);
+        declare_parameter("min_scan_valid_columns_ratio", 0.0);
     }
 
     void metadata_handler(
@@ -115,9 +116,16 @@ class OusterCloud : public OusterProcessingNodeBase {
                 });
         }
 
+        auto min_scan_valid_columns_ratio = get_parameter("min_scan_valid_columns_ratio").as_double();
+        if (min_scan_valid_columns_ratio < 0.0f || min_scan_valid_columns_ratio > 1.0f) {
+            RCLCPP_FATAL(get_logger(), "min_scan_valid_columns_ratio needs to be in the range [0, 1]");
+            throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!");
+        }
+
         int num_returns = get_n_returns(info);
 
         std::vector<LidarScanProcessor> processors;
+
         if (impl::check_token(tokens, "PCL")) {
             lidar_pubs.resize(num_returns);
             for (int i = 0; i < num_returns; ++i) {
@@ -192,7 +200,8 @@ class OusterCloud : public OusterProcessingNodeBase {
         if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN")) {
             lidar_packet_handler = LidarPacketHandler::create(
                 info, processors, timestamp_mode,
-                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
+                min_scan_valid_columns_ratio);
         }
 
         if (impl::check_token(tokens, "TLM")) {
diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp
@@ -47,6 +47,7 @@ class OusterDriver : public OusterSensor {
         declare_parameter("min_range", 0.0);
         declare_parameter("max_range", 1000.0);
         declare_parameter("rows_step", 1);
+        declare_parameter("min_scan_valid_columns_ratio", 0.0);
     }
 
     ~OusterDriver() override {
@@ -83,6 +84,12 @@ class OusterDriver : public OusterSensor {
                 static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
         }
 
+        auto min_scan_valid_columns_ratio = get_parameter("min_scan_valid_columns_ratio").as_double();
+        if (min_scan_valid_columns_ratio < 0.0f || min_scan_valid_columns_ratio > 1.0f) {
+            RCLCPP_FATAL(get_logger(), "min_scan_valid_columns_ratio needs to be in the range [0, 1]");
+            throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!");
+        }
+
         int num_returns = get_n_returns(info);
 
         std::vector<LidarScanProcessor> processors;
@@ -202,7 +209,8 @@ class OusterDriver : public OusterSensor {
             impl::check_token(tokens, "IMG"))
             lidar_packet_handler = LidarPacketHandler::create(
                 info, processors, timestamp_mode,
-                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
+                min_scan_valid_columns_ratio);
 
         if (impl::check_token(tokens, "TLM")) {
             telemetry_pub =
diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp
@@ -42,6 +42,7 @@ class OusterImage : public OusterProcessingNodeBase {
         declare_parameter("timestamp_mode", "");
         declare_parameter("ptp_utc_tai_offset", -37.0);
         declare_parameter("use_system_default_qos", false);
+        declare_parameter("min_scan_valid_columns_ratio", 0.0);
         create_metadata_subscriber(
             [this](const auto& msg) { metadata_handler(msg); });
         RCLCPP_INFO(get_logger(), "OusterImage: node initialized!");
@@ -94,6 +95,12 @@ class OusterImage : public OusterProcessingNodeBase {
                                                             selected_qos);
         }
 
+        auto min_scan_valid_columns_ratio = get_parameter("min_scan_valid_columns_ratio").as_double();
+        if (min_scan_valid_columns_ratio < 0.0f || min_scan_valid_columns_ratio > 1.0f) {
+            RCLCPP_FATAL(get_logger(), "min_scan_valid_columns_ratio needs to be in the range [0, 1]");
+            throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!");
+        }
+
         std::vector<LidarScanProcessor> processors {
             ImageProcessor::create(
                 info, "os_lidar", /*TODO: tf_bcast.point_cloud_frame_id()*/
@@ -106,7 +113,8 @@ class OusterImage : public OusterProcessingNodeBase {
 
         lidar_packet_handler = LidarPacketHandler::create(
             info, processors, timestamp_mode,
-            static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
+            static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
+            min_scan_valid_columns_ratio);
         lidar_packet_sub = create_subscription<PacketMsg>(
                 "lidar_packets", selected_qos,
                 [this](const PacketMsg::ConstSharedPtr msg) {
