diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 6d06b26cd1e17..ce49501b78d08 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -140,9 +141,14 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets); + *planner_data_->dynamic_object, data.current_lanelets, condition); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for