fix(obstacle_avoidance_planner): fixing wrong code logic porting from…

… original PR Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>
autowarefoundation · Jul 11, 2023 · 13a6ff2 · 13a6ff2
1 parent eb0689d
commit 13a6ff2
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp
@@ -446,9 +446,9 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea(
   }();
 
   if (first_outside_idx) {
+    debug_data_ptr_->stop_pose_by_drivable_area = optimized_traj_points.at(*first_outside_idx).pose;
     for (size_t i = *first_outside_idx; i < optimized_traj_points.size(); ++i) {
       optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0;
-      debug_data_ptr_->stop_pose_by_drivable_area = optimized_traj_points.at(i).pose;
     }
   }