diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index 518052a1a4b8c..da22ef125fb19 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -144,10 +144,11 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( transformPointAndCalculate(pt, pt_map, angle_bin_index, range); // Ignore obstacle points exceed the range of the raw points - if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) { - continue; // No raw point in this angle bin - } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { - continue; // Obstacle point exceeds the range of the raw points + // No raw point in this angle bin, or obstacle point exceeds the range of the raw points + if ( + raw_pointcloud_angle_bins.at(angle_bin_index).empty() || + range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { + continue; } obstacle_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]); } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index dd934dea26c55..4f5b8b8eeabed 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -153,10 +153,11 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const double dz = scan_z - obstacle_z; // Ignore obstacle points exceed the range of the raw points - if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) { - continue; // No raw point in this angle bin - } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { - continue; // Obstacle point exceeds the range of the raw points + // No raw point in this angle bin, or obstacle point exceeds the range of the raw points + if ( + raw_pointcloud_angle_bins.at(angle_bin_index).empty() || + range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { + continue; } if (dz > projection_dz_threshold_) {