diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 6c600dca84b25..b691483ae9579 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -309,8 +309,8 @@ int BlindSpotModule::insertPoint( } int insert_idx = -1; // initialize with epsilon so that comparison with insert_point_s = 0.0 would work - constexpr double eps = 1e-4; - double accum_s = eps * 2.0; + constexpr double eps = 1e-2; + double accum_s = eps + std::numeric_limits::epsilon(); for (size_t i = 1; i < inout_path->points.size(); i++) { accum_s += planning_utils::calcDist2d( inout_path->points[i].point.pose.position, inout_path->points[i - 1].point.pose.position); @@ -333,6 +333,15 @@ int BlindSpotModule::insertPoint( inout_path->points.at(insert_idx).point.longitudinal_velocity_mps = 0.0; is_point_inserted = false; return insert_idx; + } else if ( + insert_idx != 0 && + tier4_autoware_utils::calcDistance2d( + inserted_point, inout_path->points.at(static_cast(insert_idx - 1)).point) < + min_dist) { + inout_path->points.at(insert_idx - 1).point.longitudinal_velocity_mps = 0.0; + insert_idx--; + is_point_inserted = false; + return insert_idx; } inout_path->points.insert(it, inserted_point); is_point_inserted = true;