diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 13422eaac0..5903f1dbb8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -996,6 +996,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + rt_has_pending_goal_.writeFromNonRT(false); + auto action_res = std::make_shared(); + action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); + action_res->set__error_string("Current goal cancelled during deactivate transition."); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + for (size_t index = 0; index < dof_; ++index) { if (has_position_command_interface_) @@ -1466,7 +1477,6 @@ void JointTrajectoryController::preempt_active_goal() const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal) { - add_new_trajectory_msg(set_hold_position()); auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action.");