Improved execution time of "sample" for large trajectories

ros-controls · Dec 3, 2022 · e3ff776 · e3ff776
1 parent bc7be0e
commit e3ff776
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];