diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index b6f671cfc94f8..be3dd9ba96e38 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -41,6 +42,7 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using Polygon2d = tier4_autoware_utils::Polygon2d; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -85,6 +87,10 @@ PathWithLaneId extendPath( const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, const Pose & extend_pose); +std::vector createPathFootPrints( + const PathWithLaneId & path, const double base_to_front, const double base_to_rear, + const double width); + // debug MarkerArray createPullOverAreaMarkerArray( const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 8fba7f760ab9d..376adb178708a 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -258,17 +258,39 @@ std::optional ShiftPullOver::generatePullOverPath( road_lane_reference_path_to_shift_end.points.back().point.pose); pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); - // check if the parking path will leave lanes - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); - const auto combined_drivable = utils::combineDrivableLanes( - expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); - if (lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) { + // check if the parking path will leave drivable area and lanes + const bool is_in_parking_lots = std::invoke([&]() -> bool { + const auto & p = planner_data_->parameters; + const auto parking_lot_polygons = + lanelet::utils::query::getAllParkingLots(planner_data_->route_handler->getLaneletMapPtr()); + const auto path_footprints = goal_planner_utils::createPathFootPrints( + pull_over_path.getParkingPath(), p.base_link2front, p.base_link2rear, p.vehicle_width); + const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) { + return std::any_of( + parking_lot_polygons.begin(), parking_lot_polygons.end(), + [&footprint](const auto & polygon) { + return lanelet::geometry::within(footprint, lanelet::utils::to2D(polygon).basicPolygon()); + }); + }; + return std::all_of( + path_footprints.begin(), path_footprints.end(), + [&is_footprint_in_any_polygon](const auto & footprint) { + return is_footprint_in_any_polygon(footprint); + }); + }); + const bool is_in_lanes = std::invoke([&]() -> bool { + const auto drivable_lanes = + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; + const auto expanded_lanes = utils::expandLanelets( + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); + return !lane_departure_checker_.checkPathWillLeaveLane( + utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath()); + }); + if (!is_in_parking_lots && !is_in_lanes) { return {}; } diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 72ae0143783ea..e94ab74a3bbed 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -433,4 +433,17 @@ PathWithLaneId extendPath( return extendPath(target_path, reference_path, extend_distance); } +std::vector createPathFootPrints( + const PathWithLaneId & path, const double base_to_front, const double base_to_rear, + const double width) +{ + std::vector footprints; + for (const auto & point : path.points) { + const auto & pose = point.point.pose; + footprints.push_back( + tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width)); + } + return footprints; +} + } // namespace behavior_path_planner::goal_planner_utils diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index 2fb3e066f0bdd..9b19d8a81bb51 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -19,6 +19,7 @@ geometry_msgs nav_msgs nlohmann-json-dev + pybind11_vendor rosbag2_cpp std_msgs tf2