refactor: rename filter and parse regions with a different library

README.md

@@ -214,35 +214,33 @@ 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                    |
-| invalid_point_remove   | bool   | false    | true, false      | Enable ring based filter*        |
-| invalid_regions        | string |          |                  | Invalid point regions to remove* |
+| 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                                                                                                                      |
+| 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:
 
-*`invalid_point_remove` activates the ring based filter which removes points if they are within specified angle ranges. 
-
-*The format for an invalid region is [ring_id, start_angle, end_angle]
-
-*Angles are given in degrees and multiplied by 100. For instance, 34.44 degrees is represented as 3444. 
-
-*Invalid regions are specified as a string containing a list of invalid regions. Ensure that you have quotation marks to make it string. For example:
 ```xml
 <node pkg="nebula_ros" exec="velodyne_driver_ros_wrapper_node"
-          name="velodyne_cloud" output="screen">
-        ...
-        <param name="invalid_point_remove" value="true"/>
-        <param name="invalid_regions" value="'[[0, 3500, 6900], [1, 3400, 6500], [2, 3200, 4600], [3, 3200, 4600]]'"/>
-    </node>
+      name="velodyne_cloud" output="screen">
+  ...
+  <param name="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>
 ```
 
-*Multiple invalid regions are possible for the same ring.
-
 ## Software design overview
 
 ![DriverOrganization](docs/diagram.png)

nebula_common/include/nebula_common/velodyne/velodyne_common.hpp

@@ -14,7 +14,7 @@ namespace drivers
 
 /// @brief Invalid region on the cloud to be removed. `start` and `end` represent angles of the
 /// region.
-struct InvalidRegion
+struct ExcludedRegion
 {
   uint16_t start;
   uint16_t end;
@@ -28,10 +28,10 @@ struct VelodyneSensorConfiguration : LidarConfigurationBase
   uint16_t rotation_speed;
   uint16_t cloud_min_angle;
   uint16_t cloud_max_angle;
-  bool invalid_point_remove;
-  std::map<int, std::vector<InvalidRegion>>
-    invalid_regions;  // Key holds the channel id, value holds invalid regions belong to that
-                      // channel
+  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

@@ -187,12 +187,12 @@ class VelodyneScanDecoder
   /// @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_invalid_point(const int & channel, const uint16_t & azimuth)
+  bool check_excluded_point(const int & channel, const uint16_t & azimuth)
   {
-    if (!sensor_configuration_->invalid_point_remove) return false;
-    const auto & regions = sensor_configuration_->invalid_regions[channel];
-    return std::any_of(regions.begin(), regions.end(), [azimuth](const auto & region) {
-      return azimuth >= region.start && azimuth <= region.end;
+    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;
     });
   }
 };

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -244,7 +244,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                 (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))) &&
-                !check_invalid_point(corrections.laser_ring, azimuth_corrected)) {
+                !check_excluded_point(corrections.laser_ring, azimuth_corrected)) {
                 // 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

@@ -201,7 +201,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
           (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))) &&
-          !check_invalid_point(corrections.laser_ring, block.rotation)) {
+          !check_excluded_point(corrections.laser_ring, block.rotation)) {
           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

@@ -265,7 +265,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
               (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))) &&
-              !check_invalid_point(corrections.laser_ring, azimuth_corrected)) {
+              !check_excluded_point(corrections.laser_ring, azimuth_corrected)) {
               // 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,8 +16,7 @@
 #include <velodyne_msgs/msg/velodyne_packet.hpp>
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
-#include "boost/property_tree/json_parser.hpp"
-#include "boost/property_tree/ptree.hpp"
+using namespace nlohmann;
 
 namespace nebula
 {

nebula_ros/launch/velodyne_launch_all_hw.xml

@@ -19,19 +19,19 @@
 
     <arg name="setup_sensor" default="True" description="Enable sensor setup on hw-driver."/>
 
-    <arg name="invalid_point_remove" default="false"/>
-    <arg name="invalid_regions"
-         default="'[[0, 3500, 6900], [1, 3400, 6500], [2, 3200, 4600], [3, 3200, 4600]]'"/>
+    <arg name="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="invalid_point_remove" value="$(var invalid_point_remove)"/>
-        <param name="invalid_regions" value="$(var invalid_regions)"/>
+        <param name="ring_section_filter" value="$(var 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

@@ -252,40 +252,45 @@ Status VelodyneDriverRosWrapper::GetParameters(
     descriptor.read_only = false;
     descriptor.dynamic_typing = false;
     descriptor.additional_constraints = "";
-    this->declare_parameter<bool>("invalid_point_remove", false, descriptor);
-    sensor_configuration.invalid_point_remove =
-      this->get_parameter("invalid_point_remove").as_bool();
-    if (sensor_configuration.invalid_point_remove) {
-      RCLCPP_DEBUG_STREAM(this->get_logger(), "Invalid point remove is active.");
+    this->declare_parameter<bool>("ring_section_filter", false, descriptor);
+    sensor_configuration.ring_section_filter =
+      this->get_parameter("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(), "Invalid point remove is not active.");
+      RCLCPP_DEBUG_STREAM(this->get_logger(), "Ring section filter is not active.");
     }
   }
 
-  if (sensor_configuration.invalid_point_remove) {
+  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>("invalid_regions", "", descriptor);
-    const std::string regions_string = this->get_parameter("invalid_regions").as_string();
-    std::istringstream regions_stream(regions_string);
+    this->declare_parameter<std::string>("excluded_ring_sectors", "", descriptor);
+    std::string sectors = this->get_parameter("excluded_ring_sectors").as_string();
 
-    boost::property_tree::ptree pt;
-    boost::property_tree::read_json(regions_stream, pt);
+    // Put sectors string inside square bracktes so that it can be a valid JSON array
+    sectors = "[" + sectors + "]";
 
-    for (const auto & region : pt) {
-      std::vector<int> elements;  // Will store extracted ring number, start angle and end angle.
-      for (const auto & element : region.second) {
-        elements.push_back(element.second.get<int>(""));
-      }
-      sensor_configuration.invalid_regions[elements[0]].push_back(
-        {static_cast<uint16_t>(elements[1]), static_cast<uint16_t>(elements[2])});
+    // 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.invalid_regions) {
+    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";