From c3e4e1114aa240cd5eab525198cd008c2f8cbc42 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Thu, 10 Aug 2023 13:49:49 +0300 Subject: [PATCH 01/13] Check invalid points by their ring and angle and do not use them --- .../velodyne/velodyne_common.hpp | 4 ++ .../decoders/velodyne_scan_decoder.hpp | 17 +++++ .../decoders/vlp16_decoder.cpp | 3 + nebula_ros/launch/velodyne_launch_all_hw.xml | 14 ++++ .../velodyne/velodyne_decoder_ros_wrapper.cpp | 64 +++++++++++++++++++ 5 files changed, 102 insertions(+) diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index b429e7b53..71d819469 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -19,6 +19,10 @@ struct VelodyneSensorConfiguration : LidarConfigurationBase uint16_t rotation_speed; uint16_t cloud_min_angle; uint16_t cloud_max_angle; + bool invalid_point_remove; + std::vector invalid_rings; + std::vector invalid_angles_start; + std::vector invalid_angles_end; }; /// @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..6e6b79c18 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,23 @@ 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 invalid. + /// @param point The point to check. + /// @return True if the point is invalid, false otherwise. + bool check_invalid_point(const int& channel, const uint16_t& azimuth) + { + if (!sensor_configuration_->invalid_point_remove) return false; + for (size_t i = 0; i < sensor_configuration_->invalid_rings.size(); ++i) { + if ( + sensor_configuration_->invalid_rings.at(i) == channel && + azimuth >= sensor_configuration_->invalid_angles_start.at(i) && + azimuth <= sensor_configuration_->invalid_angles_end.at(i)) { + return true; + } + } + return false; + } }; } // 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..c48854eb2 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -315,6 +315,9 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa current_point.return_type = return_type; current_point.channel = corrections.laser_ring; current_point.azimuth = rotation_radians_[azimuth_corrected]; + + if (check_invalid_point(corrections.laser_ring, azimuth_corrected)) continue; + current_point.elevation = sin_vert_angle; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; if (point_ts < 0) point_ts = 0; diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index c39437039..e08b592a0 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -19,6 +19,16 @@ + + + + + + + + + + @@ -26,6 +36,10 @@ + + + + declare_parameter("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_INFO_STREAM(this->get_logger(), "Invalid point remove is active."); + } else { + RCLCPP_INFO_STREAM(this->get_logger(), "Invalid point remove is not active."); + } + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("invalid_rings", descriptor); + sensor_configuration.invalid_rings = this->get_parameter("invalid_rings").as_integer_array(); + + RCLCPP_INFO_STREAM(this->get_logger(), "Invalid rings: "); + for (const auto & invalid_ring : sensor_configuration.invalid_rings) { + RCLCPP_INFO_STREAM(this->get_logger(), invalid_ring << " "); + } + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("invalid_angles_start", descriptor); + sensor_configuration.invalid_angles_start = + this->get_parameter("invalid_angles_start").as_integer_array(); + + RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles start: "); + for (const auto & invalid_angle_start : sensor_configuration.invalid_angles_start) { + RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_start << " "); + } + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("invalid_angles_end", descriptor); + sensor_configuration.invalid_angles_end = + this->get_parameter("invalid_angles_end").as_integer_array(); + + RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles end: "); + for (const auto & invalid_angle_end : sensor_configuration.invalid_angles_end) { + RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_end << " "); + } + } + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } From 0c754f8a5c4e25f2c4e1b9f84e84acf03eac35fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Thu, 10 Aug 2023 14:22:09 +0300 Subject: [PATCH 02/13] Created InvalidRegion struct and tidied up invalid point remover --- .../velodyne/velodyne_common.hpp | 12 ++++-- .../decoders/velodyne_scan_decoder.hpp | 10 ++--- .../velodyne/velodyne_decoder_ros_wrapper.cpp | 39 +++++++++++++++---- 3 files changed, 44 insertions(+), 17 deletions(-) diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index 71d819469..1440963c1 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -11,6 +11,14 @@ namespace nebula { namespace drivers { + +/// @brief Invalid region on the cloud to be removed +struct InvalidRegion { + int ring; + uint16_t start; + uint16_t end; +}; + /// @brief struct for Velodyne sensor configuration struct VelodyneSensorConfiguration : LidarConfigurationBase { @@ -20,9 +28,7 @@ struct VelodyneSensorConfiguration : LidarConfigurationBase uint16_t cloud_min_angle; uint16_t cloud_max_angle; bool invalid_point_remove; - std::vector invalid_rings; - std::vector invalid_angles_start; - std::vector invalid_angles_end; + std::vector invalid_regions; }; /// @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 6e6b79c18..71fe2a358 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 @@ -186,14 +186,12 @@ class VelodyneScanDecoder /// @brief Checks if the point is invalid. /// @param point The point to check. /// @return True if the point is invalid, false otherwise. - bool check_invalid_point(const int& channel, const uint16_t& azimuth) + bool check_invalid_point(const int & channel, const uint16_t & azimuth) { if (!sensor_configuration_->invalid_point_remove) return false; - for (size_t i = 0; i < sensor_configuration_->invalid_rings.size(); ++i) { - if ( - sensor_configuration_->invalid_rings.at(i) == channel && - azimuth >= sensor_configuration_->invalid_angles_start.at(i) && - azimuth <= sensor_configuration_->invalid_angles_end.at(i)) { + + for (const InvalidRegion & region : sensor_configuration_->invalid_regions) { + if (region.ring == channel && azimuth >= region.start && azimuth <= region.end) { return true; } } diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index 422ca99f6..6b0ea82e6 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -263,6 +263,10 @@ Status VelodyneDriverRosWrapper::GetParameters( } } + std::vector invalid_rings; + std::vector invalid_angles_start; + std::vector invalid_angles_end; + { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; @@ -270,10 +274,10 @@ Status VelodyneDriverRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter>("invalid_rings", descriptor); - sensor_configuration.invalid_rings = this->get_parameter("invalid_rings").as_integer_array(); + invalid_rings = this->get_parameter("invalid_rings").as_integer_array(); RCLCPP_INFO_STREAM(this->get_logger(), "Invalid rings: "); - for (const auto & invalid_ring : sensor_configuration.invalid_rings) { + for (const auto & invalid_ring : invalid_rings) { RCLCPP_INFO_STREAM(this->get_logger(), invalid_ring << " "); } } @@ -285,11 +289,10 @@ Status VelodyneDriverRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter>("invalid_angles_start", descriptor); - sensor_configuration.invalid_angles_start = - this->get_parameter("invalid_angles_start").as_integer_array(); + invalid_angles_start = this->get_parameter("invalid_angles_start").as_integer_array(); RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles start: "); - for (const auto & invalid_angle_start : sensor_configuration.invalid_angles_start) { + for (const auto & invalid_angle_start : invalid_angles_start) { RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_start << " "); } } @@ -301,15 +304,35 @@ Status VelodyneDriverRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter>("invalid_angles_end", descriptor); - sensor_configuration.invalid_angles_end = - this->get_parameter("invalid_angles_end").as_integer_array(); + invalid_angles_end = this->get_parameter("invalid_angles_end").as_integer_array(); RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles end: "); - for (const auto & invalid_angle_end : sensor_configuration.invalid_angles_end) { + for (const auto & invalid_angle_end : invalid_angles_end) { RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_end << " "); } } + if (sensor_configuration.invalid_point_remove) { + // Check length of invalid_rings, invalid_angles_start and invalid_angles_end + if ( + invalid_rings.size() != invalid_angles_start.size() || + invalid_rings.size() != invalid_angles_end.size()) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Invalid rings, invalid angles start and invalid angles end must have the same length."); + return Status::SENSOR_CONFIG_ERROR; + } + + // Add invalid regions to sensor configuration + for (size_t i = 0; i < invalid_rings.size(); i++) { + drivers::InvalidRegion invalid_region; + invalid_region.ring = invalid_rings[i]; + invalid_region.start = invalid_angles_start[i]; + invalid_region.end = invalid_angles_end[i]; + sensor_configuration.invalid_regions.push_back(invalid_region); + } + } + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } From ac085aa6a8741366cda4301410bb270e49366c53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Fri, 25 Aug 2023 13:39:41 +0300 Subject: [PATCH 03/13] Add filter to VLP32 and VLS128 --- .../src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp | 3 +++ .../src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp | 3 +++ 2 files changed, 6 insertions(+) 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..21d239469 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -342,6 +342,9 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa current_point.return_type = static_cast(return_type); current_point.channel = corrections.laser_ring; current_point.azimuth = rotation_radians_[block.rotation]; + + if (check_invalid_point(corrections.laser_ring, block.rotation)) continue; + current_point.elevation = sin_vert_angle; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; if (point_ts < 0) point_ts = 0; 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..a76cde41e 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -334,6 +334,9 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p current_point.return_type = return_type; current_point.channel = corrections.laser_ring; current_point.azimuth = rotation_radians_[azimuth_corrected]; + + if (check_invalid_point(corrections.laser_ring, azimuth_corrected)) continue; + current_point.elevation = sin_vert_angle; current_point.distance = distance; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; From 20f427fdd2bcc5c36f2c7edc324850b5265b8d47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Sat, 3 Feb 2024 12:50:01 +0300 Subject: [PATCH 04/13] refactor(velodyne): improve invalid point remove MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Mehmet Emin BAŞOĞLU --- .../velodyne/velodyne_common.hpp | 3 +- .../decoders/velodyne_scan_decoder.hpp | 11 +-- .../decoders/vlp16_decoder.cpp | 8 +- .../decoders/vlp32_decoder.cpp | 8 +- .../decoders/vls128_decoder.cpp | 8 +- .../velodyne/velodyne_decoder_ros_wrapper.hpp | 3 + nebula_ros/launch/velodyne_launch_all_hw.xml | 14 +-- .../velodyne/velodyne_decoder_ros_wrapper.cpp | 85 +++++-------------- 8 files changed, 43 insertions(+), 97 deletions(-) diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index 1440963c1..aba0163e2 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -14,7 +14,6 @@ namespace drivers /// @brief Invalid region on the cloud to be removed struct InvalidRegion { - int ring; uint16_t start; uint16_t end; }; @@ -28,7 +27,7 @@ struct VelodyneSensorConfiguration : LidarConfigurationBase uint16_t cloud_min_angle; uint16_t cloud_max_angle; bool invalid_point_remove; - std::vector invalid_regions; + std::map> invalid_regions; }; /// @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 71fe2a358..04732fddc 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 @@ -189,13 +189,10 @@ class VelodyneScanDecoder bool check_invalid_point(const int & channel, const uint16_t & azimuth) { if (!sensor_configuration_->invalid_point_remove) return false; - - for (const InvalidRegion & region : sensor_configuration_->invalid_regions) { - if (region.ring == channel && azimuth >= region.start && azimuth <= region.end) { - return true; - } - } - 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; + }); } }; 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 c48854eb2..d6a9b2f88 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -238,12 +238,13 @@ 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 ( - (azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 && + ((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 && (azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 || - azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) { + azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) && + !check_invalid_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; @@ -315,9 +316,6 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa current_point.return_type = return_type; current_point.channel = corrections.laser_ring; current_point.azimuth = rotation_radians_[azimuth_corrected]; - - if (check_invalid_point(corrections.laser_ring, azimuth_corrected)) continue; - current_point.elevation = sin_vert_angle; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; if (point_ts < 0) point_ts = 0; 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 21d239469..15dcd5344 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -195,12 +195,13 @@ void Vlp32Decoder::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 ( - (block.rotation >= sensor_configuration_->cloud_min_angle * 100 && + ((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 && (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))) && + !check_invalid_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; @@ -342,9 +343,6 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa current_point.return_type = static_cast(return_type); current_point.channel = corrections.laser_ring; current_point.azimuth = rotation_radians_[block.rotation]; - - if (check_invalid_point(corrections.laser_ring, block.rotation)) continue; - current_point.elevation = sin_vert_angle; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; if (point_ts < 0) point_ts = 0; 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 a76cde41e..700aaf938 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -259,12 +259,13 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p // Condition added to avoid calculating points which are not in the interesting defined // area (cloud_min_angle < area < cloud_max_angle). if ( - (azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 && + ((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 && (azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 || - azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) { + azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) && + !check_invalid_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; @@ -334,9 +335,6 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p current_point.return_type = return_type; current_point.channel = corrections.laser_ring; current_point.azimuth = rotation_radians_[azimuth_corrected]; - - if (check_invalid_point(corrections.laser_ring, azimuth_corrected)) continue; - current_point.elevation = sin_vert_angle; current_point.distance = distance; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; 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..b65283535 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 @@ -15,6 +15,9 @@ #include #include +#include "boost/property_tree/json_parser.hpp" +#include "boost/property_tree/ptree.hpp" + 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 e08b592a0..ddfc4e047 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -20,14 +20,8 @@ - - - - - - - - + @@ -37,9 +31,7 @@ - - - + declare_parameter("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_INFO_STREAM(this->get_logger(), "Invalid point remove is active."); } else { @@ -263,74 +262,36 @@ Status VelodyneDriverRosWrapper::GetParameters( } } - std::vector invalid_rings; - std::vector invalid_angles_start; - std::vector invalid_angles_end; - { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("invalid_rings", descriptor); - invalid_rings = this->get_parameter("invalid_rings").as_integer_array(); - - RCLCPP_INFO_STREAM(this->get_logger(), "Invalid rings: "); - for (const auto & invalid_ring : invalid_rings) { - RCLCPP_INFO_STREAM(this->get_logger(), invalid_ring << " "); - } - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("invalid_angles_start", descriptor); - invalid_angles_start = this->get_parameter("invalid_angles_start").as_integer_array(); - - RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles start: "); - for (const auto & invalid_angle_start : invalid_angles_start) { - RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_start << " "); - } - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; - descriptor.read_only = false; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter>("invalid_angles_end", descriptor); - invalid_angles_end = this->get_parameter("invalid_angles_end").as_integer_array(); - - RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles end: "); - for (const auto & invalid_angle_end : invalid_angles_end) { - RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_end << " "); - } - } - - if (sensor_configuration.invalid_point_remove) { - // Check length of invalid_rings, invalid_angles_start and invalid_angles_end - if ( - invalid_rings.size() != invalid_angles_start.size() || - invalid_rings.size() != invalid_angles_end.size()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Invalid rings, invalid angles start and invalid angles end must have the same length."); - return Status::SENSOR_CONFIG_ERROR; + this->declare_parameter("invalid_regions", "", descriptor); + const std::string regions_string = this->get_parameter("invalid_regions").as_string(); + std::istringstream regions_stream(regions_string); + + boost::property_tree::ptree pt; + boost::property_tree::read_json(regions_stream, pt); + + for (const auto & region : pt) { + std::vector elements; // Will store extracted ring number, start angle and end angle. + for (const auto & element : region.second) { + elements.push_back(element.second.get("")); + } + sensor_configuration.invalid_regions[elements[0]].push_back( + {static_cast(elements[1]), static_cast(elements[2])}); } - // Add invalid regions to sensor configuration - for (size_t i = 0; i < invalid_rings.size(); i++) { - drivers::InvalidRegion invalid_region; - invalid_region.ring = invalid_rings[i]; - invalid_region.start = invalid_angles_start[i]; - invalid_region.end = invalid_angles_end[i]; - sensor_configuration.invalid_regions.push_back(invalid_region); + std::stringstream regions_log; + for (const auto & regions : sensor_configuration.invalid_regions) { + regions_log << "\nRing: " << regions.first << '\n'; + for (const auto & region : regions.second) { + regions_log << "(" << region.start << "," << region.end << ")\n"; + } } + RCLCPP_INFO_STREAM(get_logger(), regions_log.str()); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { From 7f853a972efa397a8a2280fd8185f81c7bad0da7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Fri, 23 Feb 2024 13:41:36 +0300 Subject: [PATCH 05/13] fix(velodyne): don't get invalid angle params if remove is inactive --- nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index 44c9de536..16a5fde12 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -262,7 +262,7 @@ Status VelodyneDriverRosWrapper::GetParameters( } } - { + if (sensor_configuration.invalid_point_remove) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; From e361e1216d53a51dfc9967c229e42004cd3acb17 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Fri, 23 Feb 2024 14:00:11 +0300 Subject: [PATCH 06/13] chore: tidy up --- .../include/nebula_common/velodyne/velodyne_common.hpp | 10 +++++++--- .../decoders/velodyne_scan_decoder.hpp | 5 +++-- nebula_ros/launch/velodyne_launch_all_hw.xml | 4 ++-- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index aba0163e2..b629fb5b4 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -12,8 +12,10 @@ namespace nebula namespace drivers { -/// @brief Invalid region on the cloud to be removed -struct InvalidRegion { +/// @brief Invalid region on the cloud to be removed. `start` and `end` represent angles of the +/// region. +struct InvalidRegion +{ uint16_t start; uint16_t end; }; @@ -27,7 +29,9 @@ struct VelodyneSensorConfiguration : LidarConfigurationBase uint16_t cloud_min_angle; uint16_t cloud_max_angle; bool invalid_point_remove; - std::map> invalid_regions; + std::map> + invalid_regions; // Key holds the channel id, value holds invalid regions 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 04732fddc..92a393ef8 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 @@ -183,8 +183,9 @@ class VelodyneScanDecoder /// @brief Resetting overflowed point cloud buffer virtual void reset_overflow(double time_stamp) = 0; - /// @brief Checks if the point is invalid. - /// @param point The point to check. + /// @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_invalid_point(const int & channel, const uint16_t & azimuth) { diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index ddfc4e047..58f2f6050 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -19,9 +19,9 @@ - + + default="'[[0, 3500, 6900], [1, 3400, 6500], [2, 3200, 4600], [3, 3200, 4600]]'"/> From 4d33e1f2ea7ab3a6973a3e864a07d5cfebd75561 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Fri, 8 Mar 2024 10:59:49 +0300 Subject: [PATCH 07/13] docs: add ring based filter params to readme --- README.md | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index a509950ca..f3ca5e9e3 100644 --- a/README.md +++ b/README.md @@ -214,14 +214,20 @@ 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 | +| invalid_point_remove | bool | false | true, false | Enable ring based filter* | +| invalid_regions | string | | | Invalid point regions to remove* | + +*Ring based filter to remove reflected points and vehicle points. +Points will be removed from specified rings within the corresponding angle ranges. + ## Software design overview From 74c639ea75c027352c175933c9e40c3fc1ee6678 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Fri, 8 Mar 2024 11:39:03 +0300 Subject: [PATCH 08/13] chore(nebula_ros): make logs as debug --- nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index 16a5fde12..390ca4eb4 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -256,9 +256,9 @@ Status VelodyneDriverRosWrapper::GetParameters( sensor_configuration.invalid_point_remove = this->get_parameter("invalid_point_remove").as_bool(); if (sensor_configuration.invalid_point_remove) { - RCLCPP_INFO_STREAM(this->get_logger(), "Invalid point remove is active."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Invalid point remove is active."); } else { - RCLCPP_INFO_STREAM(this->get_logger(), "Invalid point remove is not active."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Invalid point remove is not active."); } } @@ -291,7 +291,7 @@ Status VelodyneDriverRosWrapper::GetParameters( regions_log << "(" << region.start << "," << region.end << ")\n"; } } - RCLCPP_INFO_STREAM(get_logger(), regions_log.str()); + RCLCPP_DEBUG_STREAM(get_logger(), regions_log.str()); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { From 7de409b04665c23acb5a7501b50618a266de5d18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Fri, 8 Mar 2024 12:44:49 +0300 Subject: [PATCH 09/13] docs: explain invalid point remove --- README.md | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index f3ca5e9e3..ac17899c2 100644 --- a/README.md +++ b/README.md @@ -225,9 +225,23 @@ Parameters shared by all supported models: | invalid_point_remove | bool | false | true, false | Enable ring based filter* | | invalid_regions | string | | | Invalid point regions to remove* | -*Ring based filter to remove reflected points and vehicle points. -Points will be removed from specified rings within the corresponding angle ranges. +*`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 + + ... + + + +``` + +*Multiple invalid regions are possible for the same ring. ## Software design overview From 8b7ca7ee342c96e91afd4a4a81cf6feea89caf40 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Tue, 16 Apr 2024 17:32:19 +0300 Subject: [PATCH 10/13] refactor: rename filter and parse regions with a different library --- README.md | 46 +++++++++---------- .../velodyne/velodyne_common.hpp | 10 ++-- .../decoders/velodyne_scan_decoder.hpp | 10 ++-- .../decoders/vlp16_decoder.cpp | 2 +- .../decoders/vlp32_decoder.cpp | 2 +- .../decoders/vls128_decoder.cpp | 2 +- .../velodyne/velodyne_decoder_ros_wrapper.hpp | 4 +- nebula_ros/launch/velodyne_launch_all_hw.xml | 12 ++--- nebula_ros/package.xml | 1 + .../velodyne/velodyne_decoder_ros_wrapper.cpp | 45 ++++++++++-------- 10 files changed, 69 insertions(+), 65 deletions(-) diff --git a/README.md b/README.md index ac17899c2..3c0302b49 100644 --- a/README.md +++ b/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 - ... - - - + name="velodyne_cloud" output="screen"> + ... + + + ``` -*Multiple invalid regions are possible for the same ring. - ## Software design overview ![DriverOrganization](docs/diagram.png) diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index b629fb5b4..709eed871 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/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> - invalid_regions; // Key holds the channel id, value holds invalid regions belong to that - // channel + 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 92a393ef8..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 @@ -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; }); } }; 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 d6a9b2f88..855cace4a 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/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; 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 15dcd5344..6dd4ebded 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/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; 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 700aaf938..3ae9d29eb 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/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; 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 b65283535..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,8 +16,7 @@ #include #include -#include "boost/property_tree/json_parser.hpp" -#include "boost/property_tree/ptree.hpp" +using namespace nlohmann; namespace nebula { diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index 58f2f6050..6d0a9acc9 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -19,19 +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 390ca4eb4..538cb52c8 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/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("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("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("invalid_regions", "", descriptor); - const std::string regions_string = this->get_parameter("invalid_regions").as_string(); - std::istringstream regions_stream(regions_string); + this->declare_parameter("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 elements; // Will store extracted ring number, start angle and end angle. - for (const auto & element : region.second) { - elements.push_back(element.second.get("")); - } - sensor_configuration.invalid_regions[elements[0]].push_back( - {static_cast(elements[1]), static_cast(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"; From 9820954cebfa9d4e8f3163946f50b95f816077a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Tue, 16 Apr 2024 17:46:25 +0300 Subject: [PATCH 11/13] refactor: rename filter parameter name --- README.md | 22 +++++++++---------- nebula_ros/launch/velodyne_launch_all_hw.xml | 4 ++-- .../velodyne/velodyne_decoder_ros_wrapper.cpp | 4 ++-- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 3c0302b49..234a9990f 100644 --- a/README.md +++ b/README.md @@ -214,16 +214,16 @@ 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 | -| 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. | +| 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]`. @@ -235,7 +235,7 @@ Parameters shared by all supported models: ... - + diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index 6d0a9acc9..78218773b 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -19,7 +19,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index 538cb52c8..8423c0e83 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -252,9 +252,9 @@ Status VelodyneDriverRosWrapper::GetParameters( descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("ring_section_filter", false, descriptor); + this->declare_parameter("enable_ring_section_filter", false, descriptor); sensor_configuration.ring_section_filter = - this->get_parameter("ring_section_filter").as_bool(); + 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 { From 59a5c22ebf4100a674366cec011c264366b4e7c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Tue, 16 Apr 2024 17:53:53 +0300 Subject: [PATCH 12/13] fix: typo --- nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index 8423c0e83..5de2ac24c 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -271,7 +271,7 @@ Status VelodyneDriverRosWrapper::GetParameters( this->declare_parameter("excluded_ring_sectors", "", descriptor); std::string sectors = this->get_parameter("excluded_ring_sectors").as_string(); - // Put sectors string inside square bracktes so that it can be a valid JSON array + // Put sectors string inside square brackets so that it can be a valid JSON array sectors = "[" + sectors + "]"; // Parse the JSON string From 860f6a0aca8d0f331131f7f6fe655bbe45d474ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Tue, 16 Apr 2024 18:32:02 +0300 Subject: [PATCH 13/13] refactor: simplify if condition --- .../decoders/vlp16_decoder.cpp | 16 +++++++++++----- .../decoders/vlp32_decoder.cpp | 16 +++++++++++----- .../decoders/vls128_decoder.cpp | 16 +++++++++++----- 3 files changed, 33 insertions(+), 15 deletions(-) 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 855cace4a..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,14 +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 ( - ((azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 && + 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))) && - !check_excluded_point(corrections.laser_ring, azimuth_corrected)) { + 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 6dd4ebded..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,14 +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 ( - ((block.rotation >= sensor_configuration_->cloud_min_angle * 100 && + 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))) && - !check_excluded_point(corrections.laser_ring, block.rotation)) { + 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 3ae9d29eb..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,14 +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 ( - ((azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 && + 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))) && - !check_excluded_point(corrections.laser_ring, azimuth_corrected)) { + 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;