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));