From df55162641a62086124aacd923f970bce16bb6a9 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 8 May 2024 11:22:06 +0900 Subject: [PATCH 1/4] feat(goal_planner): reject candidate path whose start pose direction is not aligned with ego path Signed-off-by: Mamoru Sobue --- .cspell.json | 2 +- .../goal_planner_module.hpp | 7 ++-- .../src/goal_planner_module.cpp | 32 ++++++++++++++++--- 3 files changed, 34 insertions(+), 7 deletions(-) diff --git a/.cspell.json b/.cspell.json index 94b509370e688..6a7ae735ef025 100644 --- a/.cspell.json +++ b/.cspell.json @@ -4,5 +4,5 @@ "planning/behavior_velocity_intersection_module/scripts/**" ], "ignoreRegExpList": [], - "words": ["dltype", "tvmgen", "fromarray"] + "words": ["dltype", "tvmgen", "fromarray", "followable"] } 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..953492764f3c9 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,9 @@ 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 std::vector & reference_path_directions) const; bool isCrossingPossible( const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const; bool isCrossingPossible( @@ -517,7 +519,8 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planPullOverAsCandidate(); std::optional> selectPullOverPath( const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const double collision_check_margin) const; + const GoalCandidates & goal_candidates, const std::vector & reference_path_directions, + const double collision_check_margin) const; std::vector sortPullOverPathCandidatesByGoalPriority( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; 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..dbf3b18ec7742 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 @@ -846,7 +846,8 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri std::optional> GoalPlannerModule::selectPullOverPath( const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const double collision_check_margin) const + const GoalCandidates & goal_candidates, const std::vector & reference_path_directions, + const double collision_check_margin) const { for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe @@ -859,7 +860,7 @@ std::optional> GoalPlannerModule::selectP continue; } - if (!hasEnoughDistance(pull_over_path)) { + if (!hasEnoughDistance(pull_over_path, reference_path_directions)) { continue; } @@ -1186,8 +1187,13 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() thread_safe_data_.get_pull_over_path_candidates(), goal_candidates); // select pull over path which is safe against static objects and get it's goal + std::vector reference_path_directions; + for (const auto & point : getPreviousModuleOutput().path.points) { + reference_path_directions.push_back( + tf2::getYaw(tier4_autoware_utils::getPose(point).orientation)); + } const auto path_and_goal_opt = selectPullOverPath( - pull_over_path_candidates, goal_candidates, + pull_over_path_candidates, goal_candidates, reference_path_directions, parameters_->object_recognition_collision_check_hard_margins.back()); // update thread_safe_data_ @@ -1711,7 +1717,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 std::vector & reference_path_directions) const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; @@ -1721,6 +1728,23 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. + + // reject outdated shift path on curved roads whose start pose direction is far from ego yaw + const double pull_over_path_start_direction = tf2::getYaw(pull_over_path.start_pose.orientation); + const bool isFollowableDirection = [&]() { + for (const auto reference_path_direction : reference_path_directions) { + if ( + std::fabs(tier4_autoware_utils::normalizeRadian( + pull_over_path_start_direction - reference_path_direction)) < M_PI / 2) { + return true; + } + } + return false; + }(); + if (!isFollowableDirection) { + return false; + } + 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); From 8e553479e98e8ce80506164653f0bf0a90bd1175 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 9 May 2024 17:40:11 +0900 Subject: [PATCH 2/4] Revert "feat(goal_planner): reject candidate path whose start pose direction is not aligned with ego path" This reverts commit 0307cae9608782fe31a1449644fc497ecc256814. Signed-off-by: Mamoru Sobue --- .cspell.json | 2 +- .../goal_planner_module.hpp | 7 ++-- .../src/goal_planner_module.cpp | 32 +++---------------- 3 files changed, 7 insertions(+), 34 deletions(-) diff --git a/.cspell.json b/.cspell.json index 6a7ae735ef025..94b509370e688 100644 --- a/.cspell.json +++ b/.cspell.json @@ -4,5 +4,5 @@ "planning/behavior_velocity_intersection_module/scripts/**" ], "ignoreRegExpList": [], - "words": ["dltype", "tvmgen", "fromarray", "followable"] + "words": ["dltype", "tvmgen", "fromarray"] } 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 953492764f3c9..21ea06531c2f4 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,9 +499,7 @@ class GoalPlannerModule : public SceneModuleInterface void updateStatus(const BehaviorModuleOutput & output); // validation - bool hasEnoughDistance( - const PullOverPath & pull_over_path, - const std::vector & reference_path_directions) const; + bool hasEnoughDistance(const PullOverPath & pull_over_path) const; bool isCrossingPossible( const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const; bool isCrossingPossible( @@ -519,8 +517,7 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planPullOverAsCandidate(); std::optional> selectPullOverPath( const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const std::vector & reference_path_directions, - const double collision_check_margin) const; + const GoalCandidates & goal_candidates, const double collision_check_margin) const; std::vector sortPullOverPathCandidatesByGoalPriority( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; 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 dbf3b18ec7742..a890cfc21ed13 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 @@ -846,8 +846,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri std::optional> GoalPlannerModule::selectPullOverPath( const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const std::vector & reference_path_directions, - const double collision_check_margin) const + const GoalCandidates & goal_candidates, const double collision_check_margin) const { for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe @@ -860,7 +859,7 @@ std::optional> GoalPlannerModule::selectP continue; } - if (!hasEnoughDistance(pull_over_path, reference_path_directions)) { + if (!hasEnoughDistance(pull_over_path)) { continue; } @@ -1187,13 +1186,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() thread_safe_data_.get_pull_over_path_candidates(), goal_candidates); // select pull over path which is safe against static objects and get it's goal - std::vector reference_path_directions; - for (const auto & point : getPreviousModuleOutput().path.points) { - reference_path_directions.push_back( - tf2::getYaw(tier4_autoware_utils::getPose(point).orientation)); - } const auto path_and_goal_opt = selectPullOverPath( - pull_over_path_candidates, goal_candidates, reference_path_directions, + pull_over_path_candidates, goal_candidates, parameters_->object_recognition_collision_check_hard_margins.back()); // update thread_safe_data_ @@ -1717,8 +1711,7 @@ bool GoalPlannerModule::checkObjectsCollision( return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); } -bool GoalPlannerModule::hasEnoughDistance( - const PullOverPath & pull_over_path, const std::vector & reference_path_directions) const +bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; @@ -1728,23 +1721,6 @@ bool GoalPlannerModule::hasEnoughDistance( // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. - - // reject outdated shift path on curved roads whose start pose direction is far from ego yaw - const double pull_over_path_start_direction = tf2::getYaw(pull_over_path.start_pose.orientation); - const bool isFollowableDirection = [&]() { - for (const auto reference_path_direction : reference_path_directions) { - if ( - std::fabs(tier4_autoware_utils::normalizeRadian( - pull_over_path_start_direction - reference_path_direction)) < M_PI / 2) { - return true; - } - } - return false; - }(); - if (!isFollowableDirection) { - return false; - } - 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); From 9c4d0b1e4dc1a3579c9ea959930288ce65a92219 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 10 May 2024 11:51:01 +0900 Subject: [PATCH 3/4] 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; From f7a10caf9b2fdc9118104eb9d548ff0e56fec1af Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 14 May 2024 15:50:11 +0900 Subject: [PATCH 4/4] reflect comment Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 f728a0d22d184..f6d4a58d0d687 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 @@ -852,11 +852,11 @@ std::optional> GoalPlannerModule::selectP 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, + const double prev_path_front_to_goal_dist = calcSignedArcLength( + prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position, goal_pose.position); const auto & long_tail_reference_path = [&]() { - if (prev_path_back_to_goal_dist > backward_length) { + if (prev_path_front_to_goal_dist > backward_length) { return prev_module_output_path; } // get road lanes which is at least backward_length[m] behind the goal @@ -865,7 +865,8 @@ std::optional> GoalPlannerModule::selectP /*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); + road_lanes, std::max(0.0, goal_pose_length - backward_length), + goal_pose_length + parameters_->forward_goal_search_length); }(); for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe