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 f5d340921a2c4..4fc138725c8bd 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -54,6 +54,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options "The horizontal_ring_id should be smaller than vertical_bins. Skip blockage diag!"); return; } + updater_.setHardwareID("blockage_diag"); updater_.add( std::string(this->get_namespace()) + ": blockage_validation", this, @@ -249,8 +250,13 @@ void BlockageDiagComponent::filter( ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_)); - sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / - static_cast(ideal_horizontal_bins * horizontal_ring_id_); + + if (horizontal_ring_id_ == 0) { + sky_blockage_ratio_ = 0.0f; + } else { + sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / + static_cast(ideal_horizontal_bins * horizontal_ring_id_); + } if (ground_blockage_ratio_ > blockage_ratio_threshold_) { cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask);