From c384aadd290e27485303faddc04fa2618903f1ca Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 28 Mar 2022 15:50:16 +0900 Subject: [PATCH] chore: check overflow Signed-off-by: badai-nguyen --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index cf432306dc1f9..68e288413bfb4 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -116,8 +116,12 @@ void BlockageDiagComponent::filter( if (pcl_input->points.empty()) { ground_blockage_ratio_ = 1.0f; sky_blockage_ratio_ = 1.0f; - ground_blockage_count_ += 1; - sky_blockage_count_ += 1; + if (ground_blockage_count_ < 2 * blockage_count_threshold_) { + ground_blockage_count_ += 1; + } + if (sky_blockage_count_ < 2 * blockage_count_threshold_) { + sky_blockage_count_ += 1; + } ground_blockage_range_deg_[0] = angle_range_deg_[0]; ground_blockage_range_deg_[1] = angle_range_deg_[1]; sky_blockage_range_deg_[0] = angle_range_deg_[0]; @@ -169,7 +173,10 @@ void BlockageDiagComponent::filter( static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; ground_blockage_range_deg_[1] = static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; - ground_blockage_count_ += 1; + + if (ground_blockage_count_ < 2 * blockage_count_threshold_) { + ground_blockage_count_ += 1; + } } else { ground_blockage_count_ = 0; } @@ -179,7 +186,9 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; sky_blockage_range_deg_[1] = static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; - sky_blockage_count_ += 1; + if (sky_blockage_count_ < 2 * blockage_count_threshold_) { + sky_blockage_count_ += 1; + } } else { sky_blockage_count_ = 0; }