[JTC]: Abort goal on deactivate

[urfeex]

Currently, when deactivating the JTC once a trajectory is being executed leads to the following problems:

• The trajectory doesn't stop
• The trajectory doesn't end in a result, as it doesn't get stopped.
• The controller's shell gets flooded with [ros2_control_node-1] [WARN] [1737719032.541174555] [realtime_tools]: goal_handle attempted invalid transition from state EXECUTING with event CANCELED, at ./src/rcl_action/goal_handle.c:95 (See scaled_joint_trajectory_controller does not behave as expected when stop is pressed on the robot UniversalRobots/Universal_Robots_ROS2_Driver#1250)

I narrowed that down to the RealtimeServerGoalHandle calling gh_->canceled which is supposed to be called only, once the goal is already cancelling according to its documentation. The transition doesn't succeed and hence that message gets repeated in every cycle of the non-realtime-thread, and the action never terminates.

I've added a test for that event in this PR and a possible solution: Calling setAborted instead of setCanceled in the JTC when it is being deactivated does work. I also think, this is a valid feedback to the user, as the goal isn't preempted by the user but it is aborted due to the controller going down.

[christophfroehlich]

Thanks for this fix.
But the test is failing in every CI job, does it succeed locally?

setCanceled() is also called in preempt_active_goal(), should we also change it there?

[urfeex]

Thanks for this fix. But the test is failing in every CI job, does it succeed locally?

Yes, they were succeeding locally, I will have a look.

setCanceled() is also called in preempt_active_goal(), should we also change it there?

That seems reasonable, I will check that.

[urfeex]

setCanceled() is also called in preempt_active_goal(), should we also change it there?

Things seem to be a bit more complicated there.

From my understanding, we call preempt_active_goal() when a new goal is accepted. Inside there, we use setCanceled() to set request the target state canceled. This would be handled by the next timer callback for runNonRealtime() but this is never happening, as in the same action callback we reset the controller's timer.

If I add a manual call to runNonRealtime() as part of preempt_active_goal(), I get the expected output.

Trajectory replacement seems to be working fine and as expected, however the overwritten trajectory doesn't return until the overwriting trajectory finished. Then, it reports CANCELED. I noticed, that the succeed event is the first from the runNonRealtime() function that gets triggered, which is why this probably triggers the old action being canceled in the underlying (non-realtime) action-server.

For the time being I would suggest to restrict this PR to the deactivate problem, as this is related to UniversalRobots/Universal_Robots_ROS2_Driver#1250 and we create an issue for the replacement separately.

[codecov]

Codecov Report

Attention: Patch coverage is 86.48649% with 5 lines in your changes missing coverage. Please review.

Project coverage is 84.26%. Comparing base (694d6f1) to head (201386a).
Report is 1 commits behind head on master.

Files with missing lines	Patch %	Lines
...ectory_controller/test/test_trajectory_actions.cpp	86.11%	3 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1517      +/-   ##
==========================================
+ Coverage   84.19%   84.26%   +0.06%     
==========================================
  Files         123      123              
  Lines       11322    11359      +37     
  Branches      956      962       +6     
==========================================
+ Hits         9533     9572      +39     
+ Misses       1473     1470       -3     
- Partials      316      317       +1     

Flag	Coverage Δ
unittests	84.26% <86.48%> (+0.06%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...ory_controller/src/joint_trajectory_controller.cpp	84.89% <100.00%> (+1.00%)	⬆️
...ectory_controller/test/test_trajectory_actions.cpp	97.10% <86.11%> (-0.67%)	⬇️

... and 3 files with indirect coverage changes

[christophfroehlich]

Another reference https://design.ros2.org/articles/actions.html

• ABORTED - The goal was terminated by the action server without an external request.
• CANCELED - The goal was canceled after an external request from an action client.

indeed, abort is what we need.

Can you please open an issue regarding the preemption?

[saikishor]

LGTM as well.
Thanks for adding a proper test :)