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 6.14525% with 168 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_behavior_tree/plugins/control/pause.cpp	1.47%	134 Missing ⚠️
...lugins/control/sequence_with_blackboard_memory.cpp	5.71%	33 Missing ⚠️
...lugins/control/sequence_with_blackboard_memory.hpp	75.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...clude/nav2_behavior_tree/plugins/control/pause.hpp	`100.00% <100.00%> (ø)`
...lugins/control/sequence_with_blackboard_memory.hpp	`75.00% <75.00%> (ø)`
...lugins/control/sequence_with_blackboard_memory.cpp	`5.71% <5.71%> (ø)`
nav2_behavior_tree/plugins/control/pause.cpp	`1.47% <1.47%> (ø)`

... and 6 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

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

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.

SteveMacenski requested changes Jun 9, 2025

View reviewed changes

redvinaa added 2 commits June 10, 2025 11:22

Add pause and seq with bb memory BT nodes

839fc54

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

Requested changes

c83f7b2

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

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

Lint

e169959

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

SteveMacenski reviewed Jun 10, 2025

View reviewed changes

		@@ -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

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!

Uh oh!

SteveMacenski left a comment •

edited

Loading

SteveMacenski Jun 10, 2025 •

edited

Loading