diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 96fb5df0dce..b17a914a541 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/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 diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index d5d1fa4c3a4..8e345abe0d7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/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 & plugin_lib_names, const std::string & default_bt_xml_filename, + const std::vector & 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 search_directories_; // The wrapper class for the BT functionality std::unique_ptr bt_; 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 184f3d94aca..b8fb983f91c 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 @@ -39,12 +39,14 @@ BtActionServer::BtActionServer( const std::string & action_name, const std::vector & plugin_lib_names, const std::string & default_bt_xml_filename, + const std::vector & 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::setGrootMonitoring( template bool BtActionServer::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::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::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(client_node_, tree_); current_bt_xml_filename_ = filename; - // Enable monitoring with Groot2 if (enable_groot_monitoring_) { bt_->addGrootMonitoring(&tree_, groot_server_port_); RCLCPP_DEBUG( diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 21131be010a..a2b93f7d086 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/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, diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 23e509760cc..e66d409478f 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/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 diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 1127c3ab2cd..5b8bbbc5b30 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/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."); diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 03d77451e16..a3b64ec2cc2 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/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."); diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index fda64cec974..9e984bc6550 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -201,6 +201,12 @@ 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{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>( @@ -208,6 +214,7 @@ class BehaviorTreeNavigator : public NavigatorBase 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), 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 d36535924e0..0c23468dc45 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 @@ -71,55 +71,80 @@ class BehaviorTreeHandler } } - bool loadBehaviorTree(const std::string & filename) + BT::Blackboard::Ptr setBlackboardVariables() { - // Read the input BT XML from the specified file into a string - std::ifstream xml_file(filename); + // Create and populate the blackboard + blackboard = BT::Blackboard::create(); + blackboard->set("node", node_); + blackboard->set("server_timeout", std::chrono::milliseconds(20)); + blackboard->set("bt_loop_duration", std::chrono::milliseconds(10)); + blackboard->set("wait_for_service_timeout", + std::chrono::milliseconds(1000)); + blackboard->set("tf_buffer", tf_); + blackboard->set("initial_pose_received", false); + blackboard->set("number_recoveries", 0); + blackboard->set("odom_smoother", odom_smoother_); + + // Create dummy goal + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.header.frame_id = "map"; + blackboard->set("goal", goal); + return blackboard; + } + bool behaviorTreeFileValidation( + const std::string & filename) + { + std::ifstream xml_file(filename); if (!xml_file.good()) { - RCLCPP_ERROR(node_->get_logger(), "Couldn't open input XML file: %s", filename.c_str()); + RCLCPP_ERROR(node_->get_logger(), + "Couldn't open BT XML file: %s", filename.c_str()); return false; } + return true; + } - std::stringstream buffer; - buffer << xml_file.rdbuf(); - xml_file.close(); - std::string xml_string = buffer.str(); - // Create the blackboard that will be shared by all of the nodes in the tree - blackboard = BT::Blackboard::create(); - // Put items on the blackboard - blackboard->set("node", node_); // NOLINT - blackboard->set( - "server_timeout", std::chrono::milliseconds(20)); // NOLINT - blackboard->set( - "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT - blackboard->set( - "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT - blackboard->set("tf_buffer", tf_); // NOLINT - blackboard->set("initial_pose_received", false); // NOLINT - blackboard->set("number_recoveries", 0); // NOLINT - blackboard->set("odom_smoother", odom_smoother_); // NOLINT - - // set dummy goal on blackboard - geometry_msgs::msg::PoseStamped goal; - goal.header.stamp = node_->now(); - goal.header.frame_id = "map"; - goal.pose.position.x = 0.0; - goal.pose.position.y = 0.0; - goal.pose.position.z = 0.0; - goal.pose.orientation.x = 0.0; - goal.pose.orientation.y = 0.0; - goal.pose.orientation.z = 0.0; - goal.pose.orientation.w = 1.0; + bool loadBehaviorTree( + const std::string & filename, + const std::vector & search_directories) + { + if (!behaviorTreeFileValidation(filename)) { + return false; + } + + namespace fs = std::filesystem; + 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; + } + factory_.registerBehaviorTreeFromFile(entry.path().string()); + } + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), + "Exception reading behavior tree directory: %s", e.what()); + return false; + } + } - blackboard->set("goal", goal); // NOLINT + // Create and populate the blackboard + blackboard = setBlackboardVariables(); - // Create the Behavior Tree from the XML input + // Build the tree from the XML string try { - tree = factory_.createTreeFromText(xml_string, blackboard); + tree = factory_.createTreeFromFile(filename, blackboard); } catch (BT::RuntimeError & exp) { - RCLCPP_ERROR(node_->get_logger(), "%s: %s", filename.c_str(), exp.what()); + RCLCPP_ERROR(node_->get_logger(), "Failed to create BT from %s: %s", filename.c_str(), + exp.what()); return false; } @@ -196,19 +221,75 @@ std::shared_ptr BehaviorTreeTestFixture::bt_handler = nullp TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) { - std::filesystem::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - root /= "behavior_trees/"; - - if (std::filesystem::exists(root) && std::filesystem::is_directory(root)) { - for (auto const & entry : std::filesystem::recursive_directory_iterator(root)) { - if (std::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") { - std::cout << entry.path().string() << std::endl; - EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true); - } + // Get the BT root directory + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + + ASSERT_TRUE(std::filesystem::exists(root_dir)); + ASSERT_TRUE(std::filesystem::is_directory(root_dir)); + + std::vector search_directories = {root_dir.string()}; + + for (auto const & entry : std::filesystem::recursive_directory_iterator(root_dir)) { + if (entry.is_regular_file() && entry.path().extension() == ".xml") { + std::string main_bt = entry.path().string(); + std::cout << "Testing BT file: " << main_bt << std::endl; + + EXPECT_TRUE(bt_handler->loadBehaviorTree(main_bt, search_directories)) + << "Failed to load: " << main_bt; } } } +TEST_F(BehaviorTreeTestFixture, TestWrongBTFormatXML) +{ + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + + // File paths + std::string valid_subtree = "/tmp/valid_subtree.xml"; + std::string invalid_subtree = "/tmp/invalid_subtree.xml"; + std::string main_file = "/tmp/test_main_tree.xml"; + std::string malformed_main = "/tmp/malformed_main.xml"; + + // Valid subtree + write_file(valid_subtree, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + // Invalid subtree (malformed XML) + write_file(invalid_subtree, ""); + + // Main tree referencing the valid subtree + write_file(main_file, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + // Malformed main tree + write_file(malformed_main, ""); + + std::vector search_directories = {"/tmp"}; + + EXPECT_FALSE(bt_handler->loadBehaviorTree(main_file, search_directories)); + EXPECT_FALSE(bt_handler->loadBehaviorTree(malformed_main, search_directories)); + + std::remove(valid_subtree.c_str()); + std::remove(main_file.c_str()); + std::remove(invalid_subtree.c_str()); + std::remove(malformed_main.c_str()); +} + /** * Test scenario: * @@ -218,10 +299,14 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) TEST_F(BehaviorTreeTestFixture, TestAllSuccess) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); BT::NodeStatus result = BT::NodeStatus::RUNNING; @@ -265,10 +350,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess) TEST_F(BehaviorTreeTestFixture, TestAllFailure) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set all action server to fail the first 100 times Ranges failureRange; @@ -321,10 +410,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure) TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set ComputePathToPose and FollowPath action servers to fail for the first action Ranges failureRange; @@ -380,10 +473,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set ComputePathToPose action server to fail for the first action Ranges plannerFailureRange; @@ -478,10 +575,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set FollowPath action server to fail for the first 2 actions Ranges controllerFailureRange; @@ -546,10 +647,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set FollowPath action server to fail for the first 2 actions Ranges controllerFailureRange;