diff --git a/README.md b/README.md index a509950ca..234a9990f 100644 --- a/README.md +++ b/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 + + ... + + + +``` ## Software design overview diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index b429e7b53..709eed871 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/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> + 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 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp index 7c319e0fe..3568a07b8 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp +++ b/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 diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 338163d60..b1596fb50 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/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; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index ec3e06745..cfcaf88be 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/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; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index c42f9b0b5..10c598785 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/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; diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp index 5bc79b6c8..2637c36d7 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp +++ b/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 #include @@ -15,6 +16,8 @@ #include #include +using namespace nlohmann; + namespace nebula { namespace ros diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index c39437039..78218773b 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -19,13 +19,19 @@ + + + + name="velodyne_cloud" output="screen" args="--ros-args --log-level debug"> + + velodyne_msgs visualization_msgs yaml-cpp + nlohmann-json-dev ament_cmake_gtest ament_lint_auto diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index df484a27a..5de2ac24c 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/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("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("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; }