keep track of previous start_idx internally

ros-controls · Dec 3, 2022 · 95ced33 · 95ced33
1 parent e3ff776
commit 95ced33
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
@@ -149,6 +149,7 @@ class Trajectory
   rclcpp::Time time_before_traj_msg_;
   trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_;
 
+  size_t prev_start_idx_;
   bool sampled_already_ = false;
 };
 

diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp
@@ -55,6 +55,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;
+  prev_start_idx_ = 0;
 }
 
 bool Trajectory::sample(
@@ -81,6 +82,7 @@ bool Trajectory::sample(
       trajectory_start_time_ = sample_time;
     }
 
+    prev_start_idx_ = 0;
     sampled_already_ = true;
   }
 
@@ -120,8 +122,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;
-  const auto prev_idx = start_segment_itr - begin();
-  for (size_t i = prev_idx; i < last_idx; ++i)
+  for (size_t i = prev_start_idx_; i < last_idx; ++i)
   {
     auto & point = trajectory_msg_->points[i];
     auto & next_point = trajectory_msg_->points[i + 1];
@@ -147,6 +148,7 @@ bool Trajectory::sample(
       }
       start_segment_itr = begin() + i;
       end_segment_itr = begin() + (i + 1);
+      prev_start_idx_ = i;
       return true;
     }
   }