fix(obstacle_avoidance_planner): fix the bug of inserting stop point (autowarefoundation#4479)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

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);