1616
1717#include " autoware_point_types/types.hpp"
1818
19- #include < boost/circular_buffer.hpp>
20-
2119#include < algorithm>
2220#include < numeric>
2321
@@ -35,7 +33,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
3533 blockage_ratio_threshold_ = declare_parameter<float >(" blockage_ratio_threshold"
3634 vertical_bins_ = declare_parameter<int >(" vertical_bins"
3735 angle_range_deg_ = declare_parameter<std::vector<double >>(" angle_range"
38- lidar_model_ = declare_parameter<std::string >(" model "
36+ is_channel_order_top2down_ = declare_parameter<bool >(" is_channel_order_top2down "
3937 blockage_count_threshold_ = declare_parameter<int >(" blockage_count_threshold"
4038 blockage_buffering_frames_ = declare_parameter<int >(" blockage_buffering_frames"
4139 blockage_buffering_interval_ = declare_parameter<int >(" blockage_buffering_interval"
@@ -44,6 +42,17 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
4442 dust_kernel_size_ = declare_parameter<int >(" dust_kernel_size"
4543 dust_buffering_frames_ = declare_parameter<int >(" dust_buffering_frames"
4644 dust_buffering_interval_ = declare_parameter<int >(" dust_buffering_interval"
45+ max_distance_range_ = declare_parameter<double >(" max_distance_range"
46+ horizontal_resolution_ = declare_parameter<double >(" horizontal_resolution"
47+ blockage_kernel_ = declare_parameter<int >(" blockage_kernel"
48+ }
49+ dust_mask_buffer.set_capacity (dust_buffering_frames_);
50+ no_return_mask_buffer.set_capacity (blockage_buffering_frames_);
51+ if (vertical_bins_ <= horizontal_ring_id_) {
52+ RCLCPP_ERROR (
53+ this ->get_logger (),
54+ " The horizontal_ring_id should be smaller than vertical_bins. Skip blockage diag!"
55+ return ;
4756 }
4857
4958 updater_.setHardwareID (" blockage_diag"
@@ -150,24 +159,17 @@ void BlockageDiagComponent::filter(
150159 std::scoped_lock lock (mutex_);
151160 int vertical_bins = vertical_bins_;
152161 int ideal_horizontal_bins;
153- float distance_coefficient = 327 .67f ;
154- float horizontal_resolution_ = 0 .4f ;
155- if (lidar_model_ == " Pandar40P"
156- distance_coefficient = 327 .67f ;
157- horizontal_resolution_ = 0 .4f ;
158- } else if (lidar_model_ == " PandarQT"
159- distance_coefficient = 3276 .75f ;
160- horizontal_resolution_ = 0 .6f ;
162+ double compensate_angle = 0.0 ;
163+ // Check the case when angle_range_deg_[1] exceed 360 and shifted the range to 0~360
164+ if (angle_range_deg_[0 ] > angle_range_deg_[1 ]) {
165+ compensate_angle = 360.0 ;
161166 }
162- ideal_horizontal_bins =
163- static_cast < uint >(( angle_range_deg_[1 ] - angle_range_deg_[0 ]) / horizontal_resolution_);
167+ ideal_horizontal_bins = static_cast < int >(
168+ ( angle_range_deg_[1 ] + compensate_angle - angle_range_deg_[0 ]) / horizontal_resolution_);
164169 pcl::PointCloud<PointXYZIRADRT>::
Ptr pcl_input (
new pcl::PointCloud<PointXYZIRADRT>);
165170 pcl::fromROSMsg (*input, *pcl_input);
166- std::vector<float > horizontal_bin_reference (ideal_horizontal_bins);
167- std::vector<pcl::PointCloud<PointXYZIRADRT>> each_ring_pointcloud (vertical_bins);
168171 cv::Mat full_size_depth_map (
169172 cv::Size (ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar (0 ));
170- cv::Mat lidar_depth_map (cv::Size (ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar (0 ));
171173 cv::Mat lidar_depth_map_8u (
172174 cv::Size (ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar (0 ));
173175 if (pcl_input->points .empty ()) {
@@ -184,24 +186,23 @@ void BlockageDiagComponent::filter(
184186 sky_blockage_range_deg_[0 ] = angle_range_deg_[0 ];
185187 sky_blockage_range_deg_[1 ] = angle_range_deg_[1 ];
186188 } else {
187- for (int i = 0 ; i < ideal_horizontal_bins; ++i) {
188- horizontal_bin_reference.at (i) = angle_range_deg_[0 ] + i * horizontal_resolution_;
189- }
190189 for (const auto p : pcl_input->points ) {
191- for (int horizontal_bin = 0 ;
192- horizontal_bin < static_cast <int >(horizontal_bin_reference.size ()); horizontal_bin++) {
193- if (
194- (p.azimuth / 100 >
195- (horizontal_bin_reference.at (horizontal_bin) - horizontal_resolution_ / 2 )) &&
196- (p.azimuth / 100 <=
197- (horizontal_bin_reference.at (horizontal_bin) + horizontal_resolution_ / 2 ))) {
198- if (lidar_model_ == " Pandar40P"
199- full_size_depth_map.at <uint16_t >(p.ring , horizontal_bin) =
200- UINT16_MAX - distance_coefficient * p.distance ;
201- } else if (lidar_model_ == " PandarQT"
202- full_size_depth_map.at <uint16_t >(vertical_bins - p.ring - 1 , horizontal_bin) =
203- UINT16_MAX - distance_coefficient * p.distance ;
204- }
190+ double azimuth_deg = p.azimuth / 100 .;
191+ if (
192+ ((azimuth_deg > angle_range_deg_[0 ]) &&
193+ (azimuth_deg <= angle_range_deg_[1 ] + compensate_angle)) ||
194+ ((azimuth_deg + compensate_angle > angle_range_deg_[0 ]) &&
195+ (azimuth_deg < angle_range_deg_[1 ]))) {
196+ double current_angle_range = (azimuth_deg + compensate_angle - angle_range_deg_[0 ]);
197+ int horizontal_bin_index = static_cast <int >(current_angle_range / horizontal_resolution_) %
198+ static_cast <int >(360.0 / horizontal_resolution_);
199+ uint16_t depth_intensity =
200+ UINT16_MAX * (1.0 - std::min (p.distance / max_distance_range_, 1.0 ));
201+ if (is_channel_order_top2down_) {
202+ full_size_depth_map.at <uint16_t >(p.ring , horizontal_bin_index) = depth_intensity;
203+ } else {
204+ full_size_depth_map.at <uint16_t >(vertical_bins - p.ring - 1 , horizontal_bin_index) =
205+ depth_intensity;
205206 }
206207 }
207208 }
@@ -215,7 +216,6 @@ void BlockageDiagComponent::filter(
215216 cv::Point (blockage_kernel_, blockage_kernel_));
216217 cv::erode (no_return_mask, erosion_dst, blockage_element);
217218 cv::dilate (erosion_dst, no_return_mask, blockage_element);
218- static boost::circular_buffer<cv::Mat> no_return_mask_buffer (blockage_buffering_frames_);
219219 cv::Mat time_series_blockage_result (
220220 cv::Size (ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar (0 ));
221221 cv::Mat time_series_blockage_mask (
@@ -250,8 +250,13 @@ void BlockageDiagComponent::filter(
250250 ground_blockage_ratio_ =
251251 static_cast <float >(cv::countNonZero (ground_no_return_mask)) /
252252 static_cast <float >(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_));
253- sky_blockage_ratio_ = static_cast <float >(cv::countNonZero (sky_no_return_mask)) /
254- static_cast <float >(ideal_horizontal_bins * horizontal_ring_id_);
253+
254+ if (horizontal_ring_id_ == 0 ) {
255+ sky_blockage_ratio_ = 0 .0f ;
256+ } else {
257+ sky_blockage_ratio_ = static_cast <float >(cv::countNonZero (sky_no_return_mask)) /
258+ static_cast <float >(ideal_horizontal_bins * horizontal_ring_id_);
259+ }
255260
256261 if (ground_blockage_ratio_ > blockage_ratio_threshold_) {
257262 cv::Rect ground_blockage_bb = cv::boundingRect (ground_no_return_mask);
@@ -295,7 +300,6 @@ void BlockageDiagComponent::filter(
295300 cv::inRange (single_dust_ground_img, 254 , 255 , single_dust_ground_img);
296301 cv::Mat ground_mask (cv::Size (ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1);
297302 cv::vconcat (sky_blank, single_dust_ground_img, single_dust_img);
298- static boost::circular_buffer<cv::Mat> dust_mask_buffer (dust_buffering_frames_);
299303 cv::Mat binarized_dust_mask_ (
300304 cv::Size (ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar (0 ));
301305 cv::Mat multi_frame_dust_mask (
@@ -405,8 +409,8 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback(
405409 RCLCPP_DEBUG (
406410 get_logger (), " Setting new blockage_count_threshold to: %d."
407411 }
408- if (get_param (p, " model " lidar_model_ )) {
409- RCLCPP_DEBUG (get_logger (), " Setting new lidar model to: %s . " lidar_model_. c_str () );
412+ if (get_param (p, " is_channel_order_top2down " is_channel_order_top2down_ )) {
413+ RCLCPP_DEBUG (get_logger (), " Setting new lidar model to: %d . " is_channel_order_top2down_ );
410414 }
411415 if (get_param (p, " angle_range"
412416 RCLCPP_DEBUG (
0 commit comments