feat: add ring based filter for velodyne

README.md

@@ -214,14 +214,32 @@ Parameters shared by all supported models:
 
 #### Driver parameters
 
-| Parameter        | Type   | Default  | Accepted values  | Description                   |
-| ---------------- | ------ | -------- | ---------------- | ----------------------------- |
-| frame_id         | string | velodyne |                  | ROS frame ID                  |
-| calibration_file | string |          |                  | LiDAR calibration file        |
-| min_range        | double | 0.3      | meters, >= 0.3   | Minimum point range published |
-| max_range        | double | 300.0    | meters, <= 300.0 | Maximum point range published |
-| cloud_min_angle  | uint16 | 0        | degrees [0, 360] | FoV start angle               |
-| cloud_max_angle  | uint16 | 359      | degrees [0, 360] | FoV end angle                 |
+| Parameter                  | Type   | Default  | Accepted values  | Description                                                                                                                        |
+|----------------------------|--------|----------|------------------|------------------------------------------------------------------------------------------------------------------------------------|
+| frame_id                   | string | velodyne |                  | ROS frame ID                                                                                                                       |
+| calibration_file           | string |          |                  | LiDAR calibration file                                                                                                             |
+| min_range                  | double | 0.3      | meters, >= 0.3   | Minimum point range published                                                                                                      |
+| max_range                  | double | 300.0    | meters, <= 300.0 | Maximum point range published                                                                                                      |
+| cloud_min_angle            | uint16 | 0        | degrees [0, 360] | FoV start angle                                                                                                                    |
+| cloud_max_angle            | uint16 | 359      | degrees [0, 360] | FoV end angle                                                                                                                      |
+| enable_ring_section_filter | bool   | false    | true, false      | Toggles filtering out specific ring sectors.                                                                                       |
+| excluded_ring_sectors      | string |          |                  | Identifies and prevents specific ring sectors from being included in the point cloud based on ring ID, start angle, and end angle. |
+
+- `enable_ring_section_filter` toggles a ring-based filter to remove points located within predefined angle ranges.
+- Specify an excluded section using the format `[ring_id, start_angle, end_angle]`.
+- Angles must be in degrees, scaled by a factor of 100. For example, represent `34.44` degrees as `3444`.
+- It's possible to define multiple excluded regions for the same ring, allowing for versatile filtering configurations.
+- Define excluded regions as a string of such regions, seperated by commas. For instance:
+
+```xml
+<node pkg="nebula_ros" exec="velodyne_driver_ros_wrapper_node"
+      name="velodyne_cloud" output="screen">
+  ...
+  <param name="enable_ring_section_filter" value="true"/>
+  <param name="excluded_ring_sectors" type="str"
+         value="[0, 3500, 6900], [1, 3400, 6500], [2, 3200, 4600], [3, 3200, 4600]"/>
+</node>
+```
 
 ## Software design overview
 

nebula_common/include/nebula_common/velodyne/velodyne_common.hpp

@@ -11,6 +11,15 @@ namespace nebula
 {
 namespace drivers
 {
+
+/// @brief Invalid region on the cloud to be removed. `start` and `end` represent angles of the
+/// region.
+struct ExcludedRegion
+{
+  uint16_t start;
+  uint16_t end;
+};
+
 /// @brief struct for Velodyne sensor configuration
 struct VelodyneSensorConfiguration : LidarConfigurationBase
 {
@@ -19,6 +28,10 @@ struct VelodyneSensorConfiguration : LidarConfigurationBase
   uint16_t rotation_speed;
   uint16_t cloud_min_angle;
   uint16_t cloud_max_angle;
+  bool ring_section_filter;
+  std::map<int, std::vector<ExcludedRegion>>
+    excluded_ring_sectors;  // Key holds the channel id, value holds excluded ring sectors belong to that
+                            // channel
 };
 /// @brief Convert VelodyneSensorConfiguration to string (Overloading the << operator)
 /// @param os

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp

@@ -182,6 +182,19 @@ class VelodyneScanDecoder
   virtual void reset_pointcloud(size_t n_pts, double time_stamp) = 0;
   /// @brief Resetting overflowed point cloud buffer
   virtual void reset_overflow(double time_stamp) = 0;
+
+  /// @brief Checks if the point is inside invalid regions.
+  /// @param channel Channel id of the point.
+  /// @param azimuth Azimuth angle of the point.
+  /// @return True if the point is invalid, false otherwise.
+  bool check_excluded_point(const int & channel, const uint16_t & azimuth)
+  {
+    if (!sensor_configuration_->ring_section_filter) return false;
+    const auto & sectors = sensor_configuration_->excluded_ring_sectors[channel];
+    return std::any_of(sectors.begin(), sectors.end(), [azimuth](const auto & sector) {
+      return azimuth >= sector.start && azimuth <= sector.end;
+    });
+  }
 };
 
 }  // namespace drivers

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -237,13 +237,20 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
 
               // Condition added to avoid calculating points which are not in the interesting
               // defined area (min_angle < area < max_angle).
-              if (
+              const bool is_within_min_max_angle =
                 (azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 &&
                  azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 &&
-                 sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) ||
+                 sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle);
+
+              const bool is_outside_max_min_angle =
                 (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle &&
                  (azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 ||
-                  azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) {
+                  azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100));
+
+              const bool is_not_excluded_point =
+                !check_excluded_point(corrections.laser_ring, azimuth_corrected);
+
+              if ((is_within_min_max_angle || is_outside_max_min_angle) && is_not_excluded_point) {
                 // Convert polar coordinates to Euclidean XYZ.
                 const float cos_vert_angle = corrections.cos_vert_correction;
                 const float sin_vert_angle = corrections.sin_vert_correction;

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp

@@ -194,13 +194,20 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
         distance < sensor_configuration_->max_range) {
         /*condition added to avoid calculating points which are not
             in the interesting defined area (min_angle < area < max_angle)*/
-        if (
+        const bool is_within_min_max_angle =
           (block.rotation >= sensor_configuration_->cloud_min_angle * 100 &&
            block.rotation <= sensor_configuration_->cloud_max_angle * 100 &&
-           sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) ||
+           sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle);
+
+        const bool is_outside_max_min_angle =
           (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle &&
            (raw->blocks[i].rotation <= sensor_configuration_->cloud_max_angle * 100 ||
-            raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) {
+            raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100));
+
+        const bool is_not_excluded_point =
+          !check_excluded_point(corrections.laser_ring, block.rotation);
+
+        if ((is_within_min_max_angle || is_outside_max_min_angle) && is_not_excluded_point) {
           const float cos_vert_angle = corrections.cos_vert_correction;
           const float sin_vert_angle = corrections.sin_vert_correction;
           const float cos_rot_correction = corrections.cos_rot_correction;

nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

@@ -258,13 +258,20 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
             distance < sensor_configuration_->max_range) {
             // Condition added to avoid calculating points which are not in the interesting defined
             // area (cloud_min_angle < area < cloud_max_angle).
-            if (
+            const bool is_within_min_max_angle =
               (azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 &&
                azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 &&
-               sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) ||
+               sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle);
+
+            const bool is_outside_max_min_angle =
               (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle &&
                (azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 ||
-                azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) {
+                azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100));
+
+            const bool is_not_excluded_point =
+              !check_excluded_point(corrections.laser_ring, azimuth_corrected);
+
+            if ((is_within_min_max_angle || is_outside_max_min_angle) && is_not_excluded_point) {
               // convert polar coordinates to Euclidean XYZ.
               const float cos_vert_angle = corrections.cos_vert_correction;
               const float sin_vert_angle = corrections.sin_vert_correction;

nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp

@@ -6,6 +6,7 @@
 #include "nebula_common/velodyne/velodyne_common.hpp"
 #include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp"
 #include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp"
+#include "nlohmann/json.hpp"
 
 #include <ament_index_cpp/get_package_prefix.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
@@ -15,6 +16,8 @@
 #include <velodyne_msgs/msg/velodyne_packet.hpp>
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
+using namespace nlohmann;
+
 namespace nebula
 {
 namespace ros

nebula_ros/launch/velodyne_launch_all_hw.xml

@@ -19,13 +19,19 @@
 
     <arg name="setup_sensor" default="True" description="Enable sensor setup on hw-driver."/>
 
+    <arg name="enable_ring_section_filter" default="false"/>
+    <arg name="excluded_ring_sectors"
+         default="[0, 3500, 6900], [1, 3400, 6500], [2, 3200, 4600], [3, 3200, 4600]"/>
+
     <node pkg="nebula_ros" exec="velodyne_driver_ros_wrapper_node"
-          name="velodyne_cloud" output="screen">
+          name="velodyne_cloud" output="screen" args="--ros-args --log-level debug">
         <param name="sensor_model" value="$(var sensor_model)"/>
         <param name="return_mode" value="$(var return_mode)"/>
         <param name="frame_id" value="$(var frame_id)"/>
         <param name="scan_phase" value="$(var scan_phase)"/>
         <param name="calibration_file" value="$(var calibration_file)"/>
+        <param name="enable_ring_section_filter" value="$(var enable_ring_section_filter)"/>
+        <param name="excluded_ring_sectors" type="str" value="$(var excluded_ring_sectors)"/>
     </node>
 
     <node pkg="nebula_ros" exec="velodyne_hw_ros_wrapper_node"

nebula_ros/package.xml

@@ -31,6 +31,7 @@
   <depend>velodyne_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>yaml-cpp</depend>
+  <depend>nlohmann-json-dev</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp

@@ -246,6 +246,59 @@ Status VelodyneDriverRosWrapper::GetParameters(
     }
   }
 
+  {
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+    descriptor.read_only = false;
+    descriptor.dynamic_typing = false;
+    descriptor.additional_constraints = "";
+    this->declare_parameter<bool>("enable_ring_section_filter", false, descriptor);
+    sensor_configuration.ring_section_filter =
+      this->get_parameter("enable_ring_section_filter").as_bool();
+    if (sensor_configuration.ring_section_filter) {
+      RCLCPP_DEBUG_STREAM(this->get_logger(), "Ring section filter is active.");
+    } else {
+      RCLCPP_DEBUG_STREAM(this->get_logger(), "Ring section filter is not active.");
+    }
+  }
+
+  if (sensor_configuration.ring_section_filter) {
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
+    descriptor.read_only = true;
+    descriptor.dynamic_typing = false;
+    descriptor.additional_constraints = "";
+    this->declare_parameter<std::string>("excluded_ring_sectors", "", descriptor);
+    std::string sectors = this->get_parameter("excluded_ring_sectors").as_string();
+
+    // Put sectors string inside square brackets so that it can be a valid JSON array
+    sectors = "[" + sectors + "]";
+
+    // Parse the JSON string
+    const auto excluded_sectors_json = json::parse(sectors);
+
+    // Iterate over the parsed JSON array
+    for (const auto & sector : excluded_sectors_json) {
+      // Extract the ring number, start angle, and end angle from each sub-array
+      const int ring_number = sector[0];
+      const uint16_t start = sector[1];
+      const uint16_t end = sector[2];
+
+      // Add the ExcludedRegion to the map under the corresponding ring number
+      sensor_configuration.excluded_ring_sectors[ring_number].emplace_back(
+        drivers::ExcludedRegion{start, end});
+    }
+
+    std::stringstream regions_log;
+    for (const auto & regions : sensor_configuration.excluded_ring_sectors) {
+      regions_log << "\nRing: " << regions.first << '\n';
+      for (const auto & region : regions.second) {
+        regions_log << "(" << region.start << "," << region.end << ")\n";
+      }
+    }
+    RCLCPP_DEBUG_STREAM(get_logger(), regions_log.str());
+  }
+
   if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) {
     return Status::INVALID_SENSOR_MODEL;
   }