Fix index out of bounds in VTS for real

Mechanical-Advantage · Feb 12, 2024 · 802c31c · 802c31c
1 parent 4d1a258
commit 802c31c
Showing 1 changed file with 1 addition and 1 deletion.
diff --git 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));