From d4b7454db59d3feb2eb6fe7b10ccc289ffb2498f Mon Sep 17 00:00:00 2001
From: danielsanchezaran <daniel.sanchez@tier4.jp>
Date: Wed, 6 Mar 2024 20:22:23 +0900
Subject: [PATCH] feat(route_handler): use start pose yaw  and not lanelet
 length to select start lanelet (#6550)

use start pose yaw  and not lanelet length to select start lanelet

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Signed-off-by: kaigohirao <kaigo.hirao@proxima-ai-tech.com>
---
 planning/route_handler/src/route_handler.cpp | 55 ++++++++------------
 1 file changed, 21 insertions(+), 34 deletions(-)

diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
index 68be0ac33e934..f7d300d496e13 100644
--- a/planning/route_handler/src/route_handler.cpp
+++ b/planning/route_handler/src/route_handler.cpp
@@ -2133,24 +2133,17 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
   lanelet::routing::LaneletPath shortest_path;
   bool is_route_found = false;
 
-  lanelet::routing::LaneletPath drivable_lane_path;
-  bool drivable_lane_path_found = false;
-  double shortest_path_length2d = std::numeric_limits<double>::max();
+  double smallest_angle_diff = std::numeric_limits<double>::max();
+  constexpr double yaw_threshold = M_PI / 2.0;
 
   for (const auto & st_llt : start_lanelets) {
     // check if the angle difference between start_checkpoint and start lanelet center line
     // orientation is in yaw_threshold range
-    double yaw_threshold = M_PI / 2.0;
-    bool is_proper_angle = false;
-    {
-      double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position);
-      double pose_yaw = tf2::getYaw(start_checkpoint.orientation);
-      double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
-
-      if (angle_diff <= std::abs(yaw_threshold)) {
-        is_proper_angle = true;
-      }
-    }
+    double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position);
+    double pose_yaw = tf2::getYaw(start_checkpoint.orientation);
+    double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
+
+    bool is_proper_angle = angle_diff <= std::abs(yaw_threshold);
 
     optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0);
     if (!optional_route || !is_proper_angle) {
@@ -2161,34 +2154,28 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
                    << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl
                    << " - start lane id: " << st_llt.id() << std::endl
                    << " - goal lane id: " << goal_lanelet.id() << std::endl);
-    } else {
-      is_route_found = true;
-
-      if (optional_route->length2d() < shortest_path_length2d) {
-        shortest_path_length2d = optional_route->length2d();
-        shortest_path = optional_route->shortestPath();
-        start_lanelet = st_llt;
-      }
+      continue;
+    }
+    is_route_found = true;
+    if (angle_diff < smallest_angle_diff) {
+      smallest_angle_diff = angle_diff;
+      shortest_path = optional_route->shortestPath();
+      start_lanelet = st_llt;
     }
   }
 
   if (is_route_found) {
     lanelet::routing::LaneletPath path;
-    if (consider_no_drivable_lanes) {
-      bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path);
-      if (shortest_path_has_no_drivable_lane) {
+    path = [&]() -> lanelet::routing::LaneletPath {
+      if (!consider_no_drivable_lanes) return shortest_path;
+      lanelet::routing::LaneletPath drivable_lane_path;
+      bool drivable_lane_path_found = false;
+      if (hasNoDrivableLaneInPath(shortest_path)) {
         drivable_lane_path_found =
           findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
       }
-
-      if (drivable_lane_path_found) {
-        path = drivable_lane_path;
-      } else {
-        path = shortest_path;
-      }
-    } else {
-      path = shortest_path;
-    }
+      return (drivable_lane_path_found) ? drivable_lane_path : shortest_path;
+    }();
 
     path_lanelets->reserve(path.size());
     for (const auto & llt : path) {