@@ -848,6 +848,25 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
848
848
const std::vector<PullOverPath> & pull_over_path_candidates,
849
849
const GoalCandidates & goal_candidates, const double collision_check_margin) const
850
850
{
851
+ const auto & goal_pose = planner_data_->route_handler ->getOriginalGoalPose ();
852
+ const double backward_length =
853
+ parameters_->backward_goal_search_length + parameters_->decide_path_distance ;
854
+ const auto & prev_module_output_path = getPreviousModuleOutput ().path ;
855
+ const double prev_path_back_to_goal_dist = calcSignedArcLength (
856
+ prev_module_output_path.points , prev_module_output_path.points .back ().point .pose .position ,
857
+ goal_pose.position );
858
+ const auto & long_tail_reference_path = [&]() {
859
+ if (prev_path_back_to_goal_dist > backward_length) {
860
+ return prev_module_output_path;
861
+ }
862
+ // get road lanes which is at least backward_length[m] behind the goal
863
+ const auto road_lanes = utils::getExtendedCurrentLanesFromPath (
864
+ prev_module_output_path, planner_data_, backward_length, 0.0 ,
865
+ /* forward_only_in_route*/ false );
866
+ const auto goal_pose_length = lanelet::utils::getArcCoordinates (road_lanes, goal_pose).length ;
867
+ return planner_data_->route_handler ->getCenterLinePath (
868
+ road_lanes, std::max (0.0 , goal_pose_length - backward_length), goal_pose_length);
869
+ }();
851
870
for (const auto & pull_over_path : pull_over_path_candidates) {
852
871
// check if goal is safe
853
872
const auto goal_candidate_it = std::find_if (
@@ -859,7 +878,7 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
859
878
continue ;
860
879
}
861
880
862
- if (!hasEnoughDistance (pull_over_path)) {
881
+ if (!hasEnoughDistance (pull_over_path, long_tail_reference_path )) {
863
882
continue ;
864
883
}
865
884
@@ -1711,7 +1730,8 @@ bool GoalPlannerModule::checkObjectsCollision(
1711
1730
return utils::path_safety_checker::checkPolygonsIntersects (ego_polygons_expanded, obj_polygons);
1712
1731
}
1713
1732
1714
- bool GoalPlannerModule::hasEnoughDistance (const PullOverPath & pull_over_path) const
1733
+ bool GoalPlannerModule::hasEnoughDistance (
1734
+ const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const
1715
1735
{
1716
1736
const Pose & current_pose = planner_data_->self_odometry ->pose .pose ;
1717
1737
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
1723
1743
// otherwise, the goal would change immediately after departure.
1724
1744
const bool is_separated_path = pull_over_path.partial_paths .size () > 1 ;
1725
1745
const double distance_to_start = calcSignedArcLength (
1726
- pull_over_path. getFullPath () .points , current_pose.position , pull_over_path.start_pose .position );
1746
+ long_tail_reference_path .points , current_pose.position , pull_over_path.start_pose .position );
1727
1747
const double distance_to_restart = parameters_->decide_path_distance / 2 ;
1728
1748
const double eps_vel = 0.01 ;
1729
1749
const bool is_stopped = std::abs (current_vel) < eps_vel;
0 commit comments