fix(obsacle_stop_planner): maintain using of first_outside_idx in cas…

…e index with margin cannot be calculated

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>

planning/obstacle_avoidance_planner/src/node.cpp

@@ -449,17 +449,18 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea(
 
   if (enable_outside_drivable_area_stop_) {
     if (first_outside_idx) {
+      auto stop_idx = *first_outside_idx;
       const auto dist = tier4_autoware_utils::calcDistance2d(
-        optimized_traj_points.at(0), optimized_traj_points.at(*first_outside_idx));
-
+        optimized_traj_points.at(0), optimized_traj_points.at(stop_idx));
       const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_;
-      const auto stop_idx = motion_utils::insertStopPoint(dist_with_margin, optimized_traj_points);
-
-      if (stop_idx) {
-        publishVirtualWall(optimized_traj_points.at(*stop_idx).pose);
-        for (size_t i = *stop_idx; i < optimized_traj_points.size(); ++i) {
-          optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0;
-        }
+      const auto first_outside_idx_with_margin =
+        motion_utils::insertStopPoint(dist_with_margin, optimized_traj_points);
+      if (first_outside_idx_with_margin) {
+        stop_idx = *first_outside_idx_with_margin;
+      }
+      publishVirtualWall(optimized_traj_points.at(stop_idx).pose);
+      for (size_t i = stop_idx; i < optimized_traj_points.size(); ++i) {
+        optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0;
       }
     }
   }