From d413417ded0b4a4206121aa52241cbc816602c4b Mon Sep 17 00:00:00 2001 From: Joshua Beck Date: Mon, 28 Nov 2022 11:55:58 -0500 Subject: [PATCH] Improved execution time of "sample" for large trajectories --- joint_trajectory_controller/src/trajectory.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index f28703efc9..92cc80079a 100644 --- 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];