From df5cdfc8607601ba3448ca805fec5b369089e138 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 11 Jan 2024 15:01:12 +0100 Subject: [PATCH] [JTC] Cancel goal in on_deactivate (#962) (cherry picked from commit fdd6142c5ff9eeea2788ffa5529419a834c960b4) --- .../src/joint_trajectory_controller.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) 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.");