feat(goal_planner): cut stop path to goal

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Jan 6, 2025 · e6b5233 · e6b5233
1 parent a32cae1
commit e6b5233
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1673,7 +1673,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath(
   const auto reference_path = std::invoke([&]() -> PathWithLaneId {
     const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length;
     const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
-    const double s_end = s_current + common_parameters.forward_path_length;
+    const double s_end = std::clamp(
+      lanelet::utils::getArcCoordinates(current_lanes, route_handler->getGoalPose()).length,
+      s_current + std::numeric_limits<double>::epsilon(),
+      s_current + common_parameters.forward_path_length);
     return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true);
   });