diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 127f40480ab1f..99445a2927f2a 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -158,7 +158,12 @@ bool StartPlannerModule::isExecutionRequested() const tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose)); // Check if ego is not out of lanes - const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_); + // const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = + utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits::max()); + const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_); auto lanes = current_lanes; lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); @@ -361,7 +366,12 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() return output; } - const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_); + // const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = + utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits::max()); + const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_); auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; const auto drivable_lanes = @@ -588,7 +598,10 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); - status_.current_lanes = utils::getExtendedCurrentLanes(planner_data_); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + status_.current_lanes = + utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits::max()); status_.pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_); // combine road and shoulder lanes diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 261a297f859bf..e4f0e13953924 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -40,7 +40,11 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p PullOutPath output; // combine road lane and shoulder lane - const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + const auto road_lanes = + utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits::max()); + const auto shoulder_lanes = getPullOutLanes(planner_data_); auto lanes = road_lanes; lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 9bed1f9a47fd5..9d21dbae99171 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -44,12 +44,16 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & road_lanes = utils::getExtendedCurrentLanes(planner_data_); - const auto & shoulder_lanes = getPullOutLanes(planner_data_); + const auto shoulder_lanes = getPullOutLanes(planner_data_); if (shoulder_lanes.empty()) { return boost::none; } + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + const auto road_lanes = + utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits::max()); + // find candidate paths auto pull_out_paths = calcPullOutPaths( *route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters, @@ -130,6 +134,7 @@ std::vector ShiftPullOut::calcPullOutPaths( } // rename parameter + const double forward_path_length = common_parameter.forward_path_length; const double backward_path_length = common_parameter.backward_path_length; const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance; const double lateral_jerk = parameter.lateral_jerk; @@ -145,13 +150,13 @@ std::vector ShiftPullOut::calcPullOutPaths( const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0); const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); - const double road_lanes_length = std::accumulate( - road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) { - return sum + lanelet::utils::getLaneletLength2d(lane); - }); - // if goal is behind start pose, + + // if goal is behind start pose, use path with forward_path_length const bool goal_is_behind = arc_position_goal.length < s_start; - const double s_end = goal_is_behind ? road_lanes_length : arc_position_goal.length; + const double s_forward_length = s_start + forward_path_length; + const double s_end = + goal_is_behind ? s_forward_length : std::min(arc_position_goal.length, s_forward_length); + constexpr double RESAMPLE_INTERVAL = 1.0; PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline( route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL);