diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index b3e2f55742..66031db27f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -142,6 +142,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa Params params_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + rclcpp::Time last_commanded_time_; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7c494a63ab..cdf915619b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -318,6 +318,7 @@ controller_interface::return_type JointTrajectoryController::update( // store the previous command. Used in open-loop control mode last_commanded_state_ = state_desired_; + last_commanded_time_ = time; } if (active_goal)