Skip to content

Commit

Permalink
[JTC] Cancel goal in on_deactivate (#962) (#970)
Browse files Browse the repository at this point in the history
  • Loading branch information
mergify[bot] authored Jan 11, 2024
1 parent 1f3ce24 commit cb1ddfb
Showing 1 changed file with 11 additions and 1 deletion.
12 changes: 11 additions & 1 deletion joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1032,6 +1032,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<FollowJTrajAction::Result>();
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_)
Expand Down Expand Up @@ -1543,7 +1554,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<FollowJTrajAction::Result>();
action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL);
action_res->set__error_string("Current goal cancelled due to new incoming action.");
Expand Down

0 comments on commit cb1ddfb

Please sign in to comment.