diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 2d03dbee69..4c4d0dfdb0 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -220,7 +220,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (traj_contr_) { - // set start time of trajectory to traj_contr_ + // switch RT buffer of traj_contr_ traj_contr_->start(); } } @@ -1038,6 +1038,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { cmd_timeout_ = 0.0; } + + // activate traj_contr_, e.g., update gains + if (traj_contr_ && traj_contr_->activate() == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation."); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; }