From 00892a10702536fbcd071f9decc8953addddfd24 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 10 Jan 2025 16:55:14 +0900 Subject: [PATCH] feat(goal_planner): cut stop path to goal (#9829) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 96aeb5f5e4da8..87a6e28ff0007 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1671,7 +1671,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::epsilon(), + s_current + common_parameters.forward_path_length); return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); });