diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 2ff17bc508e89..5d82fbbfdf401 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -13,6 +13,7 @@ # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: true + allow_check_shift_path_lane_departure_override: false shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 563856c87db70..5b41edaee0da8 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -72,6 +72,7 @@ struct StartPlannerParameters // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + bool allow_check_shift_path_lane_departure_override{false}; double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index bd96a095eb767..f921df83120ae 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -54,6 +54,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); p.shift_collision_check_distance_from_end = node->declare_parameter(ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = @@ -390,6 +392,9 @@ void StartPlannerModuleManager::updateModuleParams( p->geometric_collision_check_distance_from_end); updateParam( parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure); + updateParam( + parameters, ns + "allow_check_shift_path_lane_departure_override", + p->allow_check_shift_path_lane_departure_override); updateParam(parameters, ns + "search_priority", p->search_priority); updateParam(parameters, ns + "max_back_distance", p->max_back_distance); updateParam( diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index d21a68048e34f..f5674cfb288d0 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -74,13 +74,27 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } - // check lane departure - // The method for lane departure checking verifies if the footprint of each point on the path is - // contained within a lanelet using `boost::geometry::within`, which incurs a high computational - // cost. const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + + // if lane departure check override is true, and if the initial pose is not fully within a lane, + // cancel lane departure check + const bool is_lane_departure_check_required = std::invoke([&]() -> bool { + if (!parameters_.allow_check_shift_path_lane_departure_override) + return parameters_.check_shift_path_lane_departure; + + PathWithLaneId path_with_only_first_pose{}; + path_with_only_first_pose.points.push_back(path_shift_start_to_end.points.at(0)); + return !lane_departure_checker_->checkPathWillLeaveLane( + lanelet_map_ptr, path_with_only_first_pose); + }); + + // check lane departure + // The method for lane departure checking verifies if the footprint of each point on the path + // is contained within a lanelet using `boost::geometry::within`, which incurs a high + // computational cost. + if ( - parameters_.check_shift_path_lane_departure && + is_lane_departure_check_required && lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) { continue; }