Add Pause and SequenceWithBlackboardMemory BT nodes #5247

redvinaa · 2025-06-09T13:21:33Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #5213)
Primary OS tested on	Ubuntu
Robotic platform tested on	custom robot
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Implementation of BT nodes needed for pause / resume functionality, as discussed in Enable Pause/Resume in NavigateThroughPoses & NavigateToPose #5213.

Description of documentation updates required from your changes

Headers are well-documented
Added 2 new control nodes, they need to be described in the docs with examples (create mwe from the code in the discussion)

Description of how this change was tested

Have been using for a long time on humble, now on jazzy
Haven't tested on rolling, because I don't have a rolling stack

Future work that may be required in bullet points

Tests still need to be written for both BT nodes!

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

codecov · 2025-06-09T13:54:11Z

Codecov Report

❌ Patch coverage is 93.51852% with 7 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...r_tree/plugins/control/pause_resume_controller.cpp	94.36%	4 Missing ⚠️
...avior_tree/plugins/control/persistent_sequence.cpp	89.28%	3 Missing ⚠️

Files with missing lines	Coverage Δ
...r_tree/plugins/control/pause_resume_controller.hpp	`100.00% <100.00%> (ø)`
...avior_tree/plugins/control/persistent_sequence.hpp	`100.00% <100.00%> (ø)`
...avior_tree/plugins/control/persistent_sequence.cpp	`89.28% <89.28%> (ø)`
...r_tree/plugins/control/pause_resume_controller.cpp	`94.36% <94.36%> (ø)`

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

SteveMacenski

So this is missing tests coverage - but you know that :-) Also make sure to fix DCO

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp

nav2_behavior_tree/nav2_tree_nodes.xml

nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp

nav2_behavior_tree/plugins/control/pause.cpp

mergify · 2025-06-10T13:43:12Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

redvinaa · 2025-06-10T14:38:44Z

Woah, this suddenly became more serious than I expected :D
I still have to write the tests and make this work with my jazzy stack, so it might take some time.

SteveMacenski

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

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

mergify · 2025-06-20T13:33:09Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-20T15:34:35Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

redvinaa · 2025-06-20T15:44:03Z

@SteveMacenski I started writing the tests and I have a few questions.

The dummy BT node used for tests now returns IDLE after reset. Shouldn't it always return the status that it was set to?
The SequenceWithBlackboardMemoryNode logic has a resetChildren call, and all the dummy nodes return IDLE after it, making the tests fail.

Also, could you give me some pointers on how to write tests for the SequenceWithBlackboardMemoryNode? It needs changing the value of input ports between tests. I only found tests where the input port values are set in the SetUp part, but that cannot be parameterized. I also tried creating a new BT node instance for every single test, but I haven't managed to make that work yet, and even if I did, it wouldn't be a nice solution.

redvinaa · 2025-06-23T13:58:25Z

I found the bug which didn't let me set current_child_idx input_port: #5295

SteveMacenski · 2025-07-02T21:06:11Z

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!

mergify · 2025-07-04T14:13:24Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

redvinaa · 2025-07-04T14:51:17Z

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.

mergify · 2025-07-04T15:02:43Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-07-08T07:59:28Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-07-08T12:27:30Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-07-08T14:28:29Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-07-08T14:49:29Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-07-10T14:54:57Z

This pull request is in conflict. Could you fix it @redvinaa?

redvinaa · 2025-07-11T12:47:09Z

@SteveMacenski can you take a look at this?

SteveMacenski

Last bit are docs:

Add to the Nav2 Specific BT Nodes page for how to use
Migration guide
Configuration guide for param descriptions on the nodes + examples
Add to the Navigation Plugins page

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp

...behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

mergify · 2025-07-31T13:00:48Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

redvinaa · 2025-07-31T13:01:17Z

WARNING: don't accept this yet, still waiting for submodule changes

I've finally implemented the requested changes, but it turns out to inherit from the SequenceNode, more changes are required (namely the tick method should also be protected, and the config should be passed to the parent ControlNode, which for some reason in the SequenceNode isn't). I'm gonna open another PR agains BT.CPP and see what happens.

Also, how to "upgrade" the BT.CPP version used in this repo? I added a commit hash from the BT.CPP repo HEAD in the tools/underlay.repos but I'm not sure this is the way.

P.S. I also wrote the docs

mergify · 2025-07-31T13:24:46Z

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski · 2025-07-31T19:25:10Z

Pull in / rebase once #5409 is merged - we're having an issue with circleci time limits and I'm load balancing the builds.

For a temp basis, the BT custom build is OK but not ideal. I'd just make sure we poke Davide to release a new rolling version once those are merged so that we can have that be as temporary as possible. Or, we have the main version you have now and we can have a follow up PR once new binaries are available so Nav2 main users don't need to build BT.CPP from source in the interim. I'm OK with a follow up to strip out the redundant code as well :-)

Let me know!

Signed-off-by: redvinaa <redvinaa@gmail.com>

redvinaa · 2025-08-01T08:38:31Z

Okay! Then let's wait for the BT.CPP release, and then I'm gonna open a follow up.

Until then, this PR should be ready to merge :D I removed the parts that need the new BT version.

SteveMacenski

Just the small docs update and I can merge the pair!

SteveMacenski · 2025-08-04T18:12:04Z

@redvinaa thanks so much!

By the way... if you had a nice video / gif of this, this would make a really nice linkedin post to promote the changes and contribution 😉

tonynajjar · 2025-08-07T11:53:56Z

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

+  const BT::NodeConfiguration & conf)
+: BT::ControlNode(xml_tag_name, conf)
+{
+  auto node = this->config().blackboard->get<rclcpp::Node::SharedPtr>("node");


Suggested change

auto node = this->config().blackboard->get<rclcpp::Node::SharedPtr>("node");

auto node = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");

As per #5288

Nice catch, opened a PR to fix it

) * Add pause and seq with bb memory BT nodes Signed-off-by: redvinaa <redvinaa@gmail.com> * Requested changes Signed-off-by: redvinaa <redvinaa@gmail.com> * Lint Signed-off-by: redvinaa <redvinaa@gmail.com> * Restructure pause, rename unpaused state to resumed Signed-off-by: redvinaa <redvinaa@gmail.com> * Add PauseResumeController test Signed-off-by: redvinaa <redvinaa@gmail.com> * Implement tests using xml_txt trees and dummy nodes Signed-off-by: redvinaa <redvinaa@gmail.com> * Update include Signed-off-by: redvinaa <redvinaa@gmail.com> * One more fix because of sync Signed-off-by: redvinaa <redvinaa@gmail.com> * Fix build Signed-off-by: redvinaa <redvinaa@gmail.com> * Add tests Signed-off-by: redvinaa <redvinaa@gmail.com> * Remove unreachable code Signed-off-by: redvinaa <redvinaa@gmail.com> * Fix tests Signed-off-by: redvinaa <redvinaa@gmail.com> * Update copyrights Signed-off-by: redvinaa <redvinaa@gmail.com> * Fix size calc Signed-off-by: redvinaa <redvinaa@gmail.com> * Rename to PersistentSequence Signed-off-by: redvinaa <redvinaa@gmail.com> * Fix docstring Signed-off-by: redvinaa <redvinaa@gmail.com> --------- Signed-off-by: redvinaa <redvinaa@gmail.com>

SteveMacenski requested changes Jun 9, 2025

View reviewed changes

redvinaa force-pushed the pause-bt-nodes branch from d438a6e to c83f7b2 Compare June 10, 2025 12:49

SteveMacenski reviewed Jun 10, 2025

View reviewed changes

redvinaa marked this pull request as ready for review July 4, 2025 13:27

redvinaa force-pushed the pause-bt-nodes branch from f258501 to ff9a36e Compare July 4, 2025 14:49

redvinaa mentioned this pull request Jul 4, 2025

Nav2_behavior_tree tests input_port doesn't get set #5295

Closed

redvinaa force-pushed the pause-bt-nodes branch from ff9a36e to 36301f3 Compare July 8, 2025 07:55

redvinaa force-pushed the pause-bt-nodes branch from b01dfe7 to 4e4affc Compare July 8, 2025 14:25

redvinaa force-pushed the pause-bt-nodes branch 2 times, most recently from 55be1a2 to dd67e94 Compare July 8, 2025 14:36

redvinaa force-pushed the pause-bt-nodes branch from c842db0 to f7c65c8 Compare July 11, 2025 07:58

SteveMacenski reviewed Jul 14, 2025

View reviewed changes

SteveMacenski requested a review from Copilot July 14, 2025 21:39

redvinaa mentioned this pull request Jul 31, 2025

Write docs for PauseResumeController and PersistentSequence ros-navigation/docs.nav2.org#739

Merged

redvinaa mentioned this pull request Jul 31, 2025

Make sequence node inheritable BehaviorTree/BehaviorTree.CPP#996

Open

redvinaa added 16 commits August 1, 2025 10:34

Add pause and seq with bb memory BT nodes

37da4ee

Signed-off-by: redvinaa <redvinaa@gmail.com>

Requested changes

752eded

Signed-off-by: redvinaa <redvinaa@gmail.com>

Lint

0aeea3d

Signed-off-by: redvinaa <redvinaa@gmail.com>

Restructure pause, rename unpaused state to resumed

bf1de2a

Signed-off-by: redvinaa <redvinaa@gmail.com>

Add PauseResumeController test

78dbcf5

Signed-off-by: redvinaa <redvinaa@gmail.com>

Implement tests using xml_txt trees and dummy nodes

223669c

Signed-off-by: redvinaa <redvinaa@gmail.com>

Update include

1042042

Signed-off-by: redvinaa <redvinaa@gmail.com>

One more fix because of sync

b48ea77

Signed-off-by: redvinaa <redvinaa@gmail.com>

Fix build

c1632c7

Signed-off-by: redvinaa <redvinaa@gmail.com>

Add tests

f844ee3

Signed-off-by: redvinaa <redvinaa@gmail.com>

Remove unreachable code

59c2494

Signed-off-by: redvinaa <redvinaa@gmail.com>

Fix tests

03a79e0

Signed-off-by: redvinaa <redvinaa@gmail.com>

Update copyrights

539407a

Signed-off-by: redvinaa <redvinaa@gmail.com>

Fix size calc

580f182

Signed-off-by: redvinaa <redvinaa@gmail.com>

Rename to PersistentSequence

84ce999

Signed-off-by: redvinaa <redvinaa@gmail.com>

Fix docstring

0db8b63

Signed-off-by: redvinaa <redvinaa@gmail.com>

redvinaa force-pushed the pause-bt-nodes branch from 184381f to 0db8b63 Compare August 1, 2025 08:36

SteveMacenski approved these changes Aug 1, 2025

View reviewed changes

SteveMacenski merged commit 8e3aee6 into ros-navigation:main Aug 4, 2025
16 checks passed

tonynajjar reviewed Aug 7, 2025

View reviewed changes

redvinaa mentioned this pull request Aug 7, 2025

Make pause resume controller use nav2::LifecycleNode #5427

Merged

	auto node = this->config().blackboard->get<rclcpp::Node::SharedPtr>("node");
	auto node = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");

Add Pause and SequenceWithBlackboardMemory BT nodes #5247

Add Pause and SequenceWithBlackboardMemory BT nodes #5247

Uh oh!

Conversation

redvinaa commented Jun 9, 2025

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:

Uh oh!

codecov bot commented Jun 9, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Jun 10, 2025

Uh oh!

redvinaa commented Jun 10, 2025

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Jun 20, 2025

Uh oh!

mergify bot commented Jun 20, 2025

Uh oh!

redvinaa commented Jun 20, 2025

Uh oh!

redvinaa commented Jun 23, 2025

Uh oh!

SteveMacenski commented Jul 2, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

mergify bot commented Jul 4, 2025

Uh oh!

redvinaa commented Jul 4, 2025

Uh oh!

mergify bot commented Jul 4, 2025

Uh oh!

mergify bot commented Jul 8, 2025

Uh oh!

mergify bot commented Jul 8, 2025

Uh oh!

mergify bot commented Jul 8, 2025

Uh oh!

mergify bot commented Jul 8, 2025

Uh oh!

mergify bot commented Jul 10, 2025

Uh oh!

redvinaa commented Jul 11, 2025

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Jul 31, 2025

codecov bot commented Jun 9, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski commented Jul 2, 2025 •

edited

Loading

SteveMacenski commented Jul 31, 2025 •

edited

Loading

SteveMacenski commented Aug 4, 2025 •

edited

Loading