JTC Cancels original and new trajectory when performing trajectory replacement with RT kernel

[hunterhuth]

Describe the bug
Our application builds a trajectory in real time by appending new points to the end of the trajectory. New points are added every 1.5 seconds, and send over the action interface to the joint trajectory controller. When using a standard kernel (not real-time), everything works as expected, a goal cancelled and a goal accepted message is sent to the client every 1.5 seconds. However, with a RT kernel, the JTC will cancel the old trajectory and the new trajectory. This causes the robot to stop moving until it receives a new trajectory 1.5 seconds later.

To Reproduce
Steps to reproduce the behavior:

1. Use a rt kernel
2. send a new trajectory every 1.5 seconds

Expected behavior
The controller will cancel 1 trajectory and accept 1 new trajectory every time a new trajectory is sent.

Screenshots

Figure 1. Client side output

Figure 2. Server side ouput

The lines of interest are between times 1722870436.911681030 and 1722870441.413748510.

Environment (please complete the following information):

• OS: 5.15.0-1052-realtime
• Version: Humble

[christophfroehlich]

Hi and thanks for reporting the issue. Could you maybe create a minimum example to reproduce your issue? You could choose something from the ros2_control_demos example repository and write a simple program to repeatedly add new waypoints.

[hunterhuth]

bug_example.zip
I attached two files. The first is an adaptation of the ros_control_example_1, that uses a joint trajectory controller. The seconds is a ros package that will send trajectory updates containing position and velocity information every 1.5 seconds. The terminal commands to run the programs are listed below.

step 1) ros2 launch ros2_control_demo_example_1 rrbot.launch.py
step 2) ros2 run trajectory_replacement trajectory_replacement

[christophfroehlich]

have you tried to reproduce the issue with a simple action server, without ros2_control being involved? Then we could be sure that your problem is located in ros2_control, or if it's somewhere upstream.

[hunterhuth]

I can do that, but I do not believe it will have the problem because the minimal action server does not preempt the current goal when a new one is received. However, the joint trajectory controller preempts the active goal when it accepts it. It is also worth noting that I have not observed this problem when trajectories are sent with only position values and not velocity values.

[christophfroehlich]

I see. (And it seems that there is no native preemption support in the design of ROS 2 yet at all.)

This might be an issue from here

ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp

Lines 1482 to 1493 in fa301f4

	void JointTrajectoryController::preempt_active_goal()
	{
	const auto active_goal = *rt_active_goal_.readFromNonRT();
	if (active_goal)
	{
	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.");
	active_goal->setCanceled(action_res);
	rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
	}
	}

or the RealtimeServerGoalHandle class itself.
Can you run colcon test on the realtime_tools? If they succeed, maybe one can adapt the tests here to run into that issue.

The problem is that we can't test the RT kernel within our CI, and I personally don't have access to any machine with rt kernel currently.

[hunterhuth]

On the RT kernel, all tests passed, so I will look into making a test case for the issue we see on a real-time kernel.

However, I need to understand something first. The issue is comes from the preemption when new goals are received and accepted. The current tests only use the minimal action server/client that do not perform this function. Do these tests need to use the Joint Trajectory Controller action server? Should the tests that test this case be for the Joint Trajectory Controller Instead? I am not sure if this problem is from the realtime tools package or how it is used in the Joint Trajectory controller.

[christophfroehlich]

I am not sure if this problem is from the realtime tools package or how it is used in the Joint Trajectory controller.

That's exactly the point we have to find out. in JTC we have the RealtimeGoalHandle inside a RealtimeBuffer, it can be either of those or the combination, or the usage in JTC.

[hunterhuth]

I haven't had a chance to create the test cases yet, but I have a theory about a race condition causing these errors. When preempting an active goal, it calls the setCanceled for the current rt_active_goal_ and writes a new RealtimeGoalHandlePtr() into the buffer. When returning to the goal accepted callback a new RealtimeGoalHandlePtr is created with the new trajectory and written to the buffer. I think the RT update function is reading from the buffer between when the preempt function writes to the buffer and when the goal_accepted_callback writes to the buffer.

This would explain why the server side output when this error occurs shows the "Goal Reached, Success!" and sends the goal canceled to the client twice.

[christophfroehlich]

This sounds like a possible reason.