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

Attention: 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 ⚠️
...lugins/control/sequence_with_blackboard_memory.cpp	89.28%	3 Missing ⚠️

Files with missing lines	Coverage Δ
...r_tree/plugins/control/pause_resume_controller.hpp	`100.00% <100.00%> (ø)`
...lugins/control/sequence_with_blackboard_memory.hpp	`100.00% <100.00%> (ø)`
...lugins/control/sequence_with_blackboard_memory.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

SteveMacenski · 2025-06-09T19:05:53Z

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

@@ -0,0 +1,98 @@
+// Copyright (c) 2019 Intel Corporation


I think all of these should be updated :-)

@redvinaa I meant updated with your company since you wrote these :-)

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

SteveMacenski · 2025-06-09T19:07:57Z

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -390,6 +390,15 @@

    <Control ID="RoundRobin"/>

+    <Control ID="Pause">


Note: Add to the docs.nav2.org for new pages for these 2 nodes + migration guide update for the nodes and how to use them for pause / resume navigation behavior within the BT

nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp

nav2_behavior_tree/plugins/control/pause.cpp

SteveMacenski · 2025-06-09T19:13:19Z

nav2_behavior_tree/plugins/control/pause.cpp

+BT::NodeStatus Pause::tick()
+{
+  unsigned int children_count = children_nodes_.size();
+  if (children_count < 1 || children_count > 4) {


Docs on the < 1 || > 4 is important. I'd add it both in the header doxygen, the BT walkthrough or tutorial, and the configuration guide page.

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

SteveMacenski · 2025-06-10T17:32:25Z

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

+
+  void switchState(const state_t new_state);
+
+  rclcpp::Node::SharedPtr node_;


Store the logger not the node please

SteveMacenski · 2025-06-10T17:33:47Z

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

+  }
+
+  if (state_ != PAUSED) {
+    RCLCPP_INFO(node_->get_logger(), "PAUSE_REQUESTED");


I think this log can be a little more than this 😉

SteveMacenski · 2025-06-10T17:33:51Z

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

+  }
+
+  if (state_ == PAUSED) {
+    RCLCPP_INFO(node_->get_logger(), "RESUME_REQUESTED");


I think this log can be a little more than this 😉

SteveMacenski · 2025-06-10T17:36:58Z

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

+ * - resume_service_name: name of the service to resume
+ *
+ * Usage:
+ * <Pause pause_service_name="/pause" resume_service_name="/resume">


Can we work a little on the naming here: Unpaused --> Active? Resume? Processing?

Unpaused gave me a bit of back and forth to understand & the ON_RESUME branch is using the word "resume" on "unpaused".

Also, Pause should now be PauseResumeController

SteveMacenski · 2025-06-10T17:38:33Z

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

+BT::NodeStatus PauseResumeController::tickChildAndTransition()
+{
+  // Return RUNNING and do nothing if specific child is not used
+  const uint child_idx = child_indices.at(state_);


Type out (here and anywhere else) unsigned int. This helps with windows compatibility

SteveMacenski · 2025-06-10T18:14:28Z

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

+BT::NodeStatus PauseResumeController::tickChildAndTransition()
+{
+  // Return RUNNING and do nothing if specific child is not used
+  const uint child_idx = child_indices.at(state_);


Can we do this logic in the switchState function? Switch to the correct state if not implemented so that when tickChildAndTransition is called we do useful work.

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?

Signed-off-by: redvinaa <[email protected]>

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

@SteveMacenski can you take a look at this?

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 added 4 commits July 11, 2025 09:57

Add pause and seq with bb memory BT nodes

92d2252

Signed-off-by: redvinaa <[email protected]>

Requested changes

5c9c05c

Signed-off-by: redvinaa <[email protected]>

Lint

e6bb618

Signed-off-by: redvinaa <[email protected]>

Restructure pause, rename unpaused state to resumed

3a7e8bc

Signed-off-by: redvinaa <[email protected]>

redvinaa added 8 commits July 11, 2025 09:57

Add PauseResumeController test

b16ff54

Signed-off-by: redvinaa <[email protected]>

Implement tests using xml_txt trees and dummy nodes

05ac25a

Signed-off-by: redvinaa <[email protected]>

Update include

3e83155

Signed-off-by: redvinaa <[email protected]>

One more fix because of sync

cb3c910

Signed-off-by: redvinaa <[email protected]>

Fix build

da558a5

Signed-off-by: redvinaa <[email protected]>

Add tests

d353573

Signed-off-by: redvinaa <[email protected]>

Remove unreachable code

4cb5ae2

Signed-off-by: redvinaa <[email protected]>

Fix tests

f7c65c8

Signed-off-by: redvinaa <[email protected]>

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

		@@ -390,6 +390,15 @@

		<Control ID="RoundRobin"/>

		<Control ID="Pause">


		void switchState(const state_t new_state);

		rclcpp::Node::SharedPtr node_;

Add Pause and SequenceWithBlackboardMemory BT nodes #5247

Are you sure you want to change the base?

Add Pause and SequenceWithBlackboardMemory BT nodes #5247

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!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

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!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jun 10, 2025 • 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!

Choose a reason for hiding this comment

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

codecov bot commented Jun 9, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski Jun 10, 2025 •

edited

Loading

SteveMacenski commented Jul 2, 2025 •

edited

Loading