From 802c31c3d11da7cc58a1bf9e3c4b43922d73f1a9 Mon Sep 17 00:00:00 2001
From: Cameron Earle <camearle20@me.com>
Date: Sun, 11 Feb 2024 19:01:53 -0500
Subject: [PATCH] Fix index out of bounds in VTS for real

---
 trajectory_native/src/trajectory_service.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/trajectory_native/src/trajectory_service.cpp b/trajectory_native/src/trajectory_service.cpp
index 7d77c455..5af1580f 100644
--- a/trajectory_native/src/trajectory_service.cpp
+++ b/trajectory_native/src/trajectory_service.cpp
@@ -186,7 +186,7 @@ class VehicleTrajectoryService final
                                               angle);
             }
 
-            segment_start_offset = last_waypoint_idx;
+            segment_start_offset += last_waypoint_idx + 1;
         }
 
         builder.ControlIntervalCounts(std::move(control_intervals));