diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 4eb91e423c01f..3b649168884e6 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -14,16 +14,17 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Parameters -| Name | Type | Description | -| -------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- | -| `map_frame` | string | The frame name for map | -| `arrival_check_angle_deg` | double | Angle threshold for goal check | -| `arrival_check_distance` | double | Distance threshold for goal check | -| `arrival_check_duration` | double | Duration threshold for goal check | -| `goal_angle_threshold` | double | Max goal pose angle for goal approve | -| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | -| `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible | -| `minimum_reroute_length` | double | Minimum Length for publishing a new route | +| Name | Type | Description | +| ---------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- | +| `map_frame` | string | The frame name for map | +| `arrival_check_angle_deg` | double | Angle threshold for goal check | +| `arrival_check_distance` | double | Distance threshold for goal check | +| `arrival_check_duration` | double | Duration threshold for goal check | +| `goal_angle_threshold` | double | Max goal pose angle for goal approve | +| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | +| `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible | +| `minimum_reroute_length` | double | Minimum Length for publishing a new route | +| `consider_no_drivable_lanes` | bool | This flag is for considering no_drivable_lanes in planning or not. | ### Services diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml index d596eb9816f0a..98c28344ea25c 100644 --- a/planning/mission_planner/config/mission_planner.param.yaml +++ b/planning/mission_planner/config/mission_planner.param.yaml @@ -8,3 +8,4 @@ enable_correct_goal_pose: false reroute_time_threshold: 10.0 minimum_reroute_length: 30.0 + consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not. diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 95a4798e7db68..d8289824e517c 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -147,6 +147,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); param_.enable_correct_goal_pose = node_->declare_parameter("enable_correct_goal_pose"); + param_.consider_no_drivable_lanes = node_->declare_parameter("consider_no_drivable_lanes"); } void DefaultPlanner::initialize(rclcpp::Node * node) @@ -401,7 +402,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) const auto goal_check_point = points.at(i); lanelet::ConstLanelets path_lanelets; if (!route_handler_.planPathLaneletsBetweenCheckpoints( - start_check_point, goal_check_point, &path_lanelets)) { + start_check_point, goal_check_point, &path_lanelets, param_.consider_no_drivable_lanes)) { RCLCPP_WARN(logger, "Failed to plan route."); return route_msg; } diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 09b519267c962..cf5a19443dd82 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -37,6 +37,7 @@ struct DefaultPlannerParameters { double goal_angle_threshold_deg; bool enable_correct_goal_pose; + bool consider_no_drivable_lanes; }; class DefaultPlanner : public mission_planner::PlannerPlugin diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index c60c0e431108e..f37ebd024b28b 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -89,7 +89,7 @@ class RouteHandler // for routing bool planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, - lanelet::ConstLanelets * path_lanelets) const; + lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes = false) const; std::vector createMapSegments(const lanelet::ConstLanelets & path_lanelets) const; static bool isRouteLooped(const RouteSections & route_sections); diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 3027ca0b96560..fbc9bc5effc37 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1974,7 +1974,7 @@ lanelet::ConstLanelets RouteHandler::getNextLaneSequence( bool RouteHandler::planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, - lanelet::ConstLanelets * path_lanelets) const + lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const { // Find lanelets for start point. First, find all lanelets containing the start point to calculate // all possible route later. It fails when the point is not located on any road_lanelet (e.g. the @@ -2038,15 +2038,19 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( } if (is_route_found) { - bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); - if (shortest_path_has_no_drivable_lane) { - drivable_lane_path_found = - findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); - } - lanelet::routing::LaneletPath path; - if (drivable_lane_path_found) { - path = drivable_lane_path; + if (consider_no_drivable_lanes) { + bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); + if (shortest_path_has_no_drivable_lane) { + drivable_lane_path_found = + findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); + } + + if (drivable_lane_path_found) { + path = drivable_lane_path; + } else { + path = shortest_path; + } } else { path = shortest_path; }