Revert "feat(goal_planner): reject candidate path whose start pose direction is not aligned with ego path"

This reverts commit 0307cae.

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

.cspell.json

@@ -4,5 +4,5 @@
     "planning/behavior_velocity_intersection_module/scripts/**"
   ],
   "ignoreRegExpList": [],
-  "words": ["dltype", "tvmgen", "fromarray", "followable"]
+  "words": ["dltype", "tvmgen", "fromarray"]
 }

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<double> & 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<std::pair<PullOverPath, GoalCandidate>> selectPullOverPath(
     const std::vector<PullOverPath> & pull_over_path_candidates,
-    const GoalCandidates & goal_candidates, const std::vector<double> & reference_path_directions,
-    const double collision_check_margin) const;
+    const GoalCandidates & goal_candidates, const double collision_check_margin) const;
   std::vector<PullOverPath> sortPullOverPathCandidatesByGoalPriority(
     const std::vector<PullOverPath> & pull_over_path_candidates,
     const GoalCandidates & goal_candidates) const;

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -846,8 +846,7 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
 
 std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectPullOverPath(
   const std::vector<PullOverPath> & pull_over_path_candidates,
-  const GoalCandidates & goal_candidates, const std::vector<double> & 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<std::pair<PullOverPath, GoalCandidate>> 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<double> 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<double> & 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);