fix(obstacle_avoidance_planner): using insertStopPoint instead of cur…

…rent logic

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

planning/obstacle_avoidance_planner/src/node.cpp

@@ -449,24 +449,18 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea(
 
   if (enable_outside_drivable_area_stop_) {
     if (first_outside_idx) {
-      size_t stop_idx = *first_outside_idx;
-      const auto op_target_point = motion_utils::calcLongitudinalOffsetPoint(
-        optimized_traj_points, optimized_traj_points.at(stop_idx).pose.position,
-        -1.0 * vehicle_stop_margin_outside_drivable_area_);
-      if (op_target_point) {
-        const auto target_point = op_target_point.get();
-        // confirm that target point doesn't overlap with the stop point outside drivable area
-        const auto dist =
-          tier4_autoware_utils::calcDistance2d(optimized_traj_points.at(stop_idx), target_point);
-        const double overlap_threshold = 1e-3;
-        if (dist > overlap_threshold) {
-          stop_idx = motion_utils::findNearestSegmentIndex(optimized_traj_points, target_point);
+      const auto dist = tier4_autoware_utils::calcDistance2d(
+        optimized_traj_points.at(0), optimized_traj_points.at(*first_outside_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;
         }
       }
-      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;
-      }
     }
   }