Added promixity BT node and BT tree

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v36\
+              - "<< parameters.key >>-v39\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v36\
+              - "<< parameters.key >>-v39\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v36\
+            key: "<< parameters.key >>-v39\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\
@@ -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

.github/workflows/build_main_against_distros.yml

@@ -17,7 +17,7 @@ jobs:
 
     steps:
     - name: Checkout repository
-      uses: actions/checkout@v4
+      uses: actions/checkout@v5
 
     - name: Set up Docker build context
       run: |

.github/workflows/claude.yml

@@ -34,7 +34,7 @@ jobs:
       id-token: write
     steps:
       - name: Checkout repository
-        uses: actions/checkout@v4
+        uses: actions/checkout@v5
         with:
           fetch-depth: 1
 

.github/workflows/lint.yml

@@ -13,7 +13,7 @@ jobs:
       matrix:
           linter: [xmllint, cpplint, uncrustify, pep257, flake8, mypy]
     steps:
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v5
 
     - name: Install typeshed for mypy
       if: matrix.linter == 'mypy'
@@ -30,8 +30,8 @@ jobs:
     name: pre-commit
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
-      - uses: actions/setup-python@v5
+      - uses: actions/checkout@v5
+      - uses: actions/setup-python@v6
       - uses: pre-commit/action@v3.0.1
         env:
           SKIP: >-

.github/workflows/update_ci_image.yaml

@@ -66,7 +66,7 @@ jobs:
       - check_ci_image
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v3
       - name: Login to Docker Hub

README.md

@@ -8,16 +8,16 @@
 </p>
 
 For detailed instructions on how to:
-- [Getting Started](https://docs.nav2.org/getting_started/index.html)
-- [Concepts](https://docs.nav2.org/concepts/index.html)
-- [Build](https://docs.nav2.org/development_guides/build_docs/index.html#build)
-- [Install](https://docs.nav2.org/development_guides/build_docs/index.html#install)
+- [Concepts](https://docs.nav2.org/concepts/index.html) and [Getting Started](https://docs.nav2.org/getting_started/index.html)
+- [First Time Setup Guide](https://docs.nav2.org/setup_guides/index.html)
+- [ROS Distribution Statuses](https://docs.nav2.org/#distributions)
+- [Build & Install](https://docs.nav2.org/development_guides/build_docs/index.html#build) and [Docker Containers](https://github.com/orgs/ros-navigation/packages/container/package/navigation2)
 - [General Tutorials](https://docs.nav2.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://docs.nav2.org/plugin_tutorials/index.html)
-- [Configure](https://docs.nav2.org/configuration/index.html)
+- [Configuration Guide](https://docs.nav2.org/configuration/index.html)
 - [Navigation Plugins](https://docs.nav2.org/plugins/index.html)
-- [ROSCon Talks](https://docs.nav2.org/about/roscon.html)
+- [API Docs](https://api.nav2.org/)
+- [ROSCon Talks](https://docs.nav2.org/about/roscon.html) and [Citations](https://docs.nav2.org/citations.html)
 - [Migration Guides](https://docs.nav2.org/migration/index.html)
-- [Container Images for Building Nav2](https://github.com/orgs/ros-navigation/packages/container/package/navigation2)
 - [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html)
 
 Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-uj428p0x-jKx8U7OzK1IOWp5TnDS2rA) (if this link does not work, please contact maintainers to reactivate).
@@ -29,7 +29,7 @@ Please visit our [documentation site](https://docs.nav2.org/). [Please visit our
 Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.
 
 <p align="center">
-  <img src="doc/sponsors_july_2025.png" />
+  <img src="doc/sponsors_sept_2025.png" />
 </p>
 
 ### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights.
@@ -40,6 +40,8 @@ Please thank our amazing sponsors for their generous support of Nav2 on behalf o
 
 ### [Stereolabs](https://www.stereolabs.com/) produces the high-quality ZED stereo cameras with a complete vision pipeline from neural depth to SLAM, 3D object tracking, AI and more.
 
+### [3Laws Robotics](https://3laws.io/) provide Supervisor ROS and Pro, easy-to-use dynamic collision avoidance solutions to improve safety and application throughput.
+
 ## Citation
 
 If you use the navigation framework, an algorithm from this repository, or ideas from it

nav2_amcl/CMakeLists.txt

@@ -1,7 +1,8 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_amcl)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(nav_msgs REQUIRED)

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -44,9 +44,9 @@
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "nav2_ros_common/service_server.hpp"
 #include "std_srvs/srv/empty.hpp"
-#include "tf2_ros/message_filter.h"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/message_filter.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/transform_listener.hpp"
 
 #define NEW_UNIFORM_SAMPLING 1
 

nav2_amcl/package.xml

@@ -24,6 +24,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>message_filters</depend>
   <depend>nav_msgs</depend>

nav2_amcl/src/amcl_node.cpp

@@ -38,11 +38,11 @@
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/LinearMath/Transform.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/message_filter.h"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/message_filter.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 
 #include "nav2_amcl/portable_utils.hpp"
 #include "nav2_ros_common/validate_messages.hpp"
@@ -1339,7 +1339,7 @@ AmclNode::initTransforms()
     get_node_timers_interface(),
     callback_group_);
   tf_buffer_->setCreateTimerInterface(timer_interface);
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this, true);
   tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(shared_from_this());
 
   sent_first_transform_ = false;

nav2_behavior_tree/CMakeLists.txt

@@ -1,7 +1,8 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_behavior_tree CXX)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(action_msgs REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -18,6 +19,7 @@ find_package(std_srvs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_ros_common REQUIRED)
+find_package(angles REQUIRED)
 
 nav2_package()
 
@@ -32,6 +34,7 @@ target_include_directories(${library_name}
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${library_name} PUBLIC
   ${action_msgs_TARGETS}
+  angles::angles
   behaviortree_cpp::behaviortree_cpp
   ${geometry_msgs_TARGETS}
   ${nav_msgs_TARGETS}
@@ -122,8 +125,8 @@ list(APPEND plugin_libs nav2_is_path_valid_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)
 
-add_library(nav2_path_expiring_timer_condition SHARED plugins/condition/path_expiring_timer_condition.cpp)
-list(APPEND plugin_libs nav2_path_expiring_timer_condition)
+add_library(nav2_path_expiring_timer_condition_bt_node SHARED plugins/condition/path_expiring_timer_condition.cpp)
+list(APPEND plugin_libs nav2_path_expiring_timer_condition_bt_node)
 
 add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp)
 list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
@@ -218,6 +221,12 @@ list(APPEND plugin_libs nav2_nonblocking_sequence_bt_node)
 add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp)
 list(APPEND plugin_libs nav2_round_robin_node_bt_node)
 
+add_library(nav2_pause_resume_controller_bt_node SHARED plugins/control/pause_resume_controller.cpp)
+list(APPEND plugin_libs nav2_pause_resume_controller_bt_node)
+
+add_library(nav2_persistent_sequence_bt_node SHARED plugins/control/persistent_sequence.cpp)
+list(APPEND plugin_libs nav2_persistent_sequence_bt_node)
+
 add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp)
 list(APPEND plugin_libs nav2_single_trigger_bt_node)
 
@@ -268,6 +277,7 @@ foreach(bt_plugin ${plugin_libs})
     ${std_msgs_TARGETS}
     ${std_srvs_TARGETS}
     nav2_ros_common::nav2_ros_common
+    angles::angles
   )
   target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
 endforeach()

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

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -56,7 +56,8 @@ class BtActionServer
     OnGoalReceivedCallback on_goal_received_callback,
     OnLoopCallback on_loop_callback,
     OnPreemptCallback on_preempt_callback,
-    OnCompletionCallback on_completion_callback);
+    OnCompletionCallback on_completion_callback,
+    const std::vector<std::string> & search_directories = std::vector<std::string>{});
 
   /**
    * @brief A destructor for nav2_behavior_tree::BtActionServer class
@@ -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<std::string> search_directories_;
 
   // The wrapper class for the BT functionality
   std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -42,9 +42,11 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
   OnGoalReceivedCallback on_goal_received_callback,
   OnLoopCallback on_loop_callback,
   OnPreemptCallback on_preempt_callback,
-  OnCompletionCallback on_completion_callback)
+  OnCompletionCallback on_completion_callback,
+  const std::vector<std::string> & search_directories)
 : 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<ActionT, NodeT>::setGrootMonitoring(
 template<class ActionT, class NodeT>
 bool BtActionServer<ActionT, NodeT>::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<ActionT, NodeT>::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<ActionT, NodeT>::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<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(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -102,6 +103,7 @@ class ControllerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -102,6 +103,7 @@ class GoalCheckerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree