diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 54883489f2fe6..dc9f44fbd53da 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -155,6 +155,9 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + std::vector filterObjectsInTargetLane( + const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const; + //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. //! @param obstacle_check_distance Distance to check ahead for any objects that might be //! obstructing ego path. It makes sense to use values like the maximum lane change distance. 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 d3a120b43a073..4e2f973c7fa67 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 @@ -914,6 +914,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( return filtered_obj_indices; } +std::vector NormalLaneChange::filterObjectsInTargetLane( + const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const +{ + const auto target_polygon = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + std::vector filtered_objects{}; + if (target_polygon) { + for (auto & obj : objects.target_lane) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + if (boost::geometry::intersects(target_polygon.value(), obj_polygon)) { + filtered_objects.push_back(obj); + } + } + } + + return filtered_objects; +} + PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -1293,11 +1311,13 @@ bool NormalLaneChange::getLaneChangePaths( } candidate_paths->push_back(*candidate_path); + + std::vector filtered_objects = + filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && - utils::lane_change::passParkedObject( - route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + !is_stuck && utils::lane_change::passParkedObject( + route_handler, *candidate_path, filtered_objects, lane_change_buffer, + is_goal_in_route, *lane_change_parameters_, object_debug_)) { debug_print( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change.");