Sample trajectory based on the sum of periods instead of the absolute…

… time
ros-controls · Nov 27, 2024 · 0aa4a56 · 0aa4a56
1 parent 9b344c7
commit 0aa4a56
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 3 deletions.
diff --git a/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -127,6 +127,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   Params params_;
   rclcpp::Duration update_period_{0, 0};
 
+  rclcpp::Time traj_time_;
+
   trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
   /// Specify interpolation method. Default to splines.
   interpolation_methods::InterpolationMethod interpolation_method_{

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -196,15 +196,20 @@ controller_interface::return_type JointTrajectoryController::update(
         traj_external_point_ptr_->set_point_before_trajectory_msg(
           time, state_current_, joints_angle_wraparound_);
       }
+      traj_time_ = time;
+    }
+    else
+    {
+      traj_time_ += period;
     }
 
     // Sample expected state from the trajectory
     traj_external_point_ptr_->sample(
-      time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
+      traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
 
     // Sample setpoint for next control cycle
     const bool valid_point = traj_external_point_ptr_->sample(
-      time + update_period_, interpolation_method_, command_next_, start_segment_itr,
+      traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr,
       end_segment_itr, false);
 
     if (valid_point)
@@ -217,7 +222,7 @@ controller_interface::return_type JointTrajectoryController::update(
       // time_difference is
       // - negative until first point is reached
       // - counting from zero to time_from_start of next point
-      double time_difference = time.seconds() - segment_time_from_start.seconds();
+      double time_difference = traj_time_.seconds() - segment_time_from_start.seconds();
       bool tolerance_violated_while_moving = false;
       bool outside_goal_tolerance = false;
       bool within_goal_time = true;