Add Pause and SequenceWithBlackboardMemory BT nodes #5247
Conversation
Codecov ReportAttention: Patch coverage is
... and 5 files with indirect coverage changes 🚀 New features to boost your workflow:
|
nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp
Outdated
Show resolved
Hide resolved
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
|
Woah, this suddenly became more serious than I expected :D |
There was a problem hiding this comment.
Woah, this suddenly became more serious than I expected :D
I think this is really great work! Definitely happy to have it contributed! 🥇
I still have to write the tests and make this work with my jazzy stack, so it might take some time.
OK! Only really nitpicks, tests, and doc updates left in my book
nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
Outdated
Show resolved
Hide resolved
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
1 similar comment
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@SteveMacenski I started writing the tests and I have a few questions. The dummy BT node used for tests now returns Also, could you give me some pointers on how to write tests for the |
|
I found the bug which didn't let me set |
|
Sorry for the delay - I've been underwater the last couple of weeks and now committing today to get through my email review backlog. I apologize for the delay and thank you for your help! For tests, you can do something more in the theme of the action nodes: https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/test/plugins/action/test_append_goal_pose_to_goals_action.cpp. Here, we make an example BT XML that is loaded and used instead. This is actually a better way to write the tests IMO and the dummy node manual tree population done in the control test you linked to is an outdated way. We know those can have some problems since the tree isn't being constructed in the same way it would be in an application. You could still potentially use a dummy node within the tree, but the tree itself is constructed fully, including the ports and blackboard. That way, the blackboard of the BT is maintained between ticks since it acts like a proper behavior tree (because it is one). I think that answers your question! |
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
|
I've implemented the tests using xml strings as in the action nodes, and DummyNodes. I've had to be creative, because none of the existing tests needed this level of complexity. Tell me what you think of this solution @SteveMacenski. I've added a new header file to access nodes from the tree and cast them to the correct type. I also had to modify the DummyNode implementation a bit, so it returns IDLE after resetting, but on tick it again returns the state that it was set to. I don't think it breaks existing tests. |
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
1 similar comment
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
redvinaa
force-pushed
the
pause-bt-nodes
branch
2 times, most recently
from
55be1a2 to
dd67e94
Compare
|
@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues. |
|
This pull request is in conflict. Could you fix it @redvinaa? |
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
|
@SteveMacenski can you take a look at this? |
nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
Outdated
Show resolved
Hide resolved
| * Restart the loop only if (reset_on_failure == true) | ||
| * | ||
| */ | ||
| class SequenceWithBlackboardMemoryNode : public BT::ControlNode |
There was a problem hiding this comment.
If this is otherwise the same as the Sequence node, can we inherit from it, call the base class within the tick function and return the value? We can set the port if needed on the terminal states. That would make it easier to maintain unless there's a reason we cannot.
Also, do we want this to be a normal sequence or a reactive sequence? Typically I think of users with reactive sequences or pipeline sequences with Nav2
There was a problem hiding this comment.
That would make it a lot easier, but as I see the current_child_idx is private in the member class. I opened a PR to change that, but it's gonna take time to get accepted (or rejected).
| class SequenceWithBlackboardMemoryNode : public BT::ControlNode | ||
| { | ||
| public: | ||
| SequenceWithBlackboardMemoryNode(const std::string & name, const BT::NodeConfiguration & conf); |
There was a problem hiding this comment.
This name is also very similar to a BT.CPP provided node: https://www.behaviortree.dev/docs/nodes-library/sequencenode/#sequencewithmemory which means a different thing. Maybe we can rename here like SequenceWithState?
There was a problem hiding this comment.
You're right, it is confusing. I think the name should definitely include blackboard because it's what mostly distinguishes this node's functionality. State I think is also confusing because all the nodes have states already (like RUNNING and SUCCESS). What about SequenceWithBlackboardIndex?
There was a problem hiding this comment.
I think of blackboard being an implementation detail if you said something like 'output', but I don't object.
This is actually a good ChatGPT question:
- SequenceWithProgressOutput
- SequenceWithIndex
- IndexedSequence
- SequenceWithRunningIndex
How about something like IndexedSequence? I think just WithBlackboardIndex or WithIndex implies that the port is only an output, not a possible input as well. But I don't want to spend too much time on naming, so whatever you feel is best
There was a problem hiding this comment.
You're right, blackboard is an implementation detail, but the name definitely has to include the info that the index is read and written to (and also that it's a sequence, of course). I think calling it something with "blackboard" is close enough to the truth, while being far more descriptive than the chatgpt generated ones. Do you have something better than "blackboard" that still captures this info?
There was a problem hiding this comment.
StatefulSequence, PersistentSequence, ResumableSequence
I'm thinking Stateful or Resumable?
There was a problem hiding this comment.
I think stateful is not a lot better than state, for the same reasons. Resumable also means something different, like stopping and then continuing again. This sequence doesn't stop, it just does the processing in chunks (node by node). So nothing to resume in my opinion.
If it's out of these three, I'd go with persistent. But it's still hard to decipher from the name what exactly persists.
Then I'd rather go back to IndexedSequence, the word index I think also takes us closer to the truth.
There was a problem hiding this comment.
Resumable also means something different, like stopping and then continuing again.
Isn't this how it works? If you give it a child idx of 3, wouldn't it start at child idx 3?
If it's out of these three, I'd go with persistent. But it's still hard to decipher from the name what exactly persists.
Works for me 👍 I think that's what the doc guides are to some degree. I don't think anyone could know what Truncate Path vs Truncate Path Local means without looking at some docs without knowing anything about them
| case BT::NodeStatus::FAILURE: | ||
| RCLCPP_ERROR( | ||
| logger_, "%s child returned FAILURE", state_names.at(state_).c_str()); | ||
| return BT::NodeStatus::FAILURE; |
There was a problem hiding this comment.
Question: What's the logical recovery if an On Paused or On Resume branches return a failure?
There was a problem hiding this comment.
Right now in that case we halt and set the internal state to RESUMED.
I think the transition branches shouldn't fail, unless there was an unexpected error that should fail this node too. At least that was my intuition, but I haven't put much thought into this edge case.
The reasoning is also valid, that if a transition failed, we should stay in the state before the transition. Though I don't know how we would handle that, because we either add recovery branches to this node (which would make it practically unusable), or return failure (but that halts the node, and maybe risks accidentally staying in PAUSED state).
Let me know what you think.
There was a problem hiding this comment.
Maybe have a retry tag for the number of times to retry a state transition? We have that in other BT nodes so that wouldn't be out of place here and give the opportunity for small networking errors to be recoverable. If it ultimately fails, maybe its best to set the entire BT node to failure and halt it. Let the upstream nodes of this one figure out how to handle that.
I don't think it would be good to just resume, if the ON_RESUME state didn't finish, for example. There could be some initializations or activations that didn't happen that wouldn't make navigating possible. I think if those transition branches fail, that's a terminal failure state (after a couple of retries).
There was a problem hiding this comment.
Maybe have a retry tag for the number of times to retry a state transition?
Why would this be the responsibility of the PauseResumeController? There is a Retry BT node, I think that should be used to utilize the modularity of the BT.
I don't think it would be good to just resume, if the ON_RESUME state didn't finish
Definitely agree with this one.
There was a problem hiding this comment.
If we return failure from the PauseResumeController, do we maintain the current errored state for a retry to come in and re-tick to attempt? I don't think so from
if (status() == BT::NodeStatus::IDLE) {
state_ = RESUMED;
}
Which I think is actually the correct behavior. Its easier to retry within this node knowing its current state since a failure caused by the RESUME or PAUSED branch don't need retries, only the ON_RESUME and ON_PAUSED branches need retries generated by a failed transition. I don't think it makes sense to do that externally since that's tied to internal implementation information. It would be very difficult for another node to know if a failure from this one should trigger a retry or not without leaking alot of its state onto the blackboard. That starts to get a little complex and I think retry within a Controller BT node is pretty common across our Nav2 and BT.CPP nodes.
There was a problem hiding this comment.
Sorry, I meant retry as part of the ON_PAUSE and ON_RESUME branches, like this
<PauseResumeController pause_service_name="/pause" resume_service_name="/resume">
<!-- RESUMED branch -->
<!-- PAUSED branch (optional) -->
<RetryUntilSuccessful num_attempts="3">
<!-- ON_PAUSE branch (optional) -->
</RetryUntilSuccessful>
<RetryUntilSuccessful num_attempts="3">
<!-- ON_RESUME branch (optional) -->
</RetryUntilSuccessful>
</Pause>This would be transparent and wouldn't complicate the node logic unnecessarily.
What's the advantage of doing it as part of the PauseResumeController?
There was a problem hiding this comment.
Oh, uh, that makes sense too :-)
Can you actually add that as part of your docs examples to have the ON_ transition branches use retries? That would meet my request!
There was a problem hiding this comment.
Pull Request Overview
This PR introduces two new behavior tree control nodes—SequenceWithBlackboardMemory and PauseResumeController—along with test utilities and comprehensive tests to enable pause/resume functionality and persistent sequencing state.
- Added
DummyNodeandget_node_from_treetest utilities to support node status injection and retrieval. - Implemented
SequenceWithBlackboardMemoryNodeandPauseResumeControllerplugins, registered them in CMake and XML index. - Added unit tests for both new BT nodes and updated CMakeLists to include the new tests.
Reviewed Changes
Copilot reviewed 11 out of 11 changed files in this pull request and generated 3 comments.
Show a summary per file
| File | Description |
|---|---|
| test/utils/test_dummy_tree_node.hpp | Added DummyNode test utility |
| test/utils/get_node_from_tree.hpp | Introduced helper to retrieve nodes from a tree |
| test/plugins/control/test_sequence_with_blackboard_memory.cpp | Added tests for sequence with blackboard memory |
| test/plugins/control/test_pause_resume_controller.cpp | Added tests for pause/resume controller |
| test/plugins/control/CMakeLists.txt | Registered new control node tests |
| plugins/control/sequence_with_blackboard_memory.cpp | Implemented SequenceWithBlackboardMemoryNode |
| plugins/control/pause_resume_controller.cpp | Implemented PauseResumeController |
| nav2_behavior_tree/nav2_tree_nodes.xml | Declared new BT nodes in XML index |
| include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp | Declared ports and docs for sequence node |
| include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp | Declared ports, states, and docs for pause node |
| CMakeLists.txt | Linked new plugin libraries |
Comments suppressed due to low confidence (3)
nav2_behavior_tree/plugins/control/pause_resume_controller.cpp:68
- The exception message refers to "PauseNode" but the class is named PauseResumeController. Update the message to match the class name to avoid confusion.
"PauseNode must have at least one and at most four children "
nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp:71
- The XML usage example ends with
</Pause>but the node tag isPauseResumeController. Update the closing tag to</PauseResumeController>for consistency.
* </Pause>
nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp:37
- The documentation references a
reset_on_failureoption which doesn't exist in this node. Remove or update this line to reflect the actual behavior.
* Restart the loop only if (reset_on_failure == true)
|
Do with the AI generated code review as you see fit. If you think its pedantic or unnecessary, ignore. If it brings up a good point, please fix |
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
|
@redvinaa I also noticed the BT.CPP change was merged in - but no rush |
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Description of how this change was tested
Future work that may be required in bullet points
For Maintainers: