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 b17a914a541..2371975317c 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 @@ -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 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 8e345abe0d7..9fe3b508b4e 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 @@ -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 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; 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 b8fb983f91c..104a3011f6a 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,6 +21,7 @@ #include #include #include +#include #include #include @@ -45,7 +46,7 @@ BtActionServer::BtActionServer( OnPreemptCallback on_preempt_callback, OnCompletionCallback on_completion_callback) : 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::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 bool BtActionServer::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::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::setGrootMonitoring( } template -bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) +bool BtActionServer::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 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::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("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); blackboard->set( - "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::loadBehaviorTree(const std::string & bt_xml // Optional logging and monitoring topic_logger_ = std::make_unique(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_); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index a2b93f7d086..68bb2676b99 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -18,6 +18,7 @@ #include #include #include +#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: 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 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) diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index ec16bf63051..5d5310f79cd 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -2,8 +2,8 @@ This Behavior Tree follows a dynamic pose to a certain distance --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index bffa6cee993..f2b6589d927 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/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. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml index 7df6d6e0cb6..44662cb4656 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml +++ b/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. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 6c32906f4f9..cb88cbb3eed 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/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. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index fdc96c7f62a..3fb6e4e5ff7 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/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. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index 70b9a930f5e..f79984b750e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/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. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index 23114197a16..c13b2629dea 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.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. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index cba0d5957c4..fa39ff05283 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -2,8 +2,8 @@ This Behavior Tree replans the global path after every 1m. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 9ffc88b699f..f64063bbdcb 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -2,8 +2,8 @@ This Behavior Tree replans the global path only when the goal is updated. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index 11b1050a936..1607057c0e2 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -1,8 +1,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 2fa10836816..9c9342e8bbd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -2,8 +2,8 @@ This Behavior Tree replans the global path periodically proprortional to speed. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 161fb29af0c..2e43016775d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -2,8 +2,8 @@ This Behavior Tree replans the global path periodically at 1 Hz. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml index c25970af506..4692c9e17b0 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml @@ -13,8 +13,8 @@ It also has recovery actions specific to planning / control as well as general system issues. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml index e66dded8745..a7f8760a336 100644 --- a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml +++ b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml @@ -2,8 +2,8 @@ his Behavior Tree drives in a square for odometry calibration experiments --> - - + + diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 9b8435ac4a3..92b6dfb7320 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -72,10 +72,9 @@ NavigateThroughPosesNavigator::getDefaultBTFilepath( bool NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { - auto bt_xml_filename = goal->behavior_tree; - if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { + if (!bt_action_server_->loadBehaviorTree(goal->behavior_tree)) { bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - "Error loading XML file: " + bt_xml_filename + ". Navigation canceled."); + "Error loading BT: " + goal->behavior_tree + ". Navigation canceled."); return false; } @@ -206,9 +205,9 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) { RCLCPP_INFO(logger_, "Received goal preemption request"); - if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() || + if (goal->behavior_tree == bt_action_server_->getCurrentBTFilenameOrID() || (goal->behavior_tree.empty() && - bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename())) + bt_action_server_->getCurrentBTFilenameOrID() == bt_action_server_->getDefaultBTFilenameOrID())) { // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index a9495645858..d4643a09ac8 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -82,10 +82,9 @@ NavigateToPoseNavigator::cleanup() bool NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { - auto bt_xml_filename = goal->behavior_tree; - if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { + if (!bt_action_server_->loadBehaviorTree(goal->behavior_tree)) { bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled."); + std::string("Error loading BT: ") + goal->behavior_tree + ". Navigation canceled."); return false; } @@ -186,9 +185,9 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) { RCLCPP_INFO(logger_, "Received goal preemption request"); - if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() || + if (goal->behavior_tree == bt_action_server_->getCurrentBTFilenameOrID() || (goal->behavior_tree.empty() && - bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename())) + bt_action_server_->getCurrentBTFilenameOrID() == bt_action_server_->getDefaultBTFilenameOrID())) { // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file 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 0c23468dc45..9a5f9a1826f 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 @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. +#include +#include +#include #include #include #include @@ -19,6 +22,8 @@ #include #include #include +#include +#include "tinyxml2.h" //NOLINT #include "gtest/gtest.h" @@ -35,6 +40,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_behavior_tree/plugins_list.hpp" +#include "nav2_behavior_tree/behavior_tree_engine.hpp" #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -65,6 +71,7 @@ class BehaviorTreeHandler odom_smoother_ = std::make_shared(node_); nav2_util::Tokens plugin_libs = nav2_util::split(nav2::details::BT_BUILTIN_PLUGINS, ';'); + bt_engine_ = std::make_shared(plugin_libs, node_); for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); @@ -93,39 +100,41 @@ class BehaviorTreeHandler return blackboard; } - bool behaviorTreeFileValidation( - const std::string & filename) + std::string extractBehaviorTreeID(const std::string & file_or_id) { - std::ifstream xml_file(filename); - if (!xml_file.good()) { - RCLCPP_ERROR(node_->get_logger(), - "Couldn't open BT XML file: %s", filename.c_str()); - return false; - } - return true; + return bt_engine_->extractBehaviorTreeID(file_or_id); } - bool loadBehaviorTree( - const std::string & filename, + const std::string & file_or_id, 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 + 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") { - // Skip registering the main tree file - if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) { + 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"; 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"; + } factory_.registerBehaviorTreeFromFile(entry.path().string()); } } @@ -139,12 +148,18 @@ class BehaviorTreeHandler // Create and populate the blackboard blackboard = setBlackboardVariables(); - // Build the tree from the XML string + // Build the tree from the ID (resolved from or ) try { - tree = factory_.createTreeFromFile(filename, blackboard); + 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); + } } catch (BT::RuntimeError & exp) { - RCLCPP_ERROR(node_->get_logger(), "Failed to create BT from %s: %s", filename.c_str(), - exp.what()); + RCLCPP_ERROR(node_->get_logger(), + "Failed to create BT %s: %s", file_or_id.c_str(), exp.what()); return false; } @@ -175,6 +190,8 @@ class BehaviorTreeHandler std::shared_ptr odom_smoother_; BT::BehaviorTreeFactory factory_; + + std::shared_ptr bt_engine_; }; class BehaviorTreeTestFixture : public ::testing::Test @@ -234,7 +251,6 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) 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; @@ -290,6 +306,158 @@ TEST_F(BehaviorTreeTestFixture, TestWrongBTFormatXML) std::remove(malformed_main.c_str()); } +TEST_F(BehaviorTreeTestFixture, TestExtractBehaviorTreeID) +{ + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + + // 1. Empty string input triggers "Empty file branch + auto empty_id = bt_handler->extractBehaviorTreeID(""); + EXPECT_TRUE(empty_id.empty()); + + // 2. Valid XML with ID + std::string valid_xml = "/tmp/extract_bt_id_valid.xml"; + write_file(valid_xml, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + auto id = bt_handler->extractBehaviorTreeID(valid_xml); + EXPECT_FALSE(id.empty()); + EXPECT_EQ(id, "TestTree"); + + // 3. Malformed XML (parser error) + std::string malformed_xml = "/tmp/extract_bt_id_malformed.xml"; + write_file(malformed_xml, ""); + auto missing_id = bt_handler->extractBehaviorTreeID(malformed_xml); + EXPECT_TRUE(missing_id.empty()); + + // 4. File does not exist + auto not_found = bt_handler->extractBehaviorTreeID("/tmp/does_not_exist.xml"); + EXPECT_TRUE(not_found.empty()); + + // 6. No root element + std::string no_root_file = "/tmp/extract_bt_id_no_root.xml"; + write_file(no_root_file, + "\n" + "\n"); + auto no_root_id = bt_handler->extractBehaviorTreeID(no_root_file); + EXPECT_TRUE(no_root_id.empty()); + + // 7. No child + std::string no_bt_element = "/tmp/extract_bt_id_no_bt.xml"; + write_file(no_bt_element, + "\n" + "\n" + " \n" + "\n"); + auto no_bt_id = bt_handler->extractBehaviorTreeID(no_bt_element); + EXPECT_TRUE(no_bt_id.empty()); + + // 8. No ID attribute + std::string no_id_attr = "/tmp/extract_bt_id_no_id.xml"; + write_file(no_id_attr, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + auto no_id = bt_handler->extractBehaviorTreeID(no_id_attr); + EXPECT_TRUE(no_id.empty()); + + // Cleanup + std::remove(valid_xml.c_str()); + std::remove(malformed_xml.c_str()); + std::remove(no_root_file.c_str()); + std::remove(no_bt_element.c_str()); + std::remove(no_id_attr.c_str()); +} + +TEST_F(BehaviorTreeTestFixture, TestLoadBehaviorTreeMissingAndDuplicateIDs) +{ + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + + std::string tmp_dir = "/tmp/bt_test_dir"; + 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, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + // 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 = + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"; + 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::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); + + // Restore cout and cerr + std::cout.rdbuf(old_cout_buf); + std::cerr.rdbuf(old_cerr_buf); + + // Check the captured output for the expected log messages + std::string log_output = captured_output.str(); + + // Assert that the function returned true + EXPECT_TRUE(result); + + // 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); + + // 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()}; + + // Load by BT ID instead of file + EXPECT_TRUE(bt_handler->loadBehaviorTree("NavigateToPoseWReplanningAndRecovery", + search_directories)); +} + /** * Test scenario: * diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py index 4cd1003d068..ba11a3e1ac3 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py @@ -283,8 +283,11 @@ def run_all_tests(robot_tester: NavTester) -> bool: goal_pose=pose_out_of_bounds, behavior_tree='behavior_tree_that_does_not_exist.xml', expected_error_code=NavigateThroughPoses.Result.FAILED_TO_LOAD_BEHAVIOR_TREE, - expected_error_msg=('Error loading XML file: behavior_tree_that_does_not_exist.xml. ' - 'Navigation canceled.')) + expected_error_msg=( + 'Error loading BT: behavior_tree_that_does_not_exist.xml. ' + 'Navigation canceled.' + ), + ) if result: robot_tester.info_msg('Test goal out of bounds')