Verify correct sample index in tests

ros-controls · Sep 25, 2024 · 339e36c · 339e36c
1 parent cbf99af
commit 339e36c
Show file tree

Hide file tree

Showing 2 changed files with 123 additions and 0 deletions.
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
@@ -153,6 +153,14 @@ class Trajectory
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool is_sampled_already() const { return sampled_already_; }
 
+  /// Get the index of the segment start returned by the last \p sample() operation.
+  /**
+   * As the trajectory is only accessed at monotonically increasing sampling times, this index is
+   * used to speed up the selection of relevant trajectory points.
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  size_t last_sample_index() const { return last_sample_idx_; }
+
 private:
   void deduce_from_derivatives(
     trajectory_msgs::msg::JointTrajectoryPoint & first_state,