Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
hunterhuth opened this issue Aug 5, 2024 · 9 comments
Labels

Comments

@hunterhuth
Copy link

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
image
Figure 1. Client side output
image
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
@hunterhuth hunterhuth added the bug label Aug 5, 2024
@christophfroehlich
Copy link
Contributor

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
Copy link
Author

hunterhuth commented Aug 6, 2024

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
Copy link
Contributor

christophfroehlich commented Aug 7, 2024

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
Copy link
Author

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
Copy link
Contributor

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

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
Copy link
Author

hunterhuth commented Aug 8, 2024

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
Copy link
Contributor

christophfroehlich commented Aug 8, 2024

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
Copy link
Author

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
Copy link
Contributor

This sounds like a possible reason.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

No branches or pull requests

2 participants