ros-navigation
diff --git a/‎.circleci/config.yml‎
Lines changed: 1 addition & 1 deletion b/‎.circleci/config.yml‎
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_amcl/package.xml‎
Lines changed: 1 addition & 1 deletion b/‎nav2_amcl/package.xml‎
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_amcl/src/motion_model/differential_motion_model.cpp‎
Lines changed: 1 addition & 0 deletions b/‎nav2_amcl/src/motion_model/differential_motion_model.cpp‎
Lines changed: 1 addition & 0 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp‎
Lines changed: 6 additions & 0 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp‎
Lines changed: 6 additions & 0 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp‎
Lines changed: 5 additions & 2 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp‎
Lines changed: 5 additions & 2 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎
Lines changed: 30 additions & 7 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎
Lines changed: 30 additions & 7 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp‎
Lines changed: 28 additions & 1 deletion b/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp‎
Lines changed: 28 additions & 1 deletion
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp‎
Lines changed: 1 addition & 1 deletion b/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp‎
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_behavior_tree/nav2_tree_nodes.xml‎
Lines changed: 6 additions & 0 deletions b/‎nav2_behavior_tree/nav2_tree_nodes.xml‎
Lines changed: 6 additions & 0 deletions
diff --git a/‎nav2_behavior_tree/package.xml‎
Lines changed: 1 addition & 1 deletion b/‎nav2_behavior_tree/package.xml‎
Lines changed: 1 addition & 1 deletion
@@ -532,7 +532,7 @@ jobs:
 
 _parameters:
   release_parameters: &release_parameters
-    packages_skip_regex: "nav2_system_tests"
+    packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'"
 
 workflows:
   version: 2
 
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.4.0</version>
+  <version>1.4.1</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in
 
@@ -19,6 +19,7 @@
  *
  */
 
+#include <algorithm>
 #include <cmath>
 
 #include "nav2_amcl/angleutils.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
 
@@ -55,6 +55,7 @@ class BtActionServer
     const std::string & action_name,
     const std::vector<std::string> & plugin_lib_names,
     const std::string & default_bt_xml_filename,
+    const std::vector<std::string> & search_directories,
     OnGoalReceivedCallback on_goal_received_callback,
     OnLoopCallback on_loop_callback,
     OnPreemptCallback on_preempt_callback,
@@ -104,7 +105,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
@@ -247,6 +249,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<std::string> search_directories_;
 
   // The wrapper class for the BT functionality
   std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
@@ -285,7 +288,7 @@ class BtActionServer
   bool always_reload_bt_xml_ = false;
 
   // Parameters for Groot2 monitoring
-  bool enable_groot_monitoring_ = true;
+  bool enable_groot_monitoring_ = false;
   int groot_server_port_ = 1667;
 
   // User-provided callbacks
 
@@ -39,12 +39,14 @@ BtActionServer<ActionT>::BtActionServer(
   const std::string & action_name,
   const std::vector<std::string> & plugin_lib_names,
   const std::string & default_bt_xml_filename,
+  const std::vector<std::string> & 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),
@@ -245,6 +247,8 @@ void BtActionServer<ActionT>::setGrootMonitoring(const bool enable, const unsign
 template<class ActionT>
 bool BtActionServer<ActionT>::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;
 
@@ -254,19 +258,38 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     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) {
@@ -280,15 +303,15 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     }
   } 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<RosTopicLogger>(client_node_, tree_);
 
   current_bt_xml_filename_ = filename;
 
-  // Enable monitoring with Groot2
   if (enable_groot_monitoring_) {
     bt_->addGrootMonitoring(&tree_, groot_server_port_);
     RCLCPP_DEBUG(
 
@@ -78,6 +78,11 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
   void on_wait_for_result(
     std::shared_ptr<const Action::Feedback> feedback) override;
 
+  /**
+   * @brief Function to set all feedbacks and output ports to be null values
+   */
+  void resetFeedbackAndOutputPorts();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -98,14 +103,36 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
           "Whether to use the start pose or the robot's current pose"),
         BT::InputPort<bool>(
           "use_poses", false, "Whether to use poses or IDs for start and goal"),
-        BT::OutputPort<builtin_interfaces::msg::Duration>("execution_duration",
+        BT::OutputPort<builtin_interfaces::msg::Duration>(
+          "execution_duration",
           "Time taken to compute and track route"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The compute route error code"),
         BT::OutputPort<std::string>(
           "error_msg", "The compute route error msg"),
+        BT::OutputPort<uint16_t>(
+          "last_node_id",
+          "ID of the previous node"),
+        BT::OutputPort<uint16_t>(
+          "next_node_id",
+          "ID of the next node"),
+        BT::OutputPort<uint16_t>(
+          "current_edge_id",
+          "ID of current edge"),
+        BT::OutputPort<nav2_msgs::msg::Route>(
+          "route",
+          "List of RouteNodes to go from start to end"),
+        BT::OutputPort<nav_msgs::msg::Path>(
+          "path",
+          "Path created by ComputeAndTrackRoute node"),
+        BT::OutputPort<bool>(
+          "rerouted",
+          "Whether the plan has rerouted"),
       });
   }
+
+protected:
+  Action::Feedback feedback_;
 };
 
 }  // namespace nav2_behavior_tree
 
@@ -29,7 +29,7 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent
   using Action = nav2_msgs::action::ComputePathToPose;
   using ActionResult = Action::Result;
   using ThroughAction = nav2_msgs::action::ComputePathThroughPoses;
-  using ThroughActionResult = Action::Result;
+  using ThroughActionResult = ThroughAction::Result;
 
 public:
   WouldAPlannerRecoveryHelp(
 
@@ -128,6 +128,12 @@
       <output_port name="execution_time">Duration to compute route</output_port>
       <output_port name="error_code_id">"Compute route to pose error code"</output_port>
       <output_port name="error_msg">"Compute route to pose error message"</output_port>
+      <output_port name="last_node_id">"ID of the previous node"</output_port>
+      <output_port name="next_node_id">"ID of the next node"</output_port>
+      <output_port name="current_edge_id">"ID of the current edge"</output_port>
+      <output_port name="route">"List of RouteNodes to go from start to end"</output_port>
+      <output_port name="path">"Path created by ComputeAndTrackRoute node"</output_port>
+      <output_port name="rerouted">"Whether the plan has rerouted"</output_port>
     </Action>
 
     <Action ID="RemovePassedGoals">
 
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.4.0</version>
+  <version>1.4.1</version>
   <description>Nav2 behavior tree wrappers, nodes, and utilities</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
Original file line number	Diff line number	Diff line change
`@@ -19,6 +19,7 @@`
`19`	`19`	`*`
`20`	`20`	`*/`
`21`	`21`
	`22`	`+#include <algorithm>`
`22`	`23`	`#include <cmath>`
`23`	`24`
`24`	`25`	`#include "nav2_amcl/angleutils.hpp"`