Fix/prevent warning when loading bt (#5494)

* Added BT ID finder and used createTree to resolve btcpp warning

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

* Support using either bt file path or bt id

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

* Not necessarily a file name

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

* fix logic

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

* fix function definition

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

* fixed unit test

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

* fixed BB variable

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

* fixed error msg

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

* fixed variable and its getter naming

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

* fixed definition

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

* fix xml check

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

* fix bt unit test

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

* test

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

* added back test

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

* check bt id using the root or bt id

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

* using tinyxml2 instead of regex

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

* fixed var name

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

* fixed var name

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

* check if arg is already a BT ID

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

* check if arg is already a BT ID

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

* Test was failing due to same BT ID MainTree among all registred trees

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

* Fixed error msg to be compliant

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

* python linting

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

* Removed unused createTreeFromFile since its replaced with createTree

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

* PR fixes

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

* Added new line at the end of BT xml

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

* Should cover most of the cases

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

* format

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

* Fixed BT format

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

* Removed redundant check

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

* Allow usage of file paths with a warning, while keeping BT ID usage as the recommended option

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

* Additional test

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

* Test coverage

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

@@ -86,6 +86,23 @@ class BehaviorTreeEngine
     const std::string & file_path,
     BT::Blackboard::Ptr blackboard);
 
+  /**
+   * @brief Extract BehaviorTree ID from BT file path or BT ID
+   * @param file_or_id
+   * @return std::string
+   */
+  std::string extractBehaviorTreeID(const std::string & file_or_id);
+
+    /**
+   * @brief Function to create a BT from a BehaviorTree ID
+   * @param tree_id BehaviorTree ID
+   * @param blackboard Blackboard for BT
+   * @return BT::Tree Created behavior tree
+    */
+  BT::Tree createTree(
+    const std::string & tree_id,
+    BT::Blackboard::Ptr blackboard);
+
   /**
    * @brief Add Groot2 monitor to publish BT status changes
    * @param tree BT to monitor

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -99,12 +99,18 @@ class BtActionServer
 
   /**
    * @brief Replace current BT with another one
-   * @param bt_xml_filename The file containing the new BT, uses default filename if empty
-   * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false
+   * @param bt_xml_filename_or_id The file containing the new BT, uses default filename if empty or BT ID
+   * @return bool true if the resulting BT correspond to the one in bt_xml_filename_or_id. false
    * if something went wrong, and previous BT is maintained
    */
   bool loadBehaviorTree(
-    const std::string & bt_xml_filename = "");
+    const std::string & bt_xml_filename_or_id = "");
+
+  /** @brief Extract BehaviorTree ID from XML file
+   * @param filename The file containing the BT
+   * @return std::string BehaviorTree ID if found, empty string otherwise
+   */
+  std::string extractBehaviorTreeID(const std::string & file_or_id);
 
   /**
    * @brief Getter function for BT Blackboard
@@ -119,18 +125,18 @@ class BtActionServer
    * @brief Getter function for current BT XML filename
    * @return string Containing current BT XML filename
    */
-  std::string getCurrentBTFilename() const
+  std::string getCurrentBTFilenameOrID() const
   {
-    return current_bt_xml_filename_;
+    return current_bt_file_or_id_;
   }
 
   /**
-   * @brief Getter function for default BT XML filename
-   * @return string Containing default BT XML filename
+   * @brief Getter function for default BT XML filename or ID
+   * @return string Containing default BT XML filename or ID
    */
-  std::string getDefaultBTFilename() const
+  std::string getDefaultBTFilenameOrID() const
   {
-    return default_bt_xml_filename_;
+    return default_bt_xml_filename_or_id_;
   }
 
   /**
@@ -245,8 +251,8 @@ class BtActionServer
   BT::Blackboard::Ptr blackboard_;
 
   // The XML file that contains the Behavior Tree to create
-  std::string current_bt_xml_filename_;
-  std::string default_bt_xml_filename_;
+  std::string current_bt_file_or_id_;
+  std::string default_bt_xml_filename_or_id_;
   std::vector<std::string> search_directories_;
 
   // The wrapper class for the BT functionality
@@ -283,7 +289,7 @@ class BtActionServer
   std::chrono::milliseconds wait_for_service_timeout_;
 
   // should the BT be reloaded even if the same xml filename is requested?
-  bool always_reload_bt_xml_ = false;
+  bool always_reload_bt_ = false;
 
   // Parameters for Groot2 monitoring
   bool enable_groot_monitoring_ = false;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -21,6 +21,7 @@
 #include <limits>
 #include <memory>
 #include <set>
+#include <unordered_set>
 #include <string>
 #include <vector>
 
@@ -45,7 +46,7 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
   OnCompletionCallback on_completion_callback,
   const std::vector<std::string> & search_directories)
 : action_name_(action_name),
-  default_bt_xml_filename_(default_bt_xml_filename),
+  default_bt_xml_filename_or_id_(default_bt_xml_filename),
   search_directories_(search_directories),
   plugin_lib_names_(plugin_lib_names),
   node_(parent),
@@ -178,7 +179,7 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
   int wait_for_service_timeout;
   node->get_parameter("wait_for_service_timeout", wait_for_service_timeout);
   wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout);
-  node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_);
+  node->get_parameter("always_reload_bt_xml", always_reload_bt_);
 
   // Get error code id names to grab off of the blackboard
   error_code_name_prefixes_ = node->get_parameter("error_code_name_prefixes").as_string_array();
@@ -204,8 +205,8 @@ template<class ActionT, class NodeT>
 bool BtActionServer<ActionT, NodeT>::on_activate()
 {
   resetInternalError();
-  if (!loadBehaviorTree(default_bt_xml_filename_)) {
-    RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str());
+  if (!loadBehaviorTree(default_bt_xml_filename_or_id_)) {
+    RCLCPP_ERROR(logger_, "Error loading BT: %s", default_bt_xml_filename_or_id_.c_str());
     return false;
   }
   action_server_->activate();
@@ -228,7 +229,7 @@ bool BtActionServer<ActionT, NodeT>::on_cleanup()
   action_server_.reset();
   topic_logger_.reset();
   plugin_lib_names_.clear();
-  current_bt_xml_filename_.clear();
+  current_bt_file_or_id_.clear();
   blackboard_.reset();
   bt_->haltAllActions(tree_);
   bt_->resetGrootMonitor();
@@ -246,40 +247,49 @@ void BtActionServer<ActionT, NodeT>::setGrootMonitoring(
 }
 
 template<class ActionT, class NodeT>
-bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml_filename)
+bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml_filename_or_id)
 {
   namespace fs = std::filesystem;
 
-  // Empty filename is default for backward compatibility
-  auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
+  // Empty argument is default for backward compatibility
+  auto file_or_id =
+    bt_xml_filename_or_id.empty() ? default_bt_xml_filename_or_id_ : bt_xml_filename_or_id;
 
   // Use previous BT if it is the existing one and always reload flag is not set to true
-  if (!always_reload_bt_xml_ && current_bt_xml_filename_ == filename) {
-    RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded");
+  if (!always_reload_bt_ && current_bt_file_or_id_ == file_or_id) {
+    RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml or ID is already loaded");
     return true;
   }
 
   // Reset any existing Groot2 monitoring
   bt_->resetGrootMonitor();
 
-  std::ifstream xml_file(filename);
-  if (!xml_file.good()) {
-    setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
-      "Couldn't open BT XML file: " + filename);
-    return false;
+  bool is_bt_id = false;
+  if ((file_or_id.length() < 4) ||
+    file_or_id.substr(file_or_id.length() - 4) != ".xml")
+  {
+    is_bt_id = true;
   }
 
-  const auto canonical_main_bt = fs::canonical(filename);
-
-  // Register all XML behavior Subtrees found in the given directories
+  std::unordered_set<std::string> used_bt_id;
   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)) {
+          auto current_bt_id = bt_->extractBehaviorTreeID(entry.path().string());
+          if (current_bt_id.empty()) {
+            RCLCPP_ERROR(logger_, "Skipping BT file %s (missing ID)",
+              entry.path().string().c_str());
             continue;
           }
+          auto [it, inserted] = used_bt_id.insert(current_bt_id);
+          if (!inserted) {
+            RCLCPP_WARN(
+              logger_,
+              "Warning: Duplicate BT IDs found. Make sure to have all BT IDs unique! "
+              "ID: %s File: %s",
+              current_bt_id.c_str(), entry.path().string().c_str());
+          }
           bt_->registerTreeFromFile(entry.path().string());
         }
       }
@@ -289,18 +299,21 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
       return false;
     }
   }
-
-  // Try to load the main BT tree
+  // Try to load the main BT tree (by ID)
   try {
-    tree_ = bt_->createTreeFromFile(filename, blackboard_);
+    if(!is_bt_id) {
+      tree_ = bt_->createTreeFromFile(file_or_id, blackboard_);
+    } else {
+      tree_ = bt_->createTree(file_or_id, blackboard_);
+    }
+
     for (auto & subtree : tree_.subtrees) {
       auto & blackboard = subtree->blackboard;
       blackboard->set("node", client_node_);
       blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
       blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
       blackboard->set<std::chrono::milliseconds>(
-        "wait_for_service_timeout",
-        wait_for_service_timeout_);
+          "wait_for_service_timeout", wait_for_service_timeout_);
     }
   } catch (const std::exception & e) {
     setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
@@ -310,8 +323,7 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
 
   // Optional logging and monitoring
   topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
-
-  current_bt_xml_filename_ = filename;
+  current_bt_file_or_id_ = file_or_id;
 
   if (enable_groot_monitoring_) {
     bt_->addGrootMonitoring(&tree_, groot_server_port_);

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -18,6 +18,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include "tinyxml2.h" //NOLINT
 
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/json_export.h"
@@ -102,6 +103,51 @@ BehaviorTreeEngine::createTreeFromFile(
   return factory_.createTreeFromFile(file_path, blackboard);
 }
 
+std::string BehaviorTreeEngine::extractBehaviorTreeID(
+  const std::string & bt_file)
+{
+  if(bt_file.empty()) {
+    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"),
+        "Error: Empty BT file passed to extractBehaviorTreeID");
+    return "";
+  }
+  tinyxml2::XMLDocument doc;
+  if (doc.LoadFile(bt_file.c_str()) != tinyxml2::XML_SUCCESS) {
+    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "Error: Could not open or parse file %s",
+        bt_file.c_str());
+    return "";
+  }
+  tinyxml2::XMLElement * rootElement = doc.RootElement();
+  if (!rootElement) {
+    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "Error: Root element not found in %s",
+        bt_file.c_str());
+    return "";
+  }
+  tinyxml2::XMLElement * btElement = rootElement->FirstChildElement("BehaviorTree");
+  if (!btElement) {
+    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"),
+        "Error: <BehaviorTree> element not found in %s", bt_file.c_str());
+    return "";
+  }
+  const char * idValue = btElement->Attribute("ID");
+  if (idValue) {
+    return std::string(idValue);
+  } else {
+    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"),
+        "Error: ID attribute not found on <BehaviorTree> element in %s",
+        bt_file.c_str());
+    return "";
+  }
+}
+
+BT::Tree
+BehaviorTreeEngine::createTree(
+  const std::string & tree_id,
+  BT::Blackboard::Ptr blackboard)
+{
+  return factory_.createTree(tree_id, blackboard);
+}
+
 /// @brief Register a tree from an XML file and return the tree
 void BehaviorTreeEngine::registerTreeFromFile(
   const std::string & file_path)

nav2_bt_navigator/behavior_trees/follow_point.xml

@@ -2,8 +2,8 @@
   This Behavior Tree follows a dynamic pose to a certain distance
 -->
 
-<root BTCPP_format="4" main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="FollowPoint">
+  <BehaviorTree ID="FollowPoint">
     <PipelineSequence name="NavigateWithReplanning">
       <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
       <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>

nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml

@@ -3,8 +3,8 @@
   recovery actions specific to planning / control as well as general system issues.
   This will be continuous if a kinematically valid planner is selected.
 -->
-<root BTCPP_format="4" main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="NavToPoseWithConsistentReplanningAndIfPathBecomesInvalid">
+  <BehaviorTree ID="NavToPoseWithConsistentReplanningAndIfPathBecomesInvalid">
     <RecoveryNode number_of_retries="6" name="NavigateRecovery">
       <PipelineSequence name="NavigateWithReplanning">
         <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>

nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml

@@ -10,8 +10,8 @@
   It also has recovery actions specific to planning / control as well as general system issues.
 -->
 
-<root BTCPP_format="4" main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="NavigateOnRouteGraphWRecovery">
+  <BehaviorTree ID="NavigateOnRouteGraphWRecovery">
     <RecoveryNode number_of_retries="6" name="NavigateRecovery">
       <PipelineSequence name="NavigateWithReplanning">
         <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>

nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml

@@ -3,8 +3,8 @@
   This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
    and it also has recovery actions specific to planning / control as well as general system issues.
 -->
-<root BTCPP_format="4" main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="NavigateThroughPosesWReplanningAndRecovery">
+  <BehaviorTree ID="NavigateThroughPosesWReplanningAndRecovery">
     <RecoveryNode number_of_retries="6" name="NavigateRecovery">
       <PipelineSequence name="NavigateWithReplanning">
         <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>

nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml

@@ -4,8 +4,8 @@
   recovery actions specific to planning / control as well as general system issues.
   This will be continuous if a kinematically valid planner is selected.
 -->
-<root BTCPP_format="4" main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWReplanningAndRecovery">
+  <BehaviorTree ID="NavigateToPoseWReplanningAndRecovery">
     <RecoveryNode number_of_retries="6" name="NavigateRecovery">
       <PipelineSequence name="NavigateWithReplanning">
         <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>

nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml

@@ -5,8 +5,8 @@
   make the robot wait for a specific time, to see if the obstacle clears out before
   navigating along a significantly longer path to reach the goal location.
 -->
-<root BTCPP_format="4" main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWReplanningGoalPatienceAndRecovery">
+  <BehaviorTree ID="NavigateToPoseWReplanningGoalPatienceAndRecovery">
     <RecoveryNode number_of_retries="6" name="NavigateRecovery">
       <PipelineSequence name="NavigateWithReplanning">
         <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>