Fix error parsing subtree when loading from file path

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -36,6 +36,16 @@ namespace nav2_behavior_tree
  */
 enum class BtStatus { SUCCEEDED, FAILED, CANCELED };
 
+/**
+ * @struct nav2_behavior_tree::BTInfo
+ * @brief A struct to hold Behavior Tree ID information
+ */
+struct BTInfo
+{
+  std::string main_id;
+  std::vector<std::string> behavior_tree_ids;
+};
+
 /**
  * @class nav2_behavior_tree::BehaviorTreeEngine
  * @brief A class to create and handle behavior trees
@@ -87,11 +97,11 @@ class BehaviorTreeEngine
     BT::Blackboard::Ptr blackboard);
 
   /**
-   * @brief Extract BehaviorTree ID from BT file path or BT ID
-   * @param file_or_id
-   * @return std::string
+   * @brief Function to parse Behavior Tree information from an XML file
+   * @param filename Path to BT XML file
+   * @return BTInfo Struct containing BT ID information
    */
-  std::string extractBehaviorTreeID(const std::string & file_or_id);
+  BTInfo parseTreeInfo(const std::string & filename);
 
   /**
  * @brief Function to create a BT from a BehaviorTree ID

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -106,12 +106,6 @@ class BtActionServer
   bool loadBehaviorTree(
     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
    * @return BT::Blackboard::Ptr Shared pointer to current BT blackboard

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -273,19 +273,29 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
             continue;
           }
 
-          auto id = bt_->extractBehaviorTreeID(entry.path().string());
-          if (id.empty()) {
+          auto tree_info = bt_->parseTreeInfo(entry.path().string());
+          if (tree_info.behavior_tree_ids.empty()) {
             RCLCPP_ERROR(logger_, "Skipping BT file %s (missing ID)", entry.path().c_str());
             continue;
           }
-          if (registered_ids.count(id)) {
+          // Check for conflicts with all IDs in the file
+          bool conflict_found = false;
+          for (const auto & id : tree_info.behavior_tree_ids) {
+            if (registered_ids.count(id)) {
+              conflict_found = true;
+              break;
+            }
+          }
+          if (conflict_found) {
             conflicting_files.push_back(entry.path().string());
             continue;
           }
 
           RCLCPP_DEBUG(logger_, "Registering Tree from File: %s", entry.path().string().c_str());
           bt_->registerTreeFromFile(entry.path().string());
-          registered_ids.insert(id);
+          for (const auto & id : tree_info.behavior_tree_ids) {
+            registered_ids.insert(id);
+          }
         }
       }
     };
@@ -294,14 +304,20 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
     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()) {
+      auto tree_info = bt_->parseTreeInfo(main_file);
+      if (tree_info.main_id.empty()) {
         RCLCPP_ERROR(logger_, "Failed to extract ID from %s", main_file.c_str());
+        setInternalError(
+          ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+          "Failed to extract ID from " + main_file);
         return false;
       }
+      main_id = tree_info.main_id;
       RCLCPP_DEBUG(logger_, "Registering Tree from File: %s", main_file.c_str());
       bt_->registerTreeFromFile(main_file);
-      registered_ids.insert(main_id);
+      for (const auto & id : tree_info.behavior_tree_ids) {
+        registered_ids.insert(id);
+      }
 
       // When a filename is specified, it must be register first
       // and treat it as the "main" tree to execute.

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -105,46 +105,58 @@ BehaviorTreeEngine::createTreeFromFile(
   return factory_.createTreeFromFile(file_path, blackboard);
 }
 
-std::string BehaviorTreeEngine::extractBehaviorTreeID(
-  const std::string & bt_file)
+BTInfo BehaviorTreeEngine::parseTreeInfo(const std::string & filename)
 {
-  if (bt_file.empty()) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("BehaviorTreeEngine"),
-      "Error: Empty BT file passed to extractBehaviorTreeID");
-    return "";
+  BTInfo info;
+  if (filename.empty()) {
+    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "Empty BT file path.");
+    return info;
   }
+
   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 "";
+  if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) {
+    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "Could not parse: %s", filename.c_str());
+    return info;
   }
-  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 * root = doc.RootElement();
+  if (!root) {
+    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "No root element in: %s",
+      filename.c_str());
+    return info;
   }
-  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 "";
+
+  // Loop through all BehaviorTree elements to get all IDs
+  for (auto * bt = root->FirstChildElement("BehaviorTree"); bt;
+    bt = bt->NextSiblingElement("BehaviorTree"))
+  {
+    const char * id = bt->Attribute("ID");
+    if (id) {
+      info.behavior_tree_ids.emplace_back(id);
+    }
   }
-  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 "";
+
+  // First try to get main_tree_to_execute attribute
+  const char * main_attr = root->Attribute("main_tree_to_execute");
+  if (main_attr) {
+    info.main_id = main_attr;
   }
+
+  // If main_tree_to_execute attribute is not set, we first check the number of BehaviorTree tags
+  if (info.main_id.empty()) {
+    // If only one BehaviorTree tag is found, we can use that as the main ID
+    // If multiple are found, we throw an error since we don't know
+    // which one to use as the main tree
+    if (info.behavior_tree_ids.size() == 1) {
+      info.main_id = info.behavior_tree_ids[0];
+    } else if (info.behavior_tree_ids.size() > 1) {
+      throw std::runtime_error(
+              "Multiple BehaviorTree elements found in " + filename +
+              " but no main_tree_to_execute attribute specified. Unable to determine main tree.");
+    }
+  }
+
+  return info;
 }
 
 BT::Tree

nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp

@@ -102,9 +102,9 @@ class BehaviorTreeHandler
     return blackboard;
   }
 
-  std::string extractBehaviorTreeID(const std::string & file_or_id)
+  nav2_behavior_tree::BTInfo parseTreeInfo(const std::string & filename)
   {
-    return bt_engine_->extractBehaviorTreeID(file_or_id);
+    return bt_engine_->parseTreeInfo(filename);
   }
 
   bool loadBehaviorTree(
@@ -132,34 +132,46 @@ class BehaviorTreeHandler
               continue;
             }
 
-            auto id = bt_engine_->extractBehaviorTreeID(entry.path().string());
-            if (id.empty()) {
+            auto tree_info = bt_engine_->parseTreeInfo(entry.path().string());
+            if (tree_info.behavior_tree_ids.empty()) {
               std::cerr << "Skipping BT file " << entry.path() << " (missing ID)" << "\n";
               continue;
             }
-            if (registered_ids.count(id)) {
+          // Check for conflicts with all IDs in the file
+            bool conflict_found = false;
+            for (const auto & id : tree_info.behavior_tree_ids) {
+              if (registered_ids.count(id)) {
+                conflict_found = true;
+                break;
+              }
+            }
+            if (conflict_found) {
               conflicting_files.push_back(entry.path().string());
               continue;
             }
+
             std::cout << "Registering Tree from File: " << entry.path().string() << "\n";
             factory_.registerBehaviorTreeFromFile(entry.path().string());
-            registered_ids.insert(id);
+            for (const auto & id : tree_info.behavior_tree_ids) {
+              registered_ids.insert(id);
+            }
           }
         }
       };
 
     if (!is_bt_id) {
       // file_or_id is a filename: register it first
       std::string main_file = file_or_id;
-      main_id = bt_engine_->extractBehaviorTreeID(main_file);
+      auto tree_info = bt_engine_->parseTreeInfo(main_file);
 
-      if (main_id.empty()) {
+      if (tree_info.main_id.empty()) {
         std::cerr << "Failed to extract ID from " << main_file << "\n";
         return false;
       }
+      main_id = tree_info.main_id;
       std::cout << "Registering Tree from File: " << main_file << "\n";
       factory_.registerBehaviorTreeFromFile(main_file);
-      registered_ids.insert(main_id);
+      registered_ids.insert(tree_info.main_id);
 
       // Register all other trees, skipping conflicts with main_id
       register_all_bt_files(main_file);
@@ -347,8 +359,8 @@ TEST_F(BehaviorTreeTestFixture, TestExtractBehaviorTreeID)
     };
 
   // 1. Empty string input triggers "Empty file branch
-  auto empty_id = bt_handler->extractBehaviorTreeID("");
-  EXPECT_TRUE(empty_id.empty());
+  auto tree_info = bt_handler->parseTreeInfo("");
+  EXPECT_TRUE(tree_info.main_id.empty());
 
   // 2. Valid XML with ID
   std::string valid_xml = "/tmp/extract_bt_id_valid.xml";
@@ -360,28 +372,28 @@ TEST_F(BehaviorTreeTestFixture, TestExtractBehaviorTreeID)
     "    <AlwaysSuccess />\n"
     "  </BehaviorTree>\n"
     "</root>\n");
-  auto id = bt_handler->extractBehaviorTreeID(valid_xml);
-  EXPECT_FALSE(id.empty());
-  EXPECT_EQ(id, "TestTree");
+  auto id_info = bt_handler->parseTreeInfo(valid_xml);
+  EXPECT_FALSE(id_info.main_id.empty());
+  EXPECT_EQ(id_info.main_id, "TestTree");
 
   // 3. Malformed XML (parser error)
   std::string malformed_xml = "/tmp/extract_bt_id_malformed.xml";
   write_file(malformed_xml, "<root><invalid></root>");
-  auto missing_id = bt_handler->extractBehaviorTreeID(malformed_xml);
-  EXPECT_TRUE(missing_id.empty());
+  auto missing_id = bt_handler->parseTreeInfo(malformed_xml);
+  EXPECT_TRUE(missing_id.main_id.empty());
 
   // 4. File does not exist
-  auto not_found = bt_handler->extractBehaviorTreeID("/tmp/does_not_exist.xml");
-  EXPECT_TRUE(not_found.empty());
+  auto not_found = bt_handler->parseTreeInfo("/tmp/non_existent_file.xml");
+  EXPECT_TRUE(not_found.main_id.empty());
 
   // 6. No root element
   std::string no_root_file = "/tmp/extract_bt_id_no_root.xml";
   write_file(
     no_root_file,
     "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
     "<!-- no root element, just a comment -->\n");
-  auto no_root_id = bt_handler->extractBehaviorTreeID(no_root_file);
-  EXPECT_TRUE(no_root_id.empty());
+  auto no_root_id = bt_handler->parseTreeInfo(no_root_file);
+  EXPECT_TRUE(no_root_id.main_id.empty());
 
   // 7. No <BehaviorTree> child
   std::string no_bt_element = "/tmp/extract_bt_id_no_bt.xml";
@@ -391,8 +403,8 @@ TEST_F(BehaviorTreeTestFixture, TestExtractBehaviorTreeID)
     "<root BTCPP_format=\"4\">\n"
     "  <Dummy />\n"
     "</root>\n");
-  auto no_bt_id = bt_handler->extractBehaviorTreeID(no_bt_element);
-  EXPECT_TRUE(no_bt_id.empty());
+  auto no_bt_id = bt_handler->parseTreeInfo(no_bt_element);
+  EXPECT_TRUE(no_bt_id.main_id.empty());
 
   // 8. No ID attribute
   std::string no_id_attr = "/tmp/extract_bt_id_no_id.xml";
@@ -404,8 +416,57 @@ TEST_F(BehaviorTreeTestFixture, TestExtractBehaviorTreeID)
     "    <AlwaysSuccess />\n"
     "  </BehaviorTree>\n"
     "</root>\n");
-  auto no_id = bt_handler->extractBehaviorTreeID(no_id_attr);
-  EXPECT_TRUE(no_id.empty());
+  auto no_id = bt_handler->parseTreeInfo(no_id_attr);
+  EXPECT_TRUE(no_id.main_id.empty());
+
+  // 9. Maintree and subtree defined in the same file, with subtree defined first
+  std::string main_and_subtree = "/tmp/extract_bt_id_main_and_subtree.xml";
+  write_file(
+    main_and_subtree,
+    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+    "<root BTCPP_format=\"4\" main_tree_to_execute=\"MainTree\">\n"
+    "  <BehaviorTree ID=\"SubTree\">\n"
+    "    <AlwaysSuccess />\n"
+    "  </BehaviorTree>\n"
+    "  <BehaviorTree ID=\"MainTree\">\n"
+    "    <Subtree ID=\"SubTree\"/>\n"
+    "  </BehaviorTree>\n"
+    "</root>\n");
+  auto main_and_subtree_info = bt_handler->parseTreeInfo(main_and_subtree);
+  EXPECT_FALSE(main_and_subtree_info.main_id.empty());
+  EXPECT_EQ(main_and_subtree_info.main_id, "MainTree");
+
+  // 10. Maintree and subtree defined in the same file, with MainTree defined first
+  std::string main_and_subtree_2 = "/tmp/extract_bt_id_main_and_subtree_2.xml";
+  write_file(
+    main_and_subtree_2,
+    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+    "<root BTCPP_format=\"4\" main_tree_to_execute=\"MainTree\">\n"
+    "  <BehaviorTree ID=\"MainTree\">\n"
+    "    <Subtree ID=\"SubTree\"/>\n"
+    "  </BehaviorTree>\n"
+    "  <BehaviorTree ID=\"SubTree\">\n"
+    "    <AlwaysSuccess />\n"
+    "  </BehaviorTree>\n"
+    "</root>\n");
+  auto main_and_subtree_2_info = bt_handler->parseTreeInfo(main_and_subtree_2);
+  EXPECT_FALSE(main_and_subtree_2_info.main_id.empty());
+  EXPECT_EQ(main_and_subtree_2_info.main_id, "MainTree");
+
+  // 11. Multiple <BehaviorTree> elements but no main_tree_to_execute attribute, should throw
+  std::string multiple_bt_no_main = "/tmp/extract_bt_id_multiple_bt_no_main.xml";
+  write_file(
+    multiple_bt_no_main,
+    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+    "<root BTCPP_format=\"4\">\n"
+    "  <BehaviorTree ID=\"Tree1\">\n"
+    "    <AlwaysSuccess />\n"
+    "  </BehaviorTree>\n"
+    "  <BehaviorTree ID=\"Tree2\">\n"
+    "    <AlwaysSuccess />\n"
+    "  </BehaviorTree>\n"
+    "</root>\n");
+  EXPECT_THROW(bt_handler->parseTreeInfo(multiple_bt_no_main), std::runtime_error);
 
   // Cleanup
   std::remove(valid_xml.c_str());