From 049708220b0855e6418be0e3283eac62180dde5f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 1 Aug 2023 10:41:11 +0900 Subject: [PATCH] fix(obstacle_avoidance_planner): fix the bug of inserting stop point Signed-off-by: Takayuki Murooka --- planning/obstacle_avoidance_planner/src/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index a135ffdcfa4b7..da82fb14ea6d8 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -441,8 +441,8 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( if (first_outside_idx) { debug_data_ptr_->stop_pose_by_drivable_area = optimized_traj_points.at(*first_outside_idx).pose; const auto stop_idx = [&]() { - const auto dist = tier4_autoware_utils::calcDistance2d( - optimized_traj_points.at(0), optimized_traj_points.at(*first_outside_idx)); + const auto dist = + motion_utils::calcSignedArcLength(optimized_traj_points, 0, *first_outside_idx); const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_; const auto first_outside_idx_with_margin = motion_utils::insertTargetPoint(0, dist_with_margin, optimized_traj_points);