Bt warning fix

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -21,7 +21,7 @@
 #include <limits>
 #include <memory>
 #include <set>
-#include <unordered_set>
+#include <utility>
 #include <string>
 #include <vector>
 
@@ -253,41 +253,73 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
     is_bt_id = true;
   }
 
-  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") {
-          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());
+  std::set<std::string> registered_ids;
+  std::string main_id;
+  auto register_all_bt_files = [&](const std::string & skip_file = "") {
+      for (const auto & directory : search_directories_) {
+        for (const auto & entry : fs::directory_iterator(directory)) {
+          if (entry.path().extension() != ".xml") {
+            continue;
+          }
+          if (!skip_file.empty() && entry.path().string() == skip_file) {
+            continue;
+          }
+
+          auto id = bt_->extractBehaviorTreeID(entry.path().string());
+          if (id.empty()) {
+            RCLCPP_ERROR(logger_, "Skipping BT file %s (missing ID)", entry.path().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());
+          if (registered_ids.count(id)) {
+            RCLCPP_WARN(logger_, "Skipping conflicting BT file %s (duplicate ID %s)",
+              entry.path().c_str(), id.c_str());
+            continue;
           }
+
+          RCLCPP_DEBUG(logger_, "Registering Tree from File: %s", entry.path().string().c_str());
           bt_->registerTreeFromFile(entry.path().string());
+          registered_ids.insert(id);
         }
       }
-    } 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 (by ID)
+    };
+
   try {
-    if(!is_bt_id) {
-      tree_ = bt_->createTreeFromFile(file_or_id, blackboard_);
+    if (!is_bt_id) {
+      // file_or_id is a filename: register it first
+      std::string main_file = file_or_id;
+      main_id = bt_->extractBehaviorTreeID(main_file);
+      if (main_id.empty()) {
+        RCLCPP_ERROR(logger_, "Failed to extract ID from %s", main_file.c_str());
+        return false;
+      }
+      RCLCPP_DEBUG(logger_, "Registering Tree from File: %s", main_file.c_str());
+      bt_->registerTreeFromFile(main_file);
+      registered_ids.insert(main_id);
+
+      // When a filename is specified, it must be register first
+      // and treat it as the "main" tree to execute.
+      // This ensures the requested tree is always available
+      // and prioritized, even if other files in the directory have duplicate IDs.
+      // The lambda then skips this main file to avoid
+      // re-registering it or logging a duplicate warning.
+      // In contrast, when an ID is specified, it's unknown which file is "main"
+      // so all files are registered and conflicts are handled in the lambda.
+      register_all_bt_files(main_file);
     } else {
-      tree_ = bt_->createTree(file_or_id, blackboard_);
+      // file_or_id is an ID: register all files, skipping conflicts
+      main_id = file_or_id;
+      register_all_bt_files();
     }
+  } catch (const std::exception & e) {
+    setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+      "Exception registering behavior trees: " + std::string(e.what()));
+    return false;
+  }
+
+  // Create the tree with the specified ID
+  try {
+    tree_ = bt_->createTree(main_id, blackboard_);
+    RCLCPP_INFO(logger_, "Created BT from ID: %s", main_id.c_str());
 
     for (auto & subtree : tree_.subtrees) {
       auto & blackboard = subtree->blackboard;
@@ -299,7 +331,7 @@ 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 creating BT tree from file: ") + e.what());
+      std::string("Exception when creating BT tree from ID: ") + e.what());
     return false;
   }