From 5cdce1ac2a876301c0d5350fd76a4bb03683f70a 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] 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 98d888653..d3ca641af 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 : SensorConfigurationBase 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 7f7230d06..5eb1e5017 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 @@ -195,13 +195,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 243ecd799..f9f537e96 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -236,12 +236,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; @@ -313,9 +314,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 979baaf1f..1852d3694 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -193,12 +193,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; @@ -340,9 +341,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 1c8d67219..2755ba8df 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -257,12 +257,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; @@ -332,9 +333,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) {