Add condition nodes for time and distance replanning

[naiveHobo]

Signed-off-by: Sarthak Mittal sarthakmittal2608@gmail.com

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	None
Primary OS tested on	Ubuntu 18.04
Robotic platform tested on	TurtleBot3 gazebo simulation

---

Description of contribution in a few bullet points

• Added three new condition nodes:

1. TimeExpired: returns SUCCESS after every predefined time period
2. DistanceTraveled: returns SUCCESS after every N meters distance traveled by the robot

Description of documentation updates required from your changes

• Add new condition nodes to https://navigation.ros.org/plugins/index.html

[shrijitsingh99]

• TimeExpired: returns SUCCESS after every predefined time period

Wouldn't seconds be a more intuitive parameter than Hz considering the name of the node?

[SteveMacenski]

Alright, let me get this straight because we're entering Confusionland from multiple ideas / PRs / tickets / people working on strongly related items at the same time. This is partially my fault, I shouldn't have brought up the other issues in Davide's ticket that Aitor is working on.

To be clear, what is this proposing? To my understanding, you're essentially saying

• Remove the rate controller (decorator)
• Invalidate your other PR with a distance controller (decorator)
• Replace with condition nodes for time / distance

Is that correct? If that is correct, do you think we should remove the existing decorators or just leave them but not use them in any BT?

The isNewGoal is something that @gimait is working on and we've been at that for the better part of a month down a number of rabbit holes (#1644) both self inflicted and wandering in the forest of all knowledge. I would like him to see that work through since he's spent alot of time and energy on this.

I think that covers the intent portion of this PR

---

The implementation portion of this PR also evokes some questions:

• How does the time / distance nodes interact? We're discussing that currently in the ticket about how to ensure that we're not replanning super often if the time is set to say once every 10 seconds and the distance is 1 meter, what happens when those both happen in a short period of time. Or alternatively, do we want to require one or both of these conditions to be true? This is an open question that should be discussed in the ticket before we submit a PR (oh well, too late)
• There's some talk of a speed decorator / condition as well. I would like to see that implemented, even if not used immediately. I see that as a really good idea and something we should have in our war chest.
• We will likely want to update or remove existing BT XMLs as a result of this change. We should review existing ones and figure out which we keep but update and which may be now no longer required.

[SteveMacenski]

Also the usual "update readmes, website docs, etc"

[naiveHobo]

The isNewGoal is something that @gimait is working on and we've been at that for the better part of a month down a number of rabbit holes (#1644) both self inflicted and wandering in the forest of all knowledge. I would like him to see that work through since he's spent alot of time and energy on this.

Ah, I'm sorry I missed the isNewGoal part of this PR. I assumed @gimait's work was addressing the preemption issues in the action nodes and recovery node. Should have gone deeper before jumping into it.

To be clear, what is this proposing? To my understanding, you're essentially saying

• Remove the rate controller (decorator)
• Invalidate your other PR with a distance controller (decorator)
• Replace with condition nodes for time / distance

I'd say the decorator nodes should not be removed. They propose a completely different functionality in that they can be used to control the rate of an entire branch in a BT. While throttling is possible with these condition nodes, it would involve the use of other nodes to offer that functionality. A decorator + condition nodes for these scenarios offers the most flexibility I feel.

How does the time / distance nodes interact? We're discussing that currently in the ticket about how to ensure that we're not replanning super often if the time is set to say once every 10 seconds and the distance is 1 meter, what happens when those both happen in a short period of time.

I believe we can work on the time / distance node interaction only if they are implemented as condition nodes. As decorators, I'm not sure if the two nodes can interact.

[SteveMacenski]

OK, lets wait on his new PR for is new goal.

I believe we can work on the time / distance node interaction only if they are implemented as condition nodes.

Lets follow up in the ticket about how to make these work together.

I agree, we should keep all the BT nodes around even if we don't use them. It would be nice to have a large library of primitives. There's probably a bunch of other simple nodes we could add that would be useful.

[naiveHobo]

To conclude, I'm going to remove the isNewGoal condition node and wait for @gimait's new PR. I believe we should still go ahead with the time / distance condition nodes and the BT derived using these nodes since this is inherently superior than using decorators if we want to combine the time / distance behavior.

Once @gimait's PR is in, we can just add the isNewGoal condition node to the existing BT similar to what's been done here.

[SteveMacenski]

We should still discuss in the other ticket about how to best combine them or if we even should. I agree that either way we should add these BT condition nodes.

[gimait]

Alright, I just pushed my goal updated node, I think we can make it work with the condition nodes added in this pr for distance and time conditions in this new subtree to control the replanning.

I think we should implement both decorators and control nodes with these key functionalities. The reasons for this are:

• Decorators can only do one action on the node child, meaning that they are less flexible. This makes them impossible to use for both cancelling the recoveries and triggering the planning.
• The condition nodes cannot check on the status of the nodes we are interested in checking. For the recoveries, this does not matter, however we might be interested in waiting if the planner has not finished yet when a TimeExpired node returns success (like the rate controller does).

How does the time / distance nodes interact? We're discussing that currently in the ticket about how to ensure that we're not replanning super often if the time is set to say once every 10 seconds and the distance is 1 meter, what happens when those both happen in a short period of time.

With the Fallback control node, all the new condition nodes would be halted if any returns Success.
Thanks to this, it won't happen that time and distance are triggered (almost) at the same time. If one of them is triggered, all the others are reset.

Is this right @naiveHobo? (Awesome job btw)

[naiveHobo]

With the Fallback control node, all the new condition nodes would be halted if any returns Success.
Thanks to this, it won't happen that time and distance are triggered (almost) at the same time. If one of them is triggered, all the others are reset.

Yes, that's the idea. Plus such a structure also naturally integrates your goal update node.

[Sarath18]

Hello everyone, I would like to point out a potential issue in the current implementation. During the first cycle, if the TimeExpired node returns SUCCESS, ComputPathToPose is ticked to generate a path. In the next cycle, TimeExpired node returns FAILURE and DistanceTravelled is ticked, which might return SUCCESS leading to ComputPathToPose being ticked again. This can lead to continuous replanning of the path due to these nodes. To avoid this we need to add a reset functionality.

With the Fallback control node, all the new condition nodes would be halted if any returns Success.
Thanks to this, it won't happen that time and distance are triggered (almost) at the same time. If one of them is triggered, all the others are reset.

As far as I know, the fallback node halts all the children and sets their status to IDLE whenever any child return SUCCESS or the last child return FAILURE. Reset might not be the correct terminology used here. The Condition node in the behavior tree does not allow the halt() function to be overridden as you can see here. Correct me if I am wrong.

To solve this issue, I see a few potential implementations (taking into consideration the order of TimeExpired and DistanceTravelled can be changed):

1. The easiest way to do this would be to add a condition flag in the blackboard and check the value from the blackboard to reset it.
2. The better way to solve this might be to create a custom condition node in which we can override the halt() function. SyncActionNode cannot override halt() method. Using AsyncActionNode is a bad idea as they spawn a new thread. CoroActionNode might be the way to do it.

---

I would also like to suggest a small change in the current tree structure. Instead of having a fallback node and an inverter under the PipelineSequence, we can convert these nodes to a ReactiveSequence which still follows the same logical path. A visual representation is provided below.

[naiveHobo]

As far as I know, the fallback node halts all the children and sets their status to IDLE whenever any child return SUCCESS or the last child return FAILURE.

If this is true, a simple way to mend this would be to reset the state when the status of the condition node is IDLE. The same thing also happens in the decorators.

I would also like to suggest a small change in the current tree structure. Instead of having a fallback node and an inverter under the PipelineSequence, we can convert these nodes to a ReactiveSequence which still follows the same logical path. A visual representation is provided below.

Ah, good catch! Will make this change in the BT.

[gimait]

The Condition node in the behavior tree does not allow the halt() function to be overridden as you can see here. Correct me if I am wrong.

You are completely right, but shouldn't the node be initialized only when it's ticked and idle? :D
A simple if statement can make this work pretty well without overriding the halt:

• When ticked, if the node status is idle, initialize it and return failure (this also means that the node state is set to failure).
• If it's not idle, then do whatever distance/time check you need, and return success if the condition happens.

Also, if you need to override halt sometime, you can just subclass from action nodes instead.

[gimait]

If you set to true first_time_ when the node is ticked and idle, then the node will get initialized again after being halted.

[naiveHobo]

Yes, going to follow pretty much the thing you did in the GoalUpdated condition

[naiveHobo]

Waiting for #1712 (for GoalUpdated condition) and #1699 (for testing framework) to be merged to take this forward

[SteveMacenski]

I won't re-review until #1712 is in and that is used for the new is goal new node.

If one of them is triggered, all the others are reset.

How is that? If node 1 is triggered, then node 2 is never called to update its internal state about current time / distance. @Sarath18 's comment is my understanding as well. I'm not sure resetting on idle works because if node 2 is never ticked, the state never moves from idle.

I agree that a reset functionality would indeed fix the AND or OR on the condition issues I brought up. The BT also has an issue in another way; the IsNewGoal MUST be first. If its last and the other ones trigger, then the state of the new goal isn't propagated and results in the next tick cycle a replan from a request that should have been processed in the last update. So IsNewGoal must always be first in the children for preemption. But if any of them cause update, including IsNewGoal, the time and distance need to "reset" their internal state.

That seems to me to mean that the condition node that parents these needs to reset their states. I don't see how any of the suggestions above fixes that issue. I haven't fully thought out the use of halt(), Id have to dig into the BT.CPP code to make sure that halt() on all children are called if any return in this case. If that does work out, that needs to be documented the **** out of because we're doing something really subtle.

[gimait]

How is that? If node 1 is triggered, then node 2 is never called to update its internal state about current time / distance.

The behaviour of fallback nodes is: tick the first child, did it success? yes: halt all children and return success; no: tick the next child, did it success?.. and so on.
In the tree structure we have here (which is the one that Davide proposed as solution to our problem), on each iteration of the bt the condition nodes are checked, and if any of them is successful, we replan. There is only one problem with this implementation and it is that the planning cannot be halted if it's still running when a condition happens.. We have to check on how to do that, maybe the reactive sequence or reactive callback can be used.

The BT also has an issue in another way; the IsNewGoal MUST be first. If its last and the other ones trigger, then the state of the new goal isn't propagated and results in the next tick cycle a replan from a request that should have been processed in the last update.

That's right, that is a possibility. I agree that IsNewGoal should be first.

But if any of them cause update, including IsNewGoal, the time and distance need to "reset" their internal state.
That seems to me to mean that the condition node that parents these needs to reset their states.

Isn't it okay that the node is initialized when idle?
Fallbacks do reset their children states by calling halt all children.

[naiveHobo]

The BT also has an issue in another way; the IsNewGoal MUST be first. If its last and the other ones trigger, then the state of the new goal isn't propagated and results in the next tick cycle a replan from a request that should have been processed in the last update. So IsNewGoal must always be first in the children for preemption.

I don't think this is true. At least for the replanning branch, all the TimeExpired and DistanceTravelled conditions are doing is telling ComputePathToPose to replan a path given whatever goal is currently on the blackboard. Even if the other condition nodes are ticked before IsNewGoal when there is in fact a new goal, ComputePathToPose plans a path considering the latest goal on the blackboard.
As @gimait said, since one of these condition nodes returned success, the IsNewGoal condition will be reset to update its internal goal to store the latest goal and it won't be ticked in the next cycle if the goal is not updated again.

[SteveMacenski]

@gimait none of that answers my concerns. If you have some parameter param1 in a BT node that's second in the control flow, and the first one is called 5 times. Each time that first one returns to update and the control flow halt() all of the child nodes, no where in that process that I'm aware of is param1 reset.

The practical example is if NewGoal is first and distance is second, if NewGoal comes in 5 times rapidly to replan, each time the internal state parameter distance isn't being reset to 0. Then on iteration 6, it'll trigger a replan because its moved enough, but not relative to the last update.

The halt() method that's called would need to reset those parameters in each of the conditionals.

I don't think this is true. At least for the replanning branch, all the TimeExpired and DistanceTravelled conditions are doing is telling ComputePathToPose to replan a path given whatever goal is currently on the blackboard. Even if the other condition nodes are ticked before IsNewGoal when there is in fact a new goal, ComputePathToPose plans a path considering the latest goal on the blackboard.

Not the point - another instance of internal state being incorrect. The NewGoal parameter goal_ will be wrong causing a replan the next iteration it is called because it wasn't called initially when a new goal was actually available to update its internal state.

These are all cases of state issues that need to be addressed and generalized to document for designers.

[SteveMacenski]

Ok, I think this is now unblocked. We have the IsNewGoal so we can add these nodes and BT tree updates for both - though have we come to a resolution on the state updates ?

Edit: We may want to separate PRs, one that adds the nodes, the other with the BTs.

[naiveHobo]

Sorry, had to force push after rebasing to master.

We may want to separate PRs, one that adds the nodes, the other with the BTs.

Makes sense, I'll add the nodes on this PR.

---

though have we come to a resolution on the state updates ?

Not exactly. I agree with everything @SteveMacenski said but there are still a few issues.

There are two ways we can account for state reset:

1. The state reset can be done in halt(). For this, we will need a new base condition node which allows halt() to be overridden.

2. The state reset can be done in tick() by checking that the current state of the node is IDLE. This has to be the first check in tick() before any other processing. If the state is IDLE, reset state and return failure. This saves up from adding another node but a problem here is, one tick is wasted in restting the state.

I think the second option is better. In the example that @SteveMacenski proposed:

The practical example is if NewGoal is first and distance is second, if NewGoal comes in 5 times rapidly to replan, each time the internal state parameter distance isn't being reset to 0. Then on iteration 6, it'll trigger a replan because its moved enough, but not relative to the last update.

If a NewGoal comes in 5 times rapidly to replan, the internal state of DistanceTraveled and TimeExpired will indeed be incorrect. All the nodes are set to IDLE since NewGoal returned success. On the 6th iteration, DistanceTraveled will check if its current state is IDLE and will reset the internal state parameter distance back to 0 and return failure.

---

Another thing, with the current implementation of Fallback or ReactiveFallback, all the children are halted when:

1. Any one child returns success
2. All children return failure

For the IsReplanNeeded branch, I believe we need a control node with some functionalities of Fallback but some additional things as well. The ideal behavior should be:

1. If a child returns failure, tick the next child (like fallback)
2. If a child returns success, halt all children and return success (like fallback)
3. If all children return failure, don't halt any child and return failure (unlike fallback)

---

Another issue with the GoalUpdated condition; it doesn't return success on the very first goal. Which means it can't be used as it is for replanning. When the first goal is received, GoalUpdated updates its internal goal state and returns false and the other conditions also return false as expected. All the conditions return failure so all the children are halted and set back to IDLE. In the next tick, the GoalUpdated condition checks if it is IDLE and again updates its internal goal state and returns a failure and this keeps continuing.

This should be on a new ticket though.

[gimait]

There are two ways we can account for state reset:

I agree. I think in general the second option is best for the concept of condition nodes in general, unless you need to measure something since the last halt (let's say, has time passed since halt).
For nodes like isGoalUpdated, TimeExpired or DistanceTravelled, my understanding is that you want to measure those distances from the time when you first tick the node.

If a NewGoal comes in 5 times rapidly to replan, the internal state of DistanceTraveled and TimeExpired will indeed be incorrect. All the nodes are set to IDLE since NewGoal returned success. On the 6th iteration, DistanceTraveled will check if its current state is IDLE and will reset the internal state parameter distance back to 0 and return failure.

But that is what I would expect that should happen, right? If you just replanned, you won't need to replan again until you travelled x meters more (?).

Another thing, with the current implementation of Fallback or ReactiveFallback, all the children are halted

Very good point, I didn't realise that. I like a lot the idea of a new control node for this.

Another issue with the GoalUpdated condition; it doesn't return success on the very first goal.

This is intended. If you make it return success in the first iteration, it won't work for the recovery actions (which was the main reason why it was introduced). I think we have to think of condition nodes as single, super simple conditions. Otherwise, they can't be reused in different situations. The isGoalUpdated node is a good example. In my opinion, if you want something to happen in the first iteration, you could add a IsFirstCall condition node (or something) that returns success the first time and never again.
Following that idea, it doesn't make sense to have any node apart from the IsFirstTick return success on the first tick. Which is good, because if you have three different nodes with this functionality (let's say we return success in the first call to TimeExpired and DistanceTravelled), you would tick the first, return success, and replan, then you go in the fallback again, pass the first condition and get to the second, and replan again, because it's the first time you got in the second child.

We should probably open a slack group to align on the way to go, and then make the extra prs that we need to close this issues with the behavior trees.

What do you think @SteveMacenski? Should we also write a new ticket to finalize this?

[SteveMacenski]

@naiveHobo I think we just need an on_halt() method like we have an on_*() for other parts, in that, reset state. For door #3. That deals with how to deal with state, but not how to properly make sure that they're reset only when another is successful. That could be done with the blackboard or a new control flow node.

@naiveHobo @gimait please stop talking about this in a PR and redirect it to the ticket. This isn't the place for that discussion and it won't be easily searchable in the future. Go to #1701.

The discussion here should be about the condition nodes for time and distance, only.

[SteveMacenski]

@cdelsey @crdelsey (which account are you using now?) can you review?

[SteveMacenski]

@naiveHobo you have some merge conflicts from the last update. Can you fix so that Carl can review?

[naiveHobo]

@SteveMacenski Just to confirm, should I rebase to master?

[SteveMacenski]

Yes, or pull in changes and resolve the conflict.

[SteveMacenski]

@naiveHobo please avoid force pushing on PRs. I'd rather see you spend 20 commits fixing some small issue than have a blob of untracked code I need to fully re-review.

This looks good to me, I'm just waiting on Carl's thoughts. I'm using the tag merge as a reminder for me that this is ready to merge pending confirmation actions

[naiveHobo]

Ah, that's what I wanted to confirm. I rebased to master after your review, hence the force push.

[SteveMacenski]

@naiveHobo please rebase so that CI passes. @crdelsey I'd still like your review, that's the only thing blocking this.

[codecov]

Codecov Report

Merging #1705 into master will increase coverage by 0.93%.
The diff coverage is 94.44%.

@@            Coverage Diff             @@
##           master    #1705      +/-   ##
==========================================
+ Coverage   64.43%   65.37%   +0.93%     
==========================================
  Files         196      200       +4     
  Lines       10472    10526      +54     
==========================================
+ Hits         6748     6881     +133     
+ Misses       3724     3645      -79     

Flag	Coverage Δ
#project	65.37% <94.44%> (+0.93%)	⬆️

Impacted Files	Coverage Δ
...nav2_behavior_tree/plugins/distance_controller.hpp	100.00% <ø> (ø)
...ior_tree/plugins/decorator/distance_controller.cpp	76.19% <ø> (ø)
nav2_bt_navigator/src/bt_navigator.cpp	81.69% <ø> (ø)
.../plugins/condition/distance_traveled_condition.cpp	89.28% <89.28%> (ø)
...avior_tree/plugins/distance_traveled_condition.hpp	100.00% <100.00%> (ø)
...2_behavior_tree/plugins/time_expired_condition.hpp	100.00% <100.00%> (ø)
..._tree/plugins/condition/time_expired_condition.cpp	100.00% <100.00%> (ø)
...v2_util/include/nav2_util/simple_action_server.hpp	85.80% <0.00%> (-2.47%)	⬇️
nav2_amcl/src/amcl_node.cpp	83.23% <0.00%> (-0.77%)	⬇️
... and 6 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update de0ed9a...de0f797. Read the comment docs.

[SteveMacenski]

I think this soaked long enough.