From 9ec902b4279426ea58aeb9bcd5bb148f7bd7e13a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 10 May 2024 11:51:01 +0900 Subject: [PATCH] use long tail reference path Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 3 ++- .../src/goal_planner_module.cpp | 26 ++++++++++++++++--- 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 21ea06531c2f4..32a2e6ce7659c 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -499,7 +499,8 @@ class GoalPlannerModule : public SceneModuleInterface void updateStatus(const BehaviorModuleOutput & output); // validation - bool hasEnoughDistance(const PullOverPath & pull_over_path) const; + bool hasEnoughDistance( + const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const; bool isCrossingPossible( const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const; bool isCrossingPossible( diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index a890cfc21ed13..f728a0d22d184 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -848,6 +848,25 @@ std::optional> GoalPlannerModule::selectP const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const double collision_check_margin) const { + const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); + const double backward_length = + parameters_->backward_goal_search_length + parameters_->decide_path_distance; + const auto & prev_module_output_path = getPreviousModuleOutput().path; + const double prev_path_back_to_goal_dist = calcSignedArcLength( + prev_module_output_path.points, prev_module_output_path.points.back().point.pose.position, + goal_pose.position); + const auto & long_tail_reference_path = [&]() { + if (prev_path_back_to_goal_dist > backward_length) { + return prev_module_output_path; + } + // get road lanes which is at least backward_length[m] behind the goal + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + prev_module_output_path, planner_data_, backward_length, 0.0, + /*forward_only_in_route*/ false); + const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + return planner_data_->route_handler->getCenterLinePath( + road_lanes, std::max(0.0, goal_pose_length - backward_length), goal_pose_length); + }(); for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -859,7 +878,7 @@ std::optional> GoalPlannerModule::selectP continue; } - if (!hasEnoughDistance(pull_over_path)) { + if (!hasEnoughDistance(pull_over_path, long_tail_reference_path)) { continue; } @@ -1711,7 +1730,8 @@ bool GoalPlannerModule::checkObjectsCollision( return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); } -bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const +bool GoalPlannerModule::hasEnoughDistance( + const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; @@ -1723,7 +1743,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // otherwise, the goal would change immediately after departure. const bool is_separated_path = pull_over_path.partial_paths.size() > 1; const double distance_to_start = calcSignedArcLength( - pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); + long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel;