Merge pull request #7 from ros-navigation/main

Up to date with main

Author: SakshayMahna

nav2_behavior_tree/CMakeLists.txt

@@ -120,6 +120,9 @@ list(APPEND plugin_libs nav2_goal_updated_condition_bt_node)
 add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp)
 list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node)
 
+add_library(nav2_is_pose_occupied_condition_bt_node SHARED plugins/condition/is_pose_occupied_condition.cpp)
+list(APPEND plugin_libs nav2_is_pose_occupied_condition_bt_node)
+
 add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
 list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
 
@@ -255,6 +258,9 @@ list(APPEND plugin_libs nav2_compute_and_track_route_bt_node)
 add_library(nav2_compute_route_bt_node SHARED plugins/action/compute_route_action.cpp)
 list(APPEND plugin_libs nav2_compute_route_bt_node)
 
+add_library(nav2_toggle_collision_monitor_service_bt_node SHARED plugins/action/toggle_collision_monitor_service.cpp)
+list(APPEND plugin_libs nav2_toggle_collision_monitor_service_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
   target_include_directories(${bt_plugin}
     PRIVATE

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

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<std::string> 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;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -21,6 +21,7 @@
 #include <limits>
 #include <memory>
 #include <set>
+#include <unordered_set>
 #include <string>
 #include <vector>
 
@@ -45,7 +46,7 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
   OnCompletionCallback on_completion_callback,
   const std::vector<std::string> & search_directories)
 : 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),
@@ -60,21 +61,7 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
   logger_ = node->get_logger();
   clock_ = node->get_clock();
 
-  // Declare this node's parameters
-  if (!node->has_parameter("bt_loop_duration")) {
-    node->declare_parameter("bt_loop_duration", 10);
-  }
-  if (!node->has_parameter("default_server_timeout")) {
-    node->declare_parameter("default_server_timeout", 20);
-  }
-  if (!node->has_parameter("always_reload_bt_xml")) {
-    node->declare_parameter("always_reload_bt_xml", false);
-  }
-  if (!node->has_parameter("wait_for_service_timeout")) {
-    node->declare_parameter("wait_for_service_timeout", 1000);
-  }
-
-  std::vector<std::string> error_code_name_prefixes = {
+  std::vector<std::string> default_error_code_name_prefixes = {
     "assisted_teleop",
     "backup",
     "compute_path",
@@ -94,32 +81,26 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
       " Please review migration guide and update your configuration.");
   }
 
-  if (!node->has_parameter("error_code_name_prefixes")) {
-    const rclcpp::ParameterValue value = node->declare_parameter(
-      "error_code_name_prefixes",
-      rclcpp::PARAMETER_STRING_ARRAY);
-    if (value.get_type() == rclcpp::PARAMETER_NOT_SET) {
-      std::string error_code_name_prefixes_str;
-      for (const auto & error_code_name_prefix : error_code_name_prefixes) {
-        error_code_name_prefixes_str += " " + error_code_name_prefix;
-      }
-      RCLCPP_WARN_STREAM(
-        logger_, "error_code_name_prefixes parameters were not set. Using default values of:"
-          << error_code_name_prefixes_str + "\n"
-          << "Make sure these match your BT and there are not other sources of error codes you"
-          "reported to your application");
-      rclcpp::Parameter error_code_name_prefixes_param("error_code_name_prefixes",
-        error_code_name_prefixes);
-      node->set_parameter(error_code_name_prefixes_param);
-    } else {
-      error_code_name_prefixes = value.get<std::vector<std::string>>();
-      std::string error_code_name_prefixes_str;
-      for (const auto & error_code_name_prefix : error_code_name_prefixes) {
-        error_code_name_prefixes_str += " " + error_code_name_prefix;
-      }
-      RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:"
-        << error_code_name_prefixes_str);
-    }
+  // Declare and get error code name prefixes parameter
+  error_code_name_prefixes_ = node->declare_or_get_parameter(
+    "error_code_name_prefixes",
+    default_error_code_name_prefixes);
+
+  // Provide informative logging about error code prefixes
+  std::string error_code_name_prefixes_str;
+  for (const auto & error_code_name_prefix : error_code_name_prefixes_) {
+    error_code_name_prefixes_str += " " + error_code_name_prefix;
+  }
+
+  if (error_code_name_prefixes_ == default_error_code_name_prefixes) {
+    RCLCPP_WARN_STREAM(
+      logger_, "error_code_name_prefixes parameters were not set. Using default values of:"
+        << error_code_name_prefixes_str + "\n"
+        << "Make sure these match your BT and there are not other sources of error codes you"
+        << "reported to your application");
+  } else {
+    RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:"
+      << error_code_name_prefixes_str);
   }
 }
 
@@ -169,16 +150,17 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
     nullptr, std::chrono::milliseconds(500), false);
 
   // Get parameters for BT timeouts
-  int bt_loop_duration;
-  node->get_parameter("bt_loop_duration", bt_loop_duration);
-  bt_loop_duration_ = std::chrono::milliseconds(bt_loop_duration);
-  int default_server_timeout;
-  node->get_parameter("default_server_timeout", default_server_timeout);
-  default_server_timeout_ = std::chrono::milliseconds(default_server_timeout);
-  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_);
+  bt_loop_duration_ = std::chrono::milliseconds(
+    node->declare_or_get_parameter("bt_loop_duration", 10));
+
+  default_server_timeout_ = std::chrono::milliseconds(
+    node->declare_or_get_parameter("default_server_timeout", 20));
+
+  wait_for_service_timeout_ = std::chrono::milliseconds(
+    node->declare_or_get_parameter("wait_for_service_timeout", 1000));
+
+  always_reload_bt_ = node->declare_or_get_parameter(
+    "always_reload_bt_xml", false);
 
   // 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 +186,8 @@ template<class ActionT, class NodeT>
 bool BtActionServer<ActionT, NodeT>::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 +210,7 @@ bool BtActionServer<ActionT, NodeT>::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 +228,49 @@ void BtActionServer<ActionT, NodeT>::setGrootMonitoring(
 }
 
 template<class ActionT, class NodeT>
-bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml_filename)
+bool BtActionServer<ActionT, NodeT>::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<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") {
-          // 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 +280,21 @@ bool BtActionServer<ActionT, NodeT>::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<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
       blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
       blackboard->set<std::chrono::milliseconds>(
-        "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 +304,7 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
 
   // Optional logging and monitoring
   topic_logger_ = std::make_unique<RosTopicLogger>(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_);