[jtc] Improve trajectory sampling efficiency (#1297) (#1357)

---------

Co-authored-by: Christoph Fröhlich <[email protected]>
Co-authored-by: Bence Magyar <[email protected]>
(cherry picked from commit b0391e2)

Co-authored-by: RobertWilbrandt <[email protected]>

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -69,6 +69,9 @@ class Trajectory
    * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to
    * be reached at the time defined in the segment.
    *
+   * This function assumes that sampling is only done at monotonically increasing \p sample_time
+   * for any trajectory.
+   *
    * Specific case returns for start_segment_itr and end_segment_itr:
    * - Sampling before the trajectory start:
    *   start_segment_itr = begin(), end_segment_itr = begin()
@@ -85,9 +88,12 @@ class Trajectory
    *
    * \param[in] sample_time Time at which trajectory will be sampled.
    * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at
-   * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr
-   * Iterator to the start segment for given \p sample_time. See description above. \param[out]
-   * end_segment_itr Iterator to the end segment for given \p sample_time. See description above.
+   *      all.
+   * \param[out] output_state Calculated new at \p sample_time.
+   * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See
+   *      description above.
+   * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See
+   *      description above.
    */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool sample(
@@ -147,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,
@@ -160,6 +174,7 @@ class Trajectory
   trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_;
 
   bool sampled_already_ = false;
+  size_t last_sample_idx_ = 0;
 };
 
 /**

joint_trajectory_controller/src/trajectory.cpp

@@ -85,6 +85,7 @@ void Trajectory::update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> j
   trajectory_msg_ = joint_trajectory;
   trajectory_start_time_ = static_cast<rclcpp::Time>(joint_trajectory->header.stamp);
   sampled_already_ = false;
+  last_sample_idx_ = 0;
 }
 
 bool Trajectory::sample(
@@ -150,7 +151,7 @@ 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)
+  for (size_t i = last_sample_idx_; i < last_idx; ++i)
   {
     auto & point = trajectory_msg_->points[i];
     auto & next_point = trajectory_msg_->points[i + 1];
@@ -176,13 +177,15 @@ bool Trajectory::sample(
       }
       start_segment_itr = begin() + i;
       end_segment_itr = begin() + (i + 1);
+      last_sample_idx_ = i;
       return true;
     }
   }
 
   // whole animation has played out
   start_segment_itr = --end();
   end_segment_itr = end();
+  last_sample_idx_ = last_idx;
   output_state = (*start_segment_itr);
   // the trajectories in msg may have empty velocities/accel, so resize them
   if (output_state.velocities.empty())