fix(blockage_diag): output blockage angle range bug fix (autowarefoun…

…dation#6445) * fix: blockage_range_angle bug Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> * Revert "chore: refactor" This reverts commit fc43094. --------- Signed-off-by: badai-nguyen <[email protected]>
tier4 · Feb 20, 2024 · a1ec64c · a1ec64c
1 parent 44b25d3
commit a1ec64c
Showing 1 changed file with 6 additions and 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
@@ -255,9 +255,11 @@ void BlockageDiagComponent::filter(
 
   if (ground_blockage_ratio_ > blockage_ratio_threshold_) {
     cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask);
-    ground_blockage_range_deg_[0] = static_cast<float>(ground_blockage_bb.x) + angle_range_deg_[0];
+    ground_blockage_range_deg_[0] =
+      ground_blockage_bb.x * horizontal_resolution_ + angle_range_deg_[0];
     ground_blockage_range_deg_[1] =
-      static_cast<float>(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0];
+      (ground_blockage_bb.x + ground_blockage_bb.width) * horizontal_resolution_ +
+      angle_range_deg_[0];
     if (ground_blockage_count_ <= 2 * blockage_count_threshold_) {
       ground_blockage_count_ += 1;
     }
@@ -266,9 +268,9 @@ void BlockageDiagComponent::filter(
   }
   if (sky_blockage_ratio_ > blockage_ratio_threshold_) {
     cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask);
-    sky_blockage_range_deg_[0] = static_cast<float>(sky_blockage_bx.x) + angle_range_deg_[0];
+    sky_blockage_range_deg_[0] = sky_blockage_bx.x * horizontal_resolution_ + angle_range_deg_[0];
     sky_blockage_range_deg_[1] =
-      static_cast<float>(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0];
+      (sky_blockage_bx.x + sky_blockage_bx.width) * horizontal_resolution_ + angle_range_deg_[0];
     if (sky_blockage_count_ <= 2 * blockage_count_threshold_) {
       sky_blockage_count_ += 1;
     }