Support loading multiple behavior tree files as subtrees (ros-navigation#5426)

* Support loading multiple behavior tree files as subtrees

Signed-off-by: Jad El Hajj <[email protected]>

* Fix code style

Signed-off-by: Jad El Hajj <[email protected]>

* Added default param value

Signed-off-by: Jad El Hajj <[email protected]>

* Added recursive check to loadBehaviorTree and adapted unit test accordingly

Signed-off-by: Jad El Hajj <[email protected]>

* Removed nested loadBehaviorTree check in navigators

Signed-off-by: Jad El Hajj <[email protected]>

* Removed whitespace cpplint

Signed-off-by: Jad El Hajj <[email protected]>

* Fixed goalReceived

Signed-off-by: Jad El Hajj <[email protected]>

* Let loadbehaviorTree use its own search_directories var

Signed-off-by: Jad El Hajj <[email protected]>

* PR fixes-format-lint and test

Signed-off-by: Jad El Hajj <[email protected]>

* fix pointer

Signed-off-by: Jad El Hajj <[email protected]>

* Added unit test for BT xml validity

Signed-off-by: Jad El Hajj <[email protected]>

* CPPLint

Signed-off-by: Jad El Hajj <[email protected]>

* Check non existent search directory for bt

Signed-off-by: Jad El Hajj <[email protected]>

* CPPLint

Signed-off-by: Jad El Hajj <[email protected]>

* Fixed BT tests

Signed-off-by: Jad El Hajj <[email protected]>

* Fixed BT tests

Signed-off-by: Jad El Hajj <[email protected]>

---------

Signed-off-by: Jad El Hajj <[email protected]>

Author: Jad-ELHAJJ

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -98,6 +98,12 @@ class BehaviorTreeEngine
    */
   void resetGrootMonitor();
 
+  /**
+   * @brief Function to register a BT from an XML file
+   * @param file_path Path to BT XML file
+   */
+  void registerTreeFromFile(const std::string & file_path);
+
   /**
    * @brief Function to explicitly reset all BT nodes to initial state
    * @param tree Tree to halt

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -53,6 +53,7 @@ class BtActionServer
     const std::string & action_name,
     const std::vector<std::string> & plugin_lib_names,
     const std::string & default_bt_xml_filename,
+    const std::vector<std::string> & search_directories,
     OnGoalReceivedCallback on_goal_received_callback,
     OnLoopCallback on_loop_callback,
     OnPreemptCallback on_preempt_callback,
@@ -102,7 +103,8 @@ class BtActionServer
    * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false
    * if something went wrong, and previous BT is maintained
    */
-  bool loadBehaviorTree(const std::string & bt_xml_filename = "");
+  bool loadBehaviorTree(
+    const std::string & bt_xml_filename = "");
 
   /**
    * @brief Getter function for BT Blackboard
@@ -245,6 +247,7 @@ class BtActionServer
   // The XML file that contains the Behavior Tree to create
   std::string current_bt_xml_filename_;
   std::string default_bt_xml_filename_;
+  std::vector<std::string> search_directories_;
 
   // The wrapper class for the BT functionality
   std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -39,12 +39,14 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
   const std::string & action_name,
   const std::vector<std::string> & plugin_lib_names,
   const std::string & default_bt_xml_filename,
+  const std::vector<std::string> & search_directories,
   OnGoalReceivedCallback on_goal_received_callback,
   OnLoopCallback on_loop_callback,
   OnPreemptCallback on_preempt_callback,
   OnCompletionCallback on_completion_callback)
 : action_name_(action_name),
   default_bt_xml_filename_(default_bt_xml_filename),
+  search_directories_(search_directories),
   plugin_lib_names_(plugin_lib_names),
   node_(parent),
   on_goal_received_callback_(on_goal_received_callback),
@@ -246,6 +248,8 @@ void BtActionServer<ActionT, NodeT>::setGrootMonitoring(
 template<class ActionT, class NodeT>
 bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml_filename)
 {
+  namespace fs = std::filesystem;
+
   // Empty filename is default for backward compatibility
   auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
 
@@ -255,19 +259,38 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
     return true;
   }
 
-  // if a new tree is created, than the Groot2 Publisher must be destroyed
+  // Reset any existing Groot2 monitoring
   bt_->resetGrootMonitor();
 
-  // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(filename);
-
   if (!xml_file.good()) {
     setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
-      "Couldn't open input XML file: " + filename);
+      "Couldn't open BT XML file: " + filename);
     return false;
   }
 
-  // Create the Behavior Tree from the XML input
+  const auto canonical_main_bt = fs::canonical(filename);
+
+  // Register all XML behavior Subtrees found in the given directories
+  for (const auto & directory : search_directories_) {
+    try {
+      for (const auto & entry : fs::directory_iterator(directory)) {
+        if (entry.path().extension() == ".xml") {
+          // Skip registering the main tree file
+          if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) {
+            continue;
+          }
+          bt_->registerTreeFromFile(entry.path().string());
+        }
+      }
+    } catch (const std::exception & e) {
+      setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+        "Exception reading behavior tree directory: " + std::string(e.what()));
+      return false;
+    }
+  }
+
+  // Try to load the main BT tree
   try {
     tree_ = bt_->createTreeFromFile(filename, blackboard_);
     for (auto & subtree : tree_.subtrees) {
@@ -281,15 +304,15 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
     }
   } 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;
   }
 
+  // Optional logging and monitoring
   topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
 
   current_bt_xml_filename_ = filename;
 
-  // Enable monitoring with Groot2
   if (enable_groot_monitoring_) {
     bt_->addGrootMonitoring(&tree_, groot_server_port_);
     RCLCPP_DEBUG(

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -102,6 +102,13 @@ BehaviorTreeEngine::createTreeFromFile(
   return factory_.createTreeFromFile(file_path, blackboard);
 }
 
+/// @brief Register a tree from an XML file and return the tree
+void BehaviorTreeEngine::registerTreeFromFile(
+  const std::string & file_path)
+{
+  factory_.registerBehaviorTreeFromFile(file_path);
+}
+
 void
 BehaviorTreeEngine::addGrootMonitoring(
   BT::Tree * tree,

nav2_bringup/params/nav2_params.yaml

@@ -58,6 +58,8 @@ bt_navigator:
       plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
       enable_groot_monitoring: false
       groot_server_port: 1669
+    bt_search_directories:
+      - $(find-pkg-share nav2_bt_navigator)/behavior_trees
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -73,7 +73,6 @@ bool
 NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 {
   auto bt_xml_filename = goal->behavior_tree;
-
   if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
     bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       "Error loading XML file: " + bt_xml_filename + ". Navigation canceled.");

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -83,7 +83,6 @@ bool
 NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 {
   auto bt_xml_filename = goal->behavior_tree;
-
   if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
     bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled.");

nav2_core/include/nav2_core/behavior_tree_navigator.hpp

@@ -201,13 +201,20 @@ class BehaviorTreeNavigator : public NavigatorBase
     // get the default behavior tree for this navigator
     std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node);
 
+    auto search_directories = node->declare_or_get_parameter(
+      "bt_search_directories",
+      std::vector<std::string>{ament_index_cpp::get_package_share_directory(
+      "nav2_bt_navigator") + "/behavior_trees"}
+    );
+
     // Create the Behavior Tree Action Server for this navigator
     bt_action_server_ =
       std::make_unique<nav2_behavior_tree::BtActionServer<ActionT, nav2::LifecycleNode>>(
       node,
       getName(),
       plugin_lib_names,
       default_bt_xml_filename,
+      search_directories,
       std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1),
       std::bind(&BehaviorTreeNavigator::onLoop, this),
       std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1),