Wait recovery isn't preemable from a new navigation request

[SteveMacenski]

The wait command speeds the action server thread so its unable to wake up and exit when a navigation request is preempted. We should update that with a while loop and check every 100ms if it should exit.

[gimait]

I've done a small fix in the onCycleUpdate of the Wait plugin so that the function is not blocked by the sleep of rclcpp::sleep_for. I thought this would make the fix because I assumed that the recoveries server would react to a preemption, but it looks like the onCycleUpdates are still called even if a new navigation request is sent. Is this a problem on the behaviour tree, or the recovery server?

[SteveMacenski]

I cannot tell without seeing your code.

If you've changed to something like

onCycleUpdate()
{
  sleep_for(100ms);
  if (done) {return COMPLETE;}
  return RUNNING;
}

then you should check if the preempt is called for the recoveries, and then look back into the behavior tree.

[gimait]

Sorry, my code is in this commit: b881af3
It's very much like you said. I'll look into the recovery server then.

[SteveMacenski]

Look over your function again, it doesnt make sense. Why are you sleeping for 100ms after finishing the behavior?

[gimait]

Right, that should probably go before return running.

[SteveMacenski]

The recovery behaviors are throttled already at a rate, you can probably just make use of that and check the time stamp. I dont think you actually need any sleeps at all in the implementation.

Though again you should check that we actually process preemptions and cancelations for the recoveries.

[gimait]

The recovery behaviors are throttled already at a rate, you can probably just make use of that and check the time stamp. I dont think you actually need any sleeps at all in the implementation.

Sure, I only put the sleep there to keep the wait action waiting for a bit, but it makes sense to let the node handle the update rate instead of doing it in the plugin.

Though again you should check that we actually process preemptions and cancelations for the recoveries.

Yes this seems to be the problem. At the moment the bt_navigator is calling accept_pending_goal() in the action_server_ when a preemption is requested. This function forces an update in the goal when there is a preemption, but other nodes looking at the action server are not necessarily notified of the preemption (as currently they are only looking at the result of is_preempt_requested() in the action server). In this case, we have a kind of a race condition:

The action server recieves a new goal, all nodes managed by it look in their respective loops if there is a preemption and if the bt_navigator gets there first, it accepts the new goal, making the preempt_requested_ variable false before other nodes can notice the preemption.

Could this be the issue? And in that case, should we change the behaviour of the action server to make it notify all nodes about the preemption? Or should we call halt_all_actions when there is a preemption?

[SteveMacenski]

Well I think the controller is updated by just getting a new path - its being constantly preempted. The planner also isn't getting preempted in that case then which is also not good. It seems like in that preemption we need to reset the state of the BT softly (e.g. don't stop anything just give new commands). The main processes for that I think are the recoveries.

[gimait]

Okay, I'll try to do manage it from the behaviour tree then.

[SteveMacenski]

@crdelsey any thoughts on this? We're trying to preempt action servers when the navigation action itself is preempted or cancelled (e.g. controller or recoveries could still be going and aren't cancelled) -- wait, the controller must be cancelled when navigation is @gimait because otherwise the robot could keep moving. Lets find where that cancellation is and make it analogous for recovery.

[gimait]

The thing is, the cancellation and preemption are happening on different calls. When I cancel the movement of the robot, all nodes stop, no problem. Because the preemption is handled with the preempt_requested_ variable, which is set to true or false depending on the state of the action server, we have a different problem.

[SteveMacenski]

Cancellation right now does not stop the wait recovery (is it fixed for you by your update?). That by itself is a useful update.

On preemption of the navigation command itself, I'm wondering about the best behavior for:

• Controller: I wouldn't want to "stop" it since it may be a few millisecond until a plan is had to give to it. Preemption should not stop the robot ideally.
• Planner: I would want preemption of the navigation command to preempt the planner. Right now the planner is very fast and has no preemption logic, but that might not always be true and we can make the planner handle preemptions.
• Recovery: All recoveries should be preempted on a navigation preemption.

Thoughts?

I said preemption above in the ticket but what I really meant was cancellation. If your fix resolves that, submit a PR and lets get that in. We can discuss the preemption stuff as well, but that's likely a little larger of a task (as you no doubt have found)

[gimait]

Cancellation right now does not stop the wait recovery

Yes, my current changes fix that. I'll take out the sleep for 100ms and submit a pr.

About the preemption, I think the key for me to find a solution would be to understand all the communication channels that exist between the bt navigator and the different servers. I think the navigator should be the one calling the preemption on the servers, but I am not sure how to do that.

The different servers have already a defined behaviour for the preemption as far as I can see, the problem at least with the recovery, is that this is never (or very rarely) triggered.

[SteveMacenski]

I think the only way the communication happens is in the action and service interfaces that the BT nodes derive from.

[gimait]

First, sorry about all the git mess I made, I think I got a bit confused this morning.

About this issue, I finally managed to understand a bit of how the system works with the behaviour tree and the different action servers, and I got to the conclusion that:
The reason why the wait recovery was not preemable from a new navigation request is that the recovery server does not recieve any new goal when a new navigation goal is set.
Therefore, thetree action nodes should be notified if there is a preemption accepted, so they can trigger the appropiate actions.

I made a pull request with some changes that allow the recoveries server to accept preemptions and make the tree action nodes stop the spin and wait recoveries when a new navigation goal is sent to the BtNavigator.

[SteveMacenski]

@gimait this has been a real adventure.

I'm debating on what our next steps should be.

Davide made some diagrams in the ticket showing how we could reformat the planner to replan immediately on a new goal request. That doesn't help us if the planner process is currently running (its up to the planner implementation to then handle that preemption, which currently the nav2_planner server has no while loop to check). Also doesn't help us if the recoveries are currently running. The recoveries on the other hand do have a while loop in them like the controller does.

Perhaps we implement a isNewGoal BT node like Davide suggests (very simple) and we place it under a control flow node before the recovery action (diagram above shows planner, ignore it, we'll do the opposite control flow node). As the recoveries are running, before reticking the recovery node, it will look if there's a new goal, if there is, that control flow node will terminate causing a halt to be sent to all the children, stopping the recoveries.

So that would take care of preempting the recoveries. The planner isn't covered, but currently the planner also isn't cancellable since there's no while loop either.

So to recap, having these IsNewGoal BTs, we can cause the planner to react faster to new goals on a preemption request and we can stop recoveries in progress. We cannot stop planning on preemption, but we also can't on a cancellation or halt right now anyway, so we can ... ignore that?

What do you think?

The only asterisk I see on all that is how to maintain state on IsNewGoal. Imagine we have (by this example) 3 instances of the IsNewGoal. If a new goal is set on the blackboard and we throw a boolean saying "there's a new goal", how do we set that to false on the next iteration so that in the current iteration, all 3 of them see it if their subtrees are called. We may need to find where the master BT loop is (e.g. where is the BT being ticked at 10 hz from) and after each iteration, if that flag is true, setting it back to false. That could do the trick. Or, we could have a getNewGoal BT node in the root of the BT which itself checks for new goal requests and then if it sees one, sets the flag. On the next tick, if the flag it set is still true, it sets back to false. That logic right now lives in the BT navigator in the initializeGoalPose() method called in on_loop. I'm much prefer as much as possible as BT primitives vs hardcoding in BT navigator, so I think moving that into a BT node would be a good design choice.

Edit: We can use the on_loop lambda to set the flag back to false: https://github.com/ros-planning/navigation2/blob/master/nav2_bt_navigator/src/bt_navigator.cpp#L219

[crdelsey]

That all sounds reasonable to me. If we want to terminate the planner, I think a relatively straightforward approach would be to make a subclass of simple_action_server, where we

• spawn a thread to do the planning instead of using std::async
• terminate the thread if a cancel is received.

We'd just need to inspect the code to ensure there are no memory leaks. If there are memory leaks that are hard to fix, an alternate approach would be to let the old thread continue to run in the background, drop the priority and start a new thread with the latest goal.

[SteveMacenski]

We have a PR implementing this for preemption of the recoveries. The planner still doesn't matter since the while loop of planning itself is inside of NavFn and not easy to expose. Given how fast it is, I think that's OK. I think the equation changes when we talk about planners > 100ms needing a way to check and preempt.