Improved execution time of "sample" for large trajectories

ros-controls · Nov 29, 2022 · d413417 · d413417
1 parent 2457680
commit d413417
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp
@@ -120,7 +120,8 @@ bool Trajectory::sample(
 
   // time_from_start + trajectory time is the expected arrival time of trajectory
   const auto last_idx = trajectory_msg_->points.size() - 1;
-  for (size_t i = 0; i < last_idx; ++i)
+  const auto prev_idx = start_segment_itr - begin();
+  for (size_t i = prev_idx; i < last_idx; ++i)
   {
     auto & point = trajectory_msg_->points[i];
     auto & next_point = trajectory_msg_->points[i + 1];