From 07d0d01e0fc63ba064b3a94ed70299a699114307 Mon Sep 17 00:00:00 2001
From: Zhe Shen <lucidshenzhe@gmail.com>
Date: Wed, 15 May 2024 14:17:15 +0900
Subject: [PATCH] 2 points at least in the clipped trajectory

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>
---
 control/mpc_lateral_controller/src/mpc_utils.cpp | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp
index 01a81d9015b47..ab10e5c4e4368 100644
--- a/control/mpc_lateral_controller/src/mpc_utils.cpp
+++ b/control/mpc_lateral_controller/src/mpc_utils.cpp
@@ -473,12 +473,16 @@ MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const dou
   clipped_trajectory.push_back(trajectory.at(0));
 
   double current_length = 0.0;
-  for (size_t i = 1; i < trajectory.size(); ++i) {
-    current_length += calcDistance3d(trajectory, i, i - 1);
-    if (current_length > length) {
-      break;
-    }
-    clipped_trajectory.push_back(trajectory.at(i));
+  {  
+    size_t i = 1;
+    do {
+      clipped_trajectory.push_back(trajectory.at(i));
+      current_length += calcDistance3d(trajectory, i, i - 1);
+      if (current_length > length) {
+        break;
+      }
+      ++i;
+    } while (i < trajectory.size());
   }
 
   return clipped_trajectory;