Handle a halted node differently than a preempted node

[krixkrix]

I am trying to see if I can migrate my ROS SMACH based sequencing engine to BehaviorTree.CPP.
However I don't find any concept of preemption here.

In SMACH I am using a lot of concurrent actions where an action A runs in parallel with action B and C. But A is the "master action" and B and C should maximum run as long as A.
So when A succeeds, I will preempt B and C, and they will stop their task with a preempted status. But the complete parallel task [A,B,C] will return succeeded.
If any of A,B or C fails, the parallel node should fail as always.

Can BehaviourTree.CPP support something like that?
How would I know if a preempt request was executed on a subtree or it just failed?

[facontidavide]

Indeed, the logic is that you may halt a running Async node, but not preempt it.

"Preempting" is particularly hard for AsyncActionNode, but quite easy for CoroActionNode; unfortunately, I am pretty sure that no one use the latter, because people are lazy.

I need to understand more about your use case.

• What is in your opinion a good definition or "preempting". Can you give me a practical example?
• So you want A to me mutually exclusive with B and C, is that right? Can't you use a ReacvtiveFallback or something similar?

[krixkrix]

SMACH action states will in general report an outcome either SUCCEEDED, FAILED or PREEMPTED when the state is exited.
And preempted will mean that the action was explicitly cancelled, not necessarily because of a failure.
I am using these outcomes heavily in my current code.

Use case:
An action A controls a movement from position X1 to X2.
An action B controls a light.
I want the light to be turned on as long as the movement is active.

The requirements for this scenario are:

• If the movement A succeeds, the light action B is cancelled (preempted), and the behavior subtree as a whole succeeds.
• If A fails, B is cancelled (preempted), and the behavior subtree as a whole fails.
• If B fails, A is cancelled (preempted), and the behavior subtree as a whole fails.

I even want to handle the tricky concurrency cases, where A succeeds, B is requested to cancel, but B reports a failure before it is cancelled, so the behavior subtree as a whole should fail.

In general the action B may be more complicated than turning on a light.
But the point is that it is a task that takes some time and may fail.

I am trying to understand how/if a behavior tree can be used for the above. It seems like "Halt" is equivalent to a "preempt" (or "cancel") request, except the behaviour will return "failed" regardless of it has been "halted" or it failed while executing its task.

[ramilmsh]

I realize I am late to the party, but I'd like to propose that this behavior may be modeled with a reactive sequence. Where your "main" action is the last one and everything else is an "async condition". The idea is you have "fast" async nodes ( fast, but not instantaneous, {execution time} >> 1/tick_rate, like turn on lights, store data, check stuff... ) that return running when initiated and success, as soon as an initial check succeeded, but continue executing checks in the background (then, should it fail completely - setStatus( FAILURE ), or if it has been temporarily impeded - setStatus( RUNNING ); both will preempt the main action, but Failure will also fail the whole sequence) and "slow" action in the end, which will be running until it fails, succeeds or is halted by one of the conditions.

@facontidavide, I would like your opinion on this, as this feels like a bit of a crutch.

NOTE: you should probably use CoroActionNode, instead of plain AsyncActionNode, though.

[SteveMacenski]

@facontidavide this is actually something we need in the navigation2 stack as well. The primary use-case is when a navigation command is preempted, we want the controller action node to continue to execute so there's a smooth transition between goals. But the recovery behaviors (e.g. spin, backup, whatever) should be immediately stopped on preemption to make way for the new goal. The planner should also be halted and restarted with the new goal if its still running (functionally this may not be important since planning is typically fast, but for completion's sake).

Then the question comes up with custom control flow / decorator nodes how to treat them in the preemption case. Having some BT node virtual method like halt for preemption would have value so we can implement preemption state updates for all the node types.

For practical considerations, if we have some method we can call, in ROS2 we're running actions so we have a client and a goal so we can async_cancel_goal() the recoveries, just pass for the controller, and probably mess with the internal state of control flow nodes. The coro action nodes help to kill the coroutine, but if we have a handle to the server, we can do that through ROS2 interfaces for the remote server. The control flow nodes could have a flag set and next time its ticked we return some value set by it and bypass the control flow logic from preemption. Or if its possible in the lower level of the tree to set a status and just reset the control flow node to some state post-preemption without having to wait for another tick.

[facontidavide]

Hi @SteveMacenski,

I was waiting for your message with a sense of dread and imminent doom, since I saw the discussion on the Github of Navigation2...

So, let's see if I understand this right or not:

when a navigation command is preempted, we want the controller action node to continue to execute so there's a smooth transition between goals. But the recovery behaviors (e.g. spin, backup, whatever) should be immediately stopped on preemption to make way for the new goal.

1. At the level of the entire tree, where is the preemption signal inserted? The root?
2. Do you want this preemption signal to be propagated to the entire tree?
3. How is decided if a particular Action halt() itself when a preemption signal is received? Can we create a DecoratorNode that converts a preemption signal into a halting signal?

Then the question comes up with custom control flow / decorator nodes how to treat them in the preemption case ... so we can implement preemption state updates for all the node types.

This is indeed what I am terribly worried about, I think you underestimate the combinatory explosion of potential bugs and corner cases (but maybe I am wrong).

The control flow nodes could have a flag set and next time its ticked we return some value set by it and bypass the control flow logic from preemption.

This is what I would call "your implementation details".

Or if its possible in the lower level of the tree to set a status and just reset the control flow node to some state post-preemption without having to wait for another tick.

As long you don't ask me to add NodeStatus::PREEMPTED or similar... that can be done.

The only way to have a sane discussion about this is to implement some (dummy code / unit test) without any ROS/ROS2 to validate the concept, API, usability and potential corner cases.

[SteveMacenski]

I was waiting for your message with a sense of dread and imminent doom, since I saw the discussion on the Github of Navigation2...

Oops. I never mean to be the harbinger of doom (or do I....).

Before going into the bullets, lets just talk thematically. I hate getting bogged in technical details before getting clear idea of what's happening. Also the answers to these questions are fluid since we're looking for a certain result but I believe there are many ways of accomplishing it.

We have a robot going around a (I don't know, what are you working on these days...) parking lot. While searching for a spot to park, a new one opens up right in front of you and you would like to preempt the navigation request to the other, less optimal parking spot 2 rows over you were going to.

When I preempt, just like in ROS1 navigation, I really don't want to disrupt the flow of the robot (e.g. stop the robot dead in its tracks to then replan and go to the goal). So, I believe all I want to keep moving is the controller, at least for some short duration. The planner, if planning, and recoveries, if recovering, I want to stop like its a cancel command to call anew.

Now, because we're not using a state machine but the BT, there's the added complexity of the control flow nodes (and custom ones) about how to manage the state of these things if we're not fully resetting the tree (or are we?) but the controller should still spin on its last command until a new one comes in.

Another open question is what to do if there are multiple controllers in the tree. One's still spinning but if the preemption comes in and the conditions are right, the other controller could be called and now you have 2 controllers fighting to send cmd_vels. Lets ignore that one for right now (unless you have a solution). My solution is "don't do that" or if you do that, make them both in the same controller_server and I have logic in there to make sure only one runs at a time (old one is cancelled). Either way, not your or BT.CPP's problem.

So onto the details.

At the level of the entire tree, where is the preemption signal inserted? The root?

Sure, lets say so. Right now, the action server that exposes the BT has a preemption handler that will set a new goal for the planner to use and that's it. The planner spins at 1hz so the next time around it'll use the new goal and make a path, then the controller will take the new path on the blackboard and follow it.

https://github.com/ros-planning/navigation2/blob/master/nav2_bt_navigator/src/bt_navigator.cpp#L220

I think what makes most sense is for the action server exposing the BT that gets the new navigation request should have some preempt analog to halt and methods in the implementations of nodes handle it in some way across the tree (e.g. planner triggers immediate replan, controller does nothing, recoveries immediately exit, control flow and decorator nodes do.... something?).

Do you want this preemption signal to be propagated to the entire tree?

I believe so, so that when preemption for navigation task happens, we can tell the planner to replan and the recoveries to stop. What triggered this work is that when a new navigation goal comes in from preemption, the recoveries (which can be long running like spinning or waiting) don't end blocking the new request until complete. This is not great. I assume the way we need to do this is propogating through the tree. Open for alternatives though.

How is decided if a particular Action halt() itself when a preemption signal is received? Can we create a DecoratorNode that converts a preemption signal into a halting signal?

Not sure I understand this point. Are you asking how a halt() could know if it was called due to a preemption? I'm not sure it could. I think there would need to be another method other than halt(). My guess is for many nodes (and by default) the preemption method would just halt. With the exception of the controller. For non-Action nodes, I think there would need to be some state updated in this preemption method. For the goal of the preempted recovery X returning FAILURE that another control flow recovery Y isn't then triggered and returns up the tree. Unless we can just set the state to idle so when this exits it won't tick another child. I'm not sure under the hood of BT.CPP how that is handled.

This is indeed what I am terribly worried about, I think you underestimate the combinatory explosion of potential bugs and corner cases (but maybe I am wrong).

Oh, no. I agree with you. This sounds like a total complete pain. I think the only way this doesn't become a total pain is if in the preemption we can set the state to IDLE or something and if the logic in all the control flow nodes recognizes that if its IDLE and returned from a preemption child it doesn't keep on going and just returns some status. Or optionally, the recursive visitor (by the way, great idea, lambdas make the idea of visitors in C++ way more bearable, its used alot in open_karto for finding neighbors and its way less clear there the design) has an option to apply from leaf -> root rather than root -> leaf such that the leaf action nodes have their preempt() called, then their control flow nodes have their preempt() called (which could set a flag like IDLE) and so forth. This way there's no opportunity for it to be reticked by the next one because its been handled in order. The open question for that is: what should be the state of the control flow node of the still-running processes after the preemption? E.g. the controller BT node, should its control flow node know anything about its current state? Or should it just be IDLE and the tree is none the wiser that there's still nodes' servers still processing. Alternatively as the on_preempt() of still running children, we keep the state the same?

I know that was a mouthful.

I don't necessarily think that a new status type is required. Honestly, that one didnt even cross my mind but I think that could help with some of the flag setting on the blackboard ;-). I would be interested in your thoughts if this is even the right way to think about it. This seems like an important use-case. While BT.CPP isn't ROS related, scrolling through the stars and forks, there's an awful lot of known ROS users...

Huh, that gets me thinking, maybe some clever use of the blackboard could help... it would definitely be hacky...

---

PS thanks for https://github.com/PointCloudLibrary/pcl/pull/3853/files. I would really love to do more work in PCL, but its super unclear to me who the players are. The state of the website and forums makes me think that PCL isn't being as used as much anymore outside of ROS-land (what are they using instead? Spinning their own?) and I was never sure how to "jump in" to that community. There's also a huge number of outstanding tickets and PRs that dont seem to be moving. I would really love if an organization took this over and brought the project back to life. Its a pretty critical project in my mind.

Also generally thanks for making this project too. Website, tutorials, clean code, I can't imagine convincing someone to give me the time to make something like this mostly by myself. I can usually swing it when its a higher-level application thing like navigation. Thanks a ton!

[ruffsl]

I would really love to do more work in PCL, but its super unclear to me who the players are.

Might want to reach out to the organizers behind the GSoC for PCL to lean about the maintainers:

• [Announcement] GSoC 2021 PointCloudLibrary/pcl#4008
• https://summerofcode.withgoogle.com/organizations/5248497919459328/

The chat link points to the PCL Discord channel where I guess the community is congregates.

[facontidavide]

Wow, this will take a while to answer.
Spoiler alert, we need dummy code to play around with (non ROS, just a printf in the callbacks).

Rather than answering all your questions at once, we need to approach this step by step!

Reading the description of your use case (and not thinking about navigation2 at all), I would do something like this

As you may see, at each tick (no problem running at 100 Hz) we are checking if there is a new goal.

Let's see al the cases:

• First command: planning is executed and path follower (controller) started
• Following loops: we check if there is a new path, since there is not, we jump to follower.
• if a new goal is sent, there is a replanning, but follower is never stopped
• note: if plan_trajectory fails, the entire branch fails

Isn't it what you had in mind ? As you can see, no need to introduce preemption, the grammar of reactive nodes cover this use case I dare to say "elegantly" 😛

[SteveMacenski]

This wouldn't allow for replanning of the same goal over time (e.g. every 1 s replan, or on a controller failure replan, or I suppose every N meters replan, though that's a new one). though I agree with your philosophy of "more BT primitive nodes, less blackboard background logic".

[facontidavide]

Ok, I missed the 1 Hz thing here. Easy-peasy:

Note that all the subtree with the green nodes on the left can be just a single node. Fine-grained Conditions/Node are good for reusability, but there maybe you don't care about that.

As you can see, there is always a way to express the logic.

I probably do something like this:

The node isReplanNeeded can check whatever you want, new goal, time elapsed, etc...

if_first_than_second can be implemented in 1 hour.

As you can see, Reactive Nodes is what you should consider for... well, reactive behaviours.

[SteveMacenski]

Sure, but I think we're missing the point a little, the planner isn't really a huge concern to me (though this does help, @crdelsey how do those trees look to you for a replacement?). The bigger issue is preempting running nodes. This is a good example of how to call a node. I need to be able to preempt that PlanTrajectory node if it takes, say, 10 seconds to run and a new request came in to preempt it 2 seconds into planning. Also we have recoveries for the planner too "contextual recoveries", but I think that's just implementing a control flow that keeps marching until success (so isReplanningNeeded would be inverted).

What actually triggered this work was recoveries, which are long running, and preemption. If my robot is waiting 5 seconds or spinning 360 or whatever and a preemption comes in to go to another position, I need that long running node stopped and returned to start planning / navigating / going through the tree to the new goal. However, I need the controller to continue doing what its doing so there's a smooth transition between task A and task B.

I essentially want to halt(), except some nodes, or set the state of running things. That's why we proposed an analog to halt() for preempt() so that each node can handle what it should do on a preemption request (planner, recoveries halt, controller continues, control flow... reset? exit? not sure there).

Can that be modeled through the BT?