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);