diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d5e8d8b6b39a0..32448784f5702 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -280,14 +280,11 @@ void NormalLaneChange::insertStopPoint( } // target_objects includes objects out of target lanes, so filter them out - const auto polygon = - tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(); - if (std::all_of(polygon.begin(), polygon.end(), [&](const auto & polygon_p) { - auto footprint_pose = o.initial_pose.pose; - footprint_pose.position = tier4_autoware_utils::createPoint( - polygon_p.x(), polygon_p.y(), footprint_pose.position.z); - return !utils::isInLanelets(footprint_pose, status_.target_lanes); - })) { + if (!boost::geometry::intersects( + tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + lanelet::utils::combineLaneletsShape(status_.target_lanes) + .polygon2d() + .basicPolygon())) { return false; }