From cdd08f815c3a840df30c9df9536e8a445dc737c9 Mon Sep 17 00:00:00 2001
From: Mehmet Dogru <42mehmetdogru42@gmail.com>
Date: Thu, 6 Mar 2025 07:22:41 +0200
Subject: [PATCH] fix(start_planner): use waypoints as centerline if available

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>
---
 .../start_planner_module.hpp                          |  2 +-
 .../src/start_planner_module.cpp                      | 11 +++++++----
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp
index 640220a44dfcc..e19d8bb78b32a 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp
@@ -230,7 +230,7 @@ class StartPlannerModule : public SceneModuleInterface
   bool receivedNewRoute() const;
 
   bool isModuleRunning() const;
-  bool isCurrentPoseOnMiddleOfTheRoad() const;
+  bool isCurrentPoseOnCenterline() const;
 
   /**
    * @brief Check if the ego vehicle is preventing the rear vehicle from passing through.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
index e973e7069d153..d3dda5fb7facb 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -342,12 +342,12 @@ bool StartPlannerModule::isExecutionRequested() const
   }
 
   // Return false and do not request execution if any of the following conditions are true:
-  // - The start pose is on the middle of the road.
+  // - The start pose is on the centerline or on "waypoints" (custom centerline)
   // - The vehicle has already arrived at the start position planner.
   // - The vehicle has reached the goal position.
   // - The vehicle is still moving.
   if (
-    isCurrentPoseOnMiddleOfTheRoad() || isCloseToOriginalStartPose() || hasArrivedAtGoal() ||
+    isCurrentPoseOnCenterline() || isCloseToOriginalStartPose() || hasArrivedAtGoal() ||
     isMoving()) {
     return false;
   }
@@ -367,12 +367,15 @@ bool StartPlannerModule::isModuleRunning() const
   return getCurrentStatus() == ModuleStatus::RUNNING;
 }
 
-bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const
+bool StartPlannerModule::isCurrentPoseOnCenterline() const
 {
+  const auto & lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
   const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
   const double lateral_distance_to_center_lane =
-    lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
+    lanelet::utils::getArcCoordinatesConsideringWaypoints(
+      current_lanes, current_pose, lanelet_map_ptr)
+      .distance;
 
   return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road;
 }