Skip to content

Commit 9ec902b

Browse files
committed
use long tail reference path
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 85c3f76 commit 9ec902b

File tree

2 files changed

+25
-4
lines changed

2 files changed

+25
-4
lines changed

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -499,7 +499,8 @@ class GoalPlannerModule : public SceneModuleInterface
499499
void updateStatus(const BehaviorModuleOutput & output);
500500

501501
// validation
502-
bool hasEnoughDistance(const PullOverPath & pull_over_path) const;
502+
bool hasEnoughDistance(
503+
const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const;
503504
bool isCrossingPossible(
504505
const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const;
505506
bool isCrossingPossible(

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

+23-3
Original file line numberDiff line numberDiff line change
@@ -848,6 +848,25 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
848848
const std::vector<PullOverPath> & pull_over_path_candidates,
849849
const GoalCandidates & goal_candidates, const double collision_check_margin) const
850850
{
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+
}();
851870
for (const auto & pull_over_path : pull_over_path_candidates) {
852871
// check if goal is safe
853872
const auto goal_candidate_it = std::find_if(
@@ -859,7 +878,7 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
859878
continue;
860879
}
861880

862-
if (!hasEnoughDistance(pull_over_path)) {
881+
if (!hasEnoughDistance(pull_over_path, long_tail_reference_path)) {
863882
continue;
864883
}
865884

@@ -1711,7 +1730,8 @@ bool GoalPlannerModule::checkObjectsCollision(
17111730
return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons);
17121731
}
17131732

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
17151735
{
17161736
const Pose & current_pose = planner_data_->self_odometry->pose.pose;
17171737
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
17231743
// otherwise, the goal would change immediately after departure.
17241744
const bool is_separated_path = pull_over_path.partial_paths.size() > 1;
17251745
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);
17271747
const double distance_to_restart = parameters_->decide_path_distance / 2;
17281748
const double eps_vel = 0.01;
17291749
const bool is_stopped = std::abs(current_vel) < eps_vel;

0 commit comments

Comments
 (0)