Support loading multiple behavior tree files as subtrees

[Jad-ELHAJJ]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu
Robotic platform tested on	N/A
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Added support for loading multiple behavior tree files as subtrees
• Introduced registerTreeFromFile and updated BT Navigator to use it
• Adapted NavigateToPose and NavigateThroughPoses trees to use subtree loading
• Added unit test coverage for subtree loading logic

Description of documentation updates required from your changes

Added new parameter bt_search_directories to the bt_navigator, so need to add that to documentation page to use ST. Otherwise, its default value is empty.

Description of how this change was tested

• Unit test
• Created a dummy subtree and included in the navigate to pose main tree and monitored the main bt behavior

---

Future work that may be required in bullet points

None

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
• Should this be backported to current distributions? If so, tag with backport-*.

[mergify]

@Jad-ELHAJJ, 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]

@Jad-ELHAJJ, 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]

No major notes, looks good!

I think this is worthy of a migration guide entry (which explains the parameter and 'why' for subtrees) & update the configuration guide for the new parameter

[mergify]

@Jad-ELHAJJ, 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]

So this might just be from the github view and I'm missing something, but:

It seems to me that you're adding too many search_directories_. I see one in the BT Action Server that is given it on construction and passed into load behavior trees on activation.

I don't see immediately why the navigators (navigate to pose / through poses) need to have it also stored as a member and I don't see that those are actually used / populated. The search directories are given to the bt action server in the behavior_tree-navigator.hpp file in nav2_core where it finds those values & passes it off. It does not store them.

[Jad-ELHAJJ]

search_directories_

I believe that checking/loading BT inside the navigators, exactly in goalReceived is unnecessary, since the server will any way fail loading the BTs on its activation. So, I believe now with this change, the BTs will be loaded on the server level, which was already done but also checked(unnecessarily) in the navigators.

[SteveMacenski]

@Jad-ELHAJJ this PR does not compile, check CI

[Jad-ELHAJJ]

@Jad-ELHAJJ this PR does not compile, check CI

Getting:

10 191.2 --- stderr: nav2_util
10 191.2 In file included from /root/ws/src/nav2_util/src/robot_utils.cpp:25:
10 191.2 /root/ws/src/nav2_util/include/nav2_util/robot_utils.hpp:29:10: fatal error: tf2_ros/buffer.hpp: No such file or directory
10 191.2 29 | #include "tf2_ros/buffer.hpp"
10 191.2 | ^~~~~~~~~~~~~~~~~~~~
10 191.2 compilation terminated.

package failed: nav2_util
1 package had stderr output: nav2_util
32 packages not processed
ERROR: process "/bin/sh -c . /opt/ros/jazzy/setup.sh && colcon build --packages-skip nav2_system_tests nav2_bringup nav2_simple_commander nav2_loopback_sim navigation2" did not complete successfully: exit code: 2

[6/6] RUN . /opt/ros/jazzy/setup.sh && colcon build --packages-skip nav2_system_tests nav2_bringup nav2_simple_commander nav2_loopback_sim navigation2:
191.2 gmake[1]: *** [CMakeFiles/Makefile2:212: src/CMakeFiles/nav2_util_core.dir/all] Error 2
191.2 gmake[1]: *** Waiting for unfinished jobs....
191.2 gmake: *** [Makefile:146: all] Error 2
191.2 ---
191.2 Failed <<< nav2_util [11.2s, exited with code 2]
191.3
191.3 Summary: 7 packages finished [3min 11s]
191.3 1 package failed: nav2_util

[SteveMacenski]

Please fix the build before I take a look again. Output:

                                  
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp: In member function ‘bool nav2_system_tests::BehaviorTreeHandler::loadBehaviorTree(const std::string&, const std::vector<std::__cxx11::basic_string<char> >&)’:
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:117:36: error: ‘fs’ has not been declared
  117 |     const auto canonical_main_bt = fs::canonical(filename);
      |                                    ^~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:120:35: error: ‘search_directories’ was not declared in this scope
  120 |     for (const auto & directory : search_directories) {
      |                                   ^~~~~~~~~~~~~~~~~~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:122:35: error: ‘fs’ has not been declared
  122 |         for (const auto & entry : fs::directory_iterator(directory)) {
      |                                   ^~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:125:17: error: ‘fs’ has not been declared
  125 |             if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) {
      |                 ^~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:125:32: error: ‘fs’ has not been declared
  125 |             if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) {
      |                                ^~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:128:13: error: ‘bt_’ was not declared in this scope
  128 |             bt_->registerTreeFromFile(entry.path().string());
      |             ^~~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:111:38: error: unused parameter ‘search_dirs’ [-Werror=unused-parameter]
  111 |     const std::vector<std::string> & search_dirs)
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~
In file included from /opt/ros/rolling/src/gtest_vendor/include/gtest/gtest.h:72,
                 from /opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:23:
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp: In member function ‘virtual void nav2_system_tests::BehaviorTreeTestFixture_TestNavigateSubtreeRecoveries_Test::TestBody()’:
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:371:60: error: ‘search_dir’ was not declared in this scope; did you mean ‘search_dirs’?
  371 |   EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_dir), true);
      |                                                            ^~~~~~~~~~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp: In member function ‘virtual void nav2_system_tests::BehaviorTreeTestFixture_TestNavigateRecoverySimple_Test::TestBody()’:
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:434:60: error: ‘search_dir’ was not declared in this scope; did you mean ‘search_dirs’?
  434 |   EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_dir), true);
      |                                                            ^~~~~~~~~~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp: In member function ‘virtual void nav2_system_tests::BehaviorTreeTestFixture_TestNavigateRecoveryComplex_Test::TestBody()’:
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:536:60: error: ‘search_dir’ was not declared in this scope; did you mean ‘search_dirs’?
  536 |   EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_dir), true);
      |                                                            ^~~~~~~~~~
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp: In member function ‘virtual void nav2_system_tests::BehaviorTreeTestFixture_TestRecoverySubtreeGoalUpdated_Test::TestBody()’:
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:608:60: error: ‘search_dir’ was not declared in this scope; did you mean ‘search_dirs’?
  608 |   EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_dir), true);
      |                                                            ^~~~~~~~~~
cc1plus: all warnings being treated as errors
gmake[2]: *** [src/behavior_tree/CMakeFiles/test_behavior_tree_node.dir/build.make:76: src/behavior_tree/CMakeFiles/test_behavior_tree_node.dir/test_behavior_tree_node.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:584: src/behavior_tree/CMakeFiles/test_behavior_tree_node.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< nav2_system_tests [36.8s, exited with code 2]

[SteveMacenski]

I don't see any more gaps - did you test this again?

[SteveMacenski]

More CI errors:

/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp: In member function ‘bool nav2_system_tests::BehaviorTreeHandler::loadBehaviorTree(const std::string&, const std::vector<std::__cxx11::basic_string<char> >&)’:
/opt/overlay_ws/src/navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp:129:13: error: ‘bt_’ was not declared in this scope
  129 |             bt_->registerTreeFromFile(entry.path().string());
      |             ^~~

[codecov]

Codecov Report

❌ Patch coverage is 75.00000% with 4 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...clude/nav2_behavior_tree/bt_action_server_impl.hpp	66.66%	4 Missing ⚠️

Files with missing lines	Coverage Δ
...nclude/nav2_behavior_tree/behavior_tree_engine.hpp	100.00% <ø> (ø)
...ee/include/nav2_behavior_tree/bt_action_server.hpp	83.33% <ø> (ø)
nav2_behavior_tree/src/behavior_tree_engine.cpp	87.09% <100.00%> (+4.33%)	⬆️
...avigator/src/navigators/navigate_through_poses.cpp	87.91% <ø> (ø)
...2_bt_navigator/src/navigators/navigate_to_pose.cpp	81.01% <ø> (ø)
...core/include/nav2_core/behavior_tree_navigator.hpp	89.28% <100.00%> (+0.39%)	⬆️
...clude/nav2_behavior_tree/bt_action_server_impl.hpp	88.82% <66.66%> (-1.01%)	⬇️

... and 5 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[Jad-ELHAJJ]

I don't see any more gaps - did you test this again?

Hey Steve, the PR should be good now after your suggestions :)

I test it and works as expected following these steps:

• Create a BT.xml
• Use in the main BT like navigate_to_pose
• Add in main BT
• Also tried removing the include and got Setting internal error error_code:9001, error_msg:Exception when loading BT: Can't find a tree with name: BT_ID as expected

Feel free to add further comments for improvements. Thank you for your assistance and support!

[SteveMacenski]

The test coverage isn't showing. I think these are key gaps to make sure we gracefully handle failures for this PR given this is an area that I could see being a problem for users doing R&D

+     } catch (const std::exception & e) {
+       setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+         "Exception reading behavior tree directory: " + std::string(e.what()));

or

    } catch (const std::exception & e) {
      setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
-       std::string("Exception when loading BT: ") + e.what());
+       std::string("Exception when creating BT tree from file: ") + e.what());
      return false;

if you updated the tests to try the failure conditions. I think showing that these fail gracefully is an important element of the tests for this PR and should be easy enough to develop a test to show. The first is that a file in the directory isn't valid and the second being creating the tree from file

[Jad-ELHAJJ]

The test coverage isn't showing. I think these are key gaps to make sure we gracefully handle failures for this PR given this is an area that I could see being a problem for users doing R&D

+     } catch (const std::exception & e) {
+       setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+         "Exception reading behavior tree directory: " + std::string(e.what()));

or

    } catch (const std::exception & e) {
      setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
-       std::string("Exception when loading BT: ") + e.what());
+       std::string("Exception when creating BT tree from file: ") + e.what());
      return false;

if you updated the tests to try the failure conditions. I think showing that these fail gracefully is an important element of the tests for this PR and should be easy enough to develop a test to show. The first is that a file in the directory isn't valid and the second being creating the tree from file

I added some tests that tackles that matter. Is there a reason why the coverage is not showing? The tests that I have added should be covering these exceptions.

[tonynajjar]

Hey there, just a heads up, I'm getting this warning all of a sudden after this PR. It originates from here

[bt_navigator-5] WARNING: You executed BehaviorTreeFactory::createTreeFromFile after registerBehaviorTreeFrom[File/Text].
[bt_navigator-5] This is NOT, probably, what you want to do.
[bt_navigator-5] You should probably use BehaviorTreeFactory::createTree, instead

[SteveMacenski]

Fair point. @Jad-ELHAJJ please investigate and fix :-)

[Jad-ELHAJJ]

Fair point. @Jad-ELHAJJ please investigate and fix :-)

I will fix it and link the PR to this thread as well :)

[SteveMacenski]

@Jad-ELHAJJ any update?

[Jad-ELHAJJ]

@Jad-ELHAJJ any update?

Finalizing it :)
#5494

[doisyg]

Nice work ;)
But... this is changing (aka breaking) the nav2_behavior_tree::BtActionServer API and some of us are using it outside nav2.

So a note or even better a migration step here would be appreciated : https://docs.nav2.org/migration/Kilted.html

[doisyg]

Alternatively, make it backward compatible: #5518