diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 09becc161fa..74d3700620e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include @@ -253,41 +253,73 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml is_bt_id = true; } - std::unordered_set 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 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::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; } diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 9a5f9a1826f..6a6cd178a5e 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -110,56 +110,69 @@ class BehaviorTreeHandler const std::vector & search_directories) { namespace fs = std::filesystem; - 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; - } - // Register all XML behavior subtrees in the directories - std::unordered_set 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_engine_->extractBehaviorTreeID(entry.path().string()); - if (current_bt_id.empty()) { - std::cerr << "[behavior_tree_handler]: Skipping BT file " - << entry.path().string() << " (missing ID)\n"; + const std::string kXmlExtension = ".xml"; + const bool is_bt_id = (file_or_id.length() < kXmlExtension.size()) || + (file_or_id.compare(file_or_id.length() - kXmlExtension.size(), + kXmlExtension.size(), kXmlExtension) != 0); + + std::set 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_engine_->extractBehaviorTreeID(entry.path().string()); + if (id.empty()) { + std::cerr << "Skipping BT file " << entry.path() << " (missing ID)" << "\n"; continue; } - auto [it, inserted] = used_bt_id.insert(current_bt_id); - if (!inserted) { - std::cout << "[behavior_tree_handler]: Warning: Duplicate BT IDs found. " - "Make sure to have all BT IDs unique! " - << "ID: " << current_bt_id - << " File: " << entry.path().string() << "\n"; + if (registered_ids.count(id)) { + std::cerr << "Skipping conflicting BT file " << entry.path() << " (duplicate ID " << + id << ")" << "\n"; + continue; } + std::cout << "Registering Tree from File: " << entry.path().string() << "\n"; factory_.registerBehaviorTreeFromFile(entry.path().string()); + registered_ids.insert(id); } } - } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), - "Exception reading behavior tree directory: %s", e.what()); + }; + + 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); + + if (main_id.empty()) { + std::cerr << "Failed to extract ID from " << main_file << "\n"; return false; } + std::cout << "Registering Tree from File: " << main_file << "\n"; + factory_.registerBehaviorTreeFromFile(main_file); + registered_ids.insert(main_id); + + // Register all other trees, skipping conflicts with main_id + register_all_bt_files(main_file); + } else { + // file_or_id is an ID: register all files, skipping conflicts + main_id = file_or_id; + register_all_bt_files(); } - // Create and populate the blackboard + // Create the tree with the specified ID blackboard = setBlackboardVariables(); - - // Build the tree from the ID (resolved from or ) try { - if(!is_bt_id) { - tree = factory_.createTreeFromFile(file_or_id, blackboard); - RCLCPP_WARN(node_->get_logger(), - "Loading BT using file path. This is deprecated. Please use the BT ID instead."); - } else { - tree = factory_.createTree(file_or_id, blackboard); - } + tree = factory_.createTree(main_id, blackboard); + std::cout << "Created BT from ID: " << main_id << "\n"; } catch (BT::RuntimeError & exp) { - RCLCPP_ERROR(node_->get_logger(), - "Failed to create BT %s: %s", file_or_id.c_str(), exp.what()); + std::cerr << "Failed to create BT " << main_id << ": " << exp.what() << "\n"; return false; } @@ -287,6 +300,7 @@ TEST_F(BehaviorTreeTestFixture, TestWrongBTFormatXML) write_file(main_file, "\n" "\n" + " \n" " \n" " \n" " \n" @@ -378,27 +392,164 @@ TEST_F(BehaviorTreeTestFixture, TestExtractBehaviorTreeID) std::remove(no_id_attr.c_str()); } -TEST_F(BehaviorTreeTestFixture, TestLoadBehaviorTreeMissingAndDuplicateIDs) -{ +TEST_F(BehaviorTreeTestFixture, TestDuplicateIDsWithFileSpecified) { auto write_file = [](const std::string & path, const std::string & content) { std::ofstream ofs(path); ofs << content; }; + std::string tmp_dir = "/tmp/bt_test_dup_file"; + std::filesystem::create_directories(tmp_dir); + + std::string dup1_file = tmp_dir + "/dup1.xml"; + std::string dup2_file = tmp_dir + "/dup2.xml"; + std::string dup_bt_content = + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"; + write_file(dup1_file, dup_bt_content); + write_file(dup2_file, dup_bt_content); + + std::stringstream captured_output; + std::streambuf * old_cout = std::cout.rdbuf(); + std::streambuf * old_cerr = std::cerr.rdbuf(); + std::cout.rdbuf(captured_output.rdbuf()); + std::cerr.rdbuf(captured_output.rdbuf()); + + bool result = bt_handler->loadBehaviorTree(dup1_file, {tmp_dir}); + + std::cout.rdbuf(old_cout); + std::cerr.rdbuf(old_cerr); + + std::string log_output = captured_output.str(); + std::cout << "Captured:\n" << log_output << std::endl; + + EXPECT_TRUE(result); + + bool found_conflict = + log_output.find("Skipping conflicting BT file \"" + dup2_file + + "\" (duplicate ID DuplicateTree)") != std::string::npos; + EXPECT_TRUE(found_conflict); + + EXPECT_NE(log_output.find("Registering Tree from File"), std::string::npos); + EXPECT_NE(log_output.find("Skipping conflicting BT file"), std::string::npos) + << "Should warn about duplicate ID"; + EXPECT_NE(log_output.find("Created BT from ID: DuplicateTree"), std::string::npos); - std::string tmp_dir = "/tmp/bt_test_dir"; + std::filesystem::remove_all(tmp_dir); +} + +TEST_F(BehaviorTreeTestFixture, TestAllUniqueIDsWithFileSpecified) { + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + std::string tmp_dir = "/tmp/bt_test_unique_file"; std::filesystem::create_directories(tmp_dir); - // 1. File with missing ID (should be skipped) - std::string missing_id_file = tmp_dir + "/missing_id.xml"; - write_file(missing_id_file, + // Two unique BT files + std::string file1 = tmp_dir + "/tree1.xml"; + std::string file2 = tmp_dir + "/tree2.xml"; + + std::string bt_content1 = "\n" "\n" - " \n" + " \n" " \n" " \n" - "\n"); + "\n"; + std::string bt_content2 = + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"; + + write_file(file1, bt_content1); + write_file(file2, bt_content2); + + // Redirect streams + std::stringstream captured_output; + std::streambuf * old_cout = std::cout.rdbuf(); + std::streambuf * old_cerr = std::cerr.rdbuf(); + std::cout.rdbuf(captured_output.rdbuf()); + std::cerr.rdbuf(captured_output.rdbuf()); + + bool result = bt_handler->loadBehaviorTree(file1, {tmp_dir}); + + std::cout.rdbuf(old_cout); + std::cerr.rdbuf(old_cerr); + + std::string log_output = captured_output.str(); + EXPECT_TRUE(result); + + EXPECT_NE(log_output.find("Registering Tree from File: " + file2), std::string::npos); + EXPECT_NE(log_output.find("Registering Tree from File: " + file1), std::string::npos); + + std::filesystem::remove_all(tmp_dir); +} + +TEST_F(BehaviorTreeTestFixture, TestAllUniqueIDsWithIDSpecified) { + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + std::string tmp_dir = "/tmp/bt_test_unique_id"; + std::filesystem::create_directories(tmp_dir); + + // Two unique BT files + std::string file1 = tmp_dir + "/tree1.xml"; + std::string file2 = tmp_dir + "/tree2.xml"; + + std::string bt_content1 = + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"; + std::string bt_content2 = + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"; + + write_file(file1, bt_content1); + write_file(file2, bt_content2); + + std::stringstream captured_output; + std::streambuf * old_cout = std::cout.rdbuf(); + std::streambuf * old_cerr = std::cerr.rdbuf(); + std::cout.rdbuf(captured_output.rdbuf()); + std::cerr.rdbuf(captured_output.rdbuf()); + + bool result = bt_handler->loadBehaviorTree("Tree1", {tmp_dir}); + + std::cout.rdbuf(old_cout); + std::cerr.rdbuf(old_cerr); + + std::string log_output = captured_output.str(); + EXPECT_TRUE(result); + + EXPECT_NE(log_output.find("Registering Tree from File: " + file2), std::string::npos); + EXPECT_NE(log_output.find("Created BT from ID: Tree1"), std::string::npos); + + std::filesystem::remove_all(tmp_dir); +} + +TEST_F(BehaviorTreeTestFixture, TestDuplicateIDsWithIDSpecified) { + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + std::string tmp_dir = "/tmp/bt_test_dup_id"; + std::filesystem::create_directories(tmp_dir); - // 2. Two files with the same ID (should trigger duplicate warning) std::string dup1_file = tmp_dir + "/dup1.xml"; std::string dup2_file = tmp_dir + "/dup2.xml"; std::string dup_bt_content = @@ -411,53 +562,94 @@ TEST_F(BehaviorTreeTestFixture, TestLoadBehaviorTreeMissingAndDuplicateIDs) write_file(dup1_file, dup_bt_content); write_file(dup2_file, dup_bt_content); - // Redirect cout and cerr to a stringstream std::stringstream captured_output; - std::streambuf * old_cout_buf = std::cout.rdbuf(); - std::streambuf * old_cerr_buf = std::cerr.rdbuf(); - + std::streambuf * old_cout = std::cout.rdbuf(); + std::streambuf * old_cerr = std::cerr.rdbuf(); std::cout.rdbuf(captured_output.rdbuf()); std::cerr.rdbuf(captured_output.rdbuf()); - std::vector search_dirs = {tmp_dir}; - bool result = bt_handler->loadBehaviorTree("DuplicateTree", search_dirs); + bool result = bt_handler->loadBehaviorTree("DuplicateTree", {tmp_dir}); - // Restore cout and cerr - std::cout.rdbuf(old_cout_buf); - std::cerr.rdbuf(old_cerr_buf); + std::cout.rdbuf(old_cout); + std::cerr.rdbuf(old_cerr); - // Check the captured output for the expected log messages std::string log_output = captured_output.str(); + std::cout << "Captured:\n" << log_output << std::endl; - // Assert that the function returned true - EXPECT_TRUE(result); + EXPECT_TRUE(result) << "Tree should still load despite duplicate IDs"; - // Assert that the error log for the missing ID was found - EXPECT_NE(log_output.find("[behavior_tree_handler]: Skipping BT file " + missing_id_file + - " (missing ID)"), std::string::npos); + EXPECT_NE(log_output.find("Registering Tree from File"), std::string::npos) + << "Should have registered at least one BT file"; + EXPECT_NE(log_output.find("Skipping conflicting BT file"), std::string::npos) + << "Should warn about duplicate IDs"; + EXPECT_NE(log_output.find("Created BT from ID: DuplicateTree"), std::string::npos) + << "Should have created BT from the given ID"; + +bool registered_dup1 = + log_output.find("Registering Tree from File: " + dup1_file) != std::string::npos; +bool registered_dup2 = + log_output.find("Registering Tree from File: " + dup2_file) != std::string::npos; + +EXPECT_TRUE(registered_dup1 || registered_dup2) + << "At least one duplicate file should have been registered"; +EXPECT_FALSE(registered_dup1 && registered_dup2) + << "Only one of the duplicate files should be registered as the main tree"; +EXPECT_NE(log_output.find("Skipping conflicting BT file"), std::string::npos); +EXPECT_NE(log_output.find("Created BT from ID: DuplicateTree"), std::string::npos); - // Assert that the warning log for the duplicate ID was found - EXPECT_NE( - log_output.find( - "Warning: Duplicate BT IDs found. Make sure to have all BT IDs unique! " - "ID: DuplicateTree File: "), std::string::npos); - // Cleanup std::filesystem::remove_all(tmp_dir); } -TEST_F(BehaviorTreeTestFixture, TestLoadByIdInsteadOfFile) -{ - const auto root_dir = std::filesystem::path( - ament_index_cpp::get_package_share_directory("nav2_bt_navigator") - ) / "behavior_trees"; - std::vector search_directories = {root_dir.string()}; +TEST_F(BehaviorTreeTestFixture, TestSkipFilesWithMissingID) { + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + + std::string tmp_dir = "/tmp/bt_test_missing_id"; + std::filesystem::create_directories(tmp_dir); + + // File with missing ID + std::string no_id_file = tmp_dir + "/no_id.xml"; + write_file(no_id_file, + "\n" + "\n" + " \n" // No ID attribute + " \n" + " \n" + "\n"); - // Load by BT ID instead of file - EXPECT_TRUE(bt_handler->loadBehaviorTree("NavigateToPoseWReplanningAndRecovery", - search_directories)); + std::string valid_file = tmp_dir + "/valid.xml"; + write_file(valid_file, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + std::stringstream captured_output; + std::streambuf * old_cout = std::cout.rdbuf(); + std::streambuf * old_cerr = std::cerr.rdbuf(); + std::cout.rdbuf(captured_output.rdbuf()); + std::cerr.rdbuf(captured_output.rdbuf()); + + bool result = bt_handler->loadBehaviorTree(valid_file, {tmp_dir}); + + std::cout.rdbuf(old_cout); + std::cerr.rdbuf(old_cerr); + + std::string log_output = captured_output.str(); + + EXPECT_TRUE(result); + EXPECT_NE(log_output.find("Skipping BT file"), std::string::npos); + EXPECT_NE(log_output.find("(missing ID)"), std::string::npos); + + std::filesystem::remove_all(tmp_dir); } + /** * Test scenario: *