ros-navigation · SteveMacenski · Aug 20, 2025 · Jun 9, 2025 · Jun 10, 2025 · Jun 12, 2025
diff --git a/.circleci/config.yml b/.circleci/config.yml
@@ -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

diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -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

diff --git a/nav2_amcl/src/motion_model/differential_motion_model.cpp b/nav2_amcl/src/motion_model/differential_motion_model.cpp
@@ -19,6 +19,7 @@
  *
  */
 
+#include <algorithm>
 #include <cmath>
 
 #include "nav2_amcl/angleutils.hpp"

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
@@ -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
@@ -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

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
@@ -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(

diff --git a/...ehavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp b/...ehavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp
@@ -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

diff --git a/.../include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp b/.../include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp
@@ -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(

diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -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">

diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
@@ -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>

diff --git a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp
@@ -26,6 +26,14 @@ ComputeAndTrackRouteAction::ComputeAndTrackRouteAction(
   const BT::NodeConfiguration & conf)
 : BtActionNode<Action>(xml_tag_name, action_name, conf)
 {
+  nav_msgs::msg::Path empty_path;
+  nav2_msgs::msg::Route empty_route;
+  feedback_.last_node_id = 0;
+  feedback_.next_node_id = 0;
+  feedback_.current_edge_id = 0;
+  feedback_.route = empty_route;
+  feedback_.path = empty_path;
+  feedback_.rerouted = false;
 }
 
 void ComputeAndTrackRouteAction::on_tick()
@@ -52,6 +60,7 @@ void ComputeAndTrackRouteAction::on_tick()
 
 BT::NodeStatus ComputeAndTrackRouteAction::on_success()
 {
+  resetFeedbackAndOutputPorts();
   setOutput("execution_duration", result_.result->execution_duration);
   setOutput("error_code_id", ActionResult::NONE);
   setOutput("error_msg", "");
@@ -60,6 +69,7 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_success()
 
 BT::NodeStatus ComputeAndTrackRouteAction::on_aborted()
 {
+  resetFeedbackAndOutputPorts();
   setOutput("execution_duration", builtin_interfaces::msg::Duration());
   setOutput("error_code_id", result_.result->error_code);
   setOutput("error_msg", result_.result->error_msg);
@@ -68,6 +78,7 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_aborted()
 
 BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled()
 {
+  resetFeedbackAndOutputPorts();
   // Set empty error code, action was cancelled
   setOutput("execution_duration", builtin_interfaces::msg::Duration());
   setOutput("error_code_id", ActionResult::NONE);
@@ -82,7 +93,7 @@ void ComputeAndTrackRouteAction::on_timeout()
 }
 
 void ComputeAndTrackRouteAction::on_wait_for_result(
-  std::shared_ptr<const Action::Feedback>/*feedback*/)
+  std::shared_ptr<const Action::Feedback> feedback)
 {
   // Check for request updates to the goal
   bool use_poses = false, use_start = false;
@@ -129,6 +140,34 @@ void ComputeAndTrackRouteAction::on_wait_for_result(
   if (goal_updated_) {
     on_tick();
   }
+
+  if (feedback) {
+    feedback_ = *feedback;
+    setOutput("last_node_id", feedback_.last_node_id);
+    setOutput("next_node_id", feedback_.next_node_id);
+    setOutput("current_edge_id", feedback_.current_edge_id);
+    setOutput("route", feedback_.route);
+    setOutput("path", feedback_.path);
+    setOutput("rerouted", feedback_.rerouted);
+  }
+}
+
+void ComputeAndTrackRouteAction::resetFeedbackAndOutputPorts()
+{
+  nav_msgs::msg::Path empty_path;
+  nav2_msgs::msg::Route empty_route;
+  feedback_.last_node_id = 0;
+  feedback_.next_node_id = 0;
+  feedback_.current_edge_id = 0;
+  feedback_.route = empty_route;
+  feedback_.path = empty_path;
+  feedback_.rerouted = false;
+  setOutput("last_node_id", feedback_.last_node_id);
+  setOutput("next_node_id", feedback_.next_node_id);
+  setOutput("current_edge_id", feedback_.current_edge_id);
+  setOutput("route", feedback_.route);
+  setOutput("path", feedback_.path);
+  setOutput("rerouted", feedback_.rerouted);
 }
 
 }  // namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -101,6 +101,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_behaviors/package.xml b/nav2_behaviors/package.xml
@@ -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_behaviors</name>
-  <version>1.4.0</version>
+  <version>1.4.1</version>
   <description>Nav2 behavior server</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
@@ -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_bringup</name>
-  <version>1.4.0</version>
+  <version>1.4.1</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git 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/package.xml b/nav2_bt_navigator/package.xml
@@ -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_bt_navigator</name>
-  <version>1.4.0</version>
+  <version>1.4.1</version>
   <description>Nav2 BT Navigator Server</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
@@ -92,7 +92,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
@@ -99,7 +99,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_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
@@ -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_collision_monitor</name>
-  <version>1.4.0</version>
+  <version>1.4.1</version>
   <description>Collision Monitor</description>
   <maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
-Original file line number
+Diff line change
@@ Expand Up / @@ -19,6 +19,7 @@ @@
      *
      */
+    #include <algorithm>
     #include <cmath>
     #include "nav2_amcl/angleutils.hpp"
@@ Expand Down @@