diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 6bdad3a3063ef..ea0f57c000aa6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -66,6 +66,14 @@ class GoalSearcher : public GoalSearcherBase BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; + /** @brief Create a lanelet that represents the departure check area. + * @param [in] pull_over_lanes Lanelets that the vehicle will pull over to. + * @param [in] route_handler RouteHandler object. + * @return Lanelet that goal footprints should be inside. + */ + lanelet::Lanelet createDepartureCehckLanelet( + const lanelet::ConstLanelets & pull_over_lanes, + const route_handler::RouteHandler & route_handler) const; LinearRing2d vehicle_footprint_{}; bool left_side_parking_{true}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 2d49f930c2692..dcbd1e8c6864f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -115,10 +115,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); - auto lanes = utils::getExtendedCurrentLanes( - planner_data, backward_length, forward_length, - /*forward_only_in_route*/ false); - lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); + const auto departure_check_lane = createDepartureCehckLanelet(pull_over_lanes, *route_handler); const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); @@ -175,7 +172,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p break; } - if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { + if (boost::geometry::intersects( + transformed_vehicle_footprint, departure_check_lane.polygon2d().basicPolygon())) { continue; } @@ -527,4 +525,52 @@ GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( return *closest_goal_candidate; } +lanelet::Lanelet GoalSearcher::createDepartureCehckLanelet( + const lanelet::ConstLanelets & pull_over_lanes, + const route_handler::RouteHandler & route_handler) const +{ + const auto getBoundPoints = + [&](const lanelet::ConstLanelet & lane, const bool is_outer) -> lanelet::Points3d { + lanelet::Points3d points; + const auto & bound = left_side_parking_ ? (is_outer ? lane.leftBound() : lane.rightBound()) + : (is_outer ? lane.rightBound() : lane.leftBound()); + for (const auto & pt : bound) { + points.push_back(lanelet::Point3d(pt)); + } + return points; + }; + + const auto getMostInnerLane = + [&](const lanelet::ConstLanelet & lane) -> std::optional { + auto neighboring_inner_lane = left_side_parking_ ? route_handler.getRightShoulderLanelet(lane) + : route_handler.getLeftShoulderLanelet(lane); + if (!neighboring_inner_lane) { + neighboring_inner_lane = left_side_parking_ ? route_handler.getRightLanelet(lane) + : route_handler.getLeftLanelet(lane); + } + return neighboring_inner_lane; + }; + + lanelet::Points3d outer_bound_points{}; + lanelet::Points3d inner_bound_points{}; + for (const auto & lane : pull_over_lanes) { + outer_bound_points = getBoundPoints(lane, true); + auto neighboring_inner_lane = getMostInnerLane(lane); + while (neighboring_inner_lane.has_value()) { + neighboring_inner_lane = getMostInnerLane(*neighboring_inner_lane); + } + if (neighboring_inner_lane.has_value()) { + inner_bound_points = getBoundPoints(*neighboring_inner_lane, false); + } else { + inner_bound_points = getBoundPoints(lane, false); + } + } + + const auto outer_linestring = lanelet::LineString3d(lanelet::InvalId, outer_bound_points); + const auto inner_linestring = lanelet::LineString3d(lanelet::InvalId, inner_bound_points); + return lanelet::Lanelet( + lanelet::InvalId, left_side_parking_ ? outer_linestring : inner_linestring, + left_side_parking_ ? inner_linestring : outer_linestring); +} + } // namespace autoware::behavior_path_planner