[JTC] Action goal replacement doesn't cancel the old goal in time

[fmauch]

Describe the bug

When sending a new action goal to the JTC while there is a goal running, the replacement seems to be working fine, but the old action does not return until the new action finished.

As originally posted in #1517 (comment)

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.

This might be potentially related to #1241

To Reproduce
Steps to reproduce the behavior:

1. Launch a robot with the JTC running (can also be mock hw), e.g.
   ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e use_mock_hardware:=true initial_joint_controller:=joint_trajectory_controller robot_ip:=1234
2. Run an action on the JTC, e.g. by ros2 run ur_robot_driver example_move.py --ros-args -p controller_name:=joint_trajectory_controller
3. Once the action starts executing (the robot moves in RViz), run a second client in another shell with the same command
4. The robot executes the second trajectory. Once the trajectory of the second client successfully finished, the first client also returns telling that the action was canceled.

With the example move script it looks like this:

first client:

[INFO 2025-02-03 23:19:43.467] [jtc_client]: Waiting for action server on joint_trajectory_controller/follow_joint_trajectory
[INFO 2025-02-03 23:19:45.470] [jtc_client]: Executing trajectory traj0
[INFO 2025-02-03 23:19:55.979] [jtc_client]: Done with result: CANCELED
[ERROR 2025-02-03 23:19:55.980] [jtc_client]: Executing trajectory failed.

second client:

INFO 2025-02-03 23:19:47.978] [jtc_client]: Waiting for action server on joint_trajectory_controller/follow_joint_trajectory
[INFO 2025-02-03 23:19:47.978] [jtc_client]: Executing trajectory traj0
[INFO 2025-02-03 23:19:55.981] [jtc_client]: Done with result: SUCCEEDED

Expected behavior
The first client should report its trajectory as preempted / aborted (to be discussed) as soon as the second client starts executing its action.

Environment (please complete the following information):

• Tested on Rolling master at the time of writing
• For the reproduction test having the package ros-rolling-ur installed should be sufficient

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.