Include goal heading mode to allow bidirectional and all direction goals

Signed-off-by: stevedan <[email protected]> New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> [behavior_tree] support both ports and blackboard (ros-navigation#4060) * check result of blackboard->get and use halTree Signed-off-by: Davide Faconti <[email protected]> * remove unused header Signed-off-by: Davide Faconti <[email protected]> * BT: add port inputs when missing and use getInputPortOrBlackboard Signed-off-by: Davide Faconti <[email protected]> * add description Signed-off-by: Davide Faconti <[email protected]> * change return value of getInputPortOrBlackboard Signed-off-by: Davide Faconti <[email protected]> * updated tree XML Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Use ament_export_targets for all targets (ros-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <[email protected]> adding new Friends of Nav2 Sponsors! Update README.md (ros-navigation#4114) Signed-off-by: Steve Macenski <[email protected]> CI green P3 (ros-navigation#4117) Refactors `possible_inscribed_cost` to `possible_circumscribed_cost` in collision checker for smac_planner and mppi_controller (ros-navigation#4113). (ros-navigation#4118) * Utilizes circumscribed radius to assess potential collision points on the robot. * Ensures safety when the robot's center is outside the circumscribed radius, eliminating additional checks and optimizing CPU usage. Signed-off-by: Alan Xue <[email protected]> Added cast to float to getClosestAngularBin return for behavior consistency (ros-navigation#4123) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song <[email protected]> * Revised test name Signed-off-by: Hunter Song <[email protected]> --------- Signed-off-by: Hunter Song <[email protected]> Adding context to MPPI readme Signed-off-by: Steve Macenski <[email protected]> [behavior_tree] Add generate_nav2_tree_nodes_xml (ros-navigation#4073) * Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti <[email protected]> * behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti <[email protected]> * check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti <[email protected]> * fix previous commit (uncrustify) Signed-off-by: Davide Faconti <[email protected]> * revert change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Fix compilation with clang (ros-navigation#4131) Signed-off-by: Ramon Wijnands <[email protected]> code improvement based on feedback. Name changes and minor code improvement Signed-off-by: stevedan <[email protected]> Fix goal handle was not freed correctly (ros-navigation#4137) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Ziohang <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Adding Nvidia as Gold Sponsor! (ros-navigation#4144) Prevent a possible segmentation fault (ros-navigation#4141) (ros-navigation#4147) * Prevent a possible segmentation fault Signed-off-by: Joni Pöllänen <[email protected]> * Cleanup Signed-off-by: Joni Pöllänen <[email protected]> --------- Signed-off-by: Joni Pöllänen <[email protected]> Updating stereolabs logo (ros-navigation#4158) Update README.md Signed-off-by: Steve Macenski <[email protected]> behavior_tree: migration to BT.CPP 4.5.x (ros-navigation#4128) * behavior_tree: migration to BT.CPP 4.5.x Signed-off-by: Davide Faconti <[email protected]> * fix formatting Signed-off-by: Davide Faconti <[email protected]> * fix lint warning Signed-off-by: Davide Faconti <[email protected]> * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti <[email protected]> * handle SKIPPED state Signed-off-by: Davide Faconti <[email protected]> * fix SequenceStar Signed-off-by: Davide Faconti <[email protected]> * add tests Signed-off-by: Davide Faconti <[email protected]> * fix Signed-off-by: Davide Faconti <[email protected]> * fix unit tests Signed-off-by: Davide Faconti <[email protected]> * add emitWakeUpSignal() Signed-off-by: Davide Faconti <[email protected]> * fit test Signed-off-by: Davide Faconti <[email protected]> * fix Signed-off-by: Davide Faconti <[email protected]> * remove SequenceStar Signed-off-by: Davide Faconti <[email protected]> * remove SequenceStar Signed-off-by: Davide Faconti <[email protected]> * keep resetStatus() in action_node Signed-off-by: Davide Faconti <[email protected]> * add suggestions from peer reviewer Signed-off-by: Davide Faconti <[email protected]> * fix compilation Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> bug fix (ros-navigation#4160) Signed-off-by: Tianchu <[email protected]> Stop planner if the goal is cancelled (ros-navigation#4148) * Add support for stopping planners when the action is cancelled Signed-off-by: Kemal Bektas <[email protected]> * Support cancel in Smac planner Signed-off-by: Kemal Bektas <[email protected]> * Support cancel in Theta* planner Signed-off-by: Kemal Bektas <[email protected]> * Support cancel in Navfn planner Signed-off-by: Kemal Bektas <[email protected]> * Update nav2_system_tests to use cancel checker Signed-off-by: Kemal Bektas <[email protected]> * Add terminal_checking_interval parameter Signed-off-by: Kemal Bektas <[email protected]> * Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas <[email protected]> * Add system tests for cancel Signed-off-by: Kemal Bektas <[email protected]> --------- Signed-off-by: Kemal Bektas <[email protected]> Co-authored-by: Kemal Bektas <[email protected]> Improvement on the wat the distance heuristic is calculated Signed-off-by: stevedan <[email protected]> Fix BT.CPP import (ros-navigation#4165) Signed-off-by: Tony Najjar <[email protected]> Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <[email protected]> nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <[email protected]> * added current controller loop rate logging Signed-off-by: ARK3r <[email protected]> * linting Signed-off-by: ARK3r <[email protected]> * uncrustify lint Signed-off-by: ARK3r <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: ARK3r <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> improvement based on feedback Signed-off-by: stevedan <[email protected]>
stevedanomodolor · Mar 25, 2024 · e1b1f24 · e1b1f24
1 parent 70dc1f9
commit e1b1f24
Show file tree

Hide file tree

Showing 242 changed files with 2,157 additions and 1,375 deletions.
diff --git a/README.md b/README.md
@@ -27,11 +27,13 @@ If you need professional services related to Nav2, please contact Open Navigatio
 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_may_2023.png" />
+  <img src="doc/sponsors_feb_2024.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.
 
+### [Nvidia](https://www.nvidia.com/en-us/deep-learning-ai/industries/robotics/) develops GPU and AI technologies that power modern robotics, autonomous driving, data centers, gaming, and more.
+
 ### [Polymath Robotics](https://www.polymathrobotics.com/) creates safety-critical navigation systems for industrial vehicles that are radically simple to enable and deploy.
 
 ### [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.

diff --git a/doc/sponsors_feb_2024.png b/doc/sponsors_feb_2024.png
diff --git a/doc/sponsors_jan_2024.png b/doc/sponsors_jan_2024.png
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
@@ -11,7 +11,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -34,7 +34,7 @@ set(dependencies
   sensor_msgs
   nav2_msgs
   nav_msgs
-  behaviortree_cpp_v3
+  behaviortree_cpp
   tf2
   tf2_ros
   tf2_geometry_msgs
@@ -222,6 +222,17 @@ install(TARGETS ${library_name}
   RUNTIME DESTINATION bin
 )
 
+# we will embed the list of plugin names inside a header file
+set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/gen)
+configure_file(plugins_list.hpp.in ${GENERATED_DIR}/plugins_list.hpp)
+
+add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp)
+ament_target_dependencies(generate_nav2_tree_nodes_xml ${dependencies})
+# allow generate_nav2_tree_nodes_xml to find plugins_list.hpp
+target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR})
+install(TARGETS generate_nav2_tree_nodes_xml DESTINATION lib/${PROJECT_NAME})
+
+
 install(DIRECTORY include/
   DESTINATION include/
 )
@@ -231,6 +242,7 @@ install(DIRECTORY test/utils/
 )
 
 install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
+install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md
@@ -41,7 +41,7 @@ BehaviorTreeEngine::BehaviorTreeEngine()
 Once a new node is registered with the factory, it is now available to the BehaviorTreeEngine and can be used in Behavior Trees. For example, the following simple XML description of a BT shows the FollowPath node in use:
 
 ```XML
-<root main_tree_to_execute="MainTree">
+<root BTCPP_format="4">
   <BehaviorTree ID="MainTree">
     <Sequence name="root">
       <ComputePathToPose goal="${goal}"/>

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
@@ -20,10 +20,9 @@
 #include <string>
 #include <vector>
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "behaviortree_cpp_v3/xml_parsing.h"
-#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/xml_parsing.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -19,7 +19,7 @@
 #include <string>
 #include <chrono>
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
@@ -188,7 +188,7 @@ class BtActionNode : public BT::ActionNodeBase
   BT::NodeStatus tick() override
   {
     // first step to be done only at the beginning of the Action
-    if (status() == BT::NodeStatus::IDLE) {
+    if (!BT::isStatusActive(status())) {
       // setting the status to RUNNING to notify the BT Loggers (if any)
       setStatus(BT::NodeStatus::RUNNING);
 
@@ -325,7 +325,9 @@ class BtActionNode : public BT::ActionNodeBase
       on_cancelled();
     }
 
-    setStatus(BT::NodeStatus::IDLE);
+    // this is probably redundant, since the parent node
+    // is supposed to call it, but we keep it, just in case
+    resetStatus();
   }
 
 protected:
@@ -376,12 +378,14 @@ class BtActionNode : public BT::ActionNodeBase
         if (this->goal_handle_->get_goal_id() == result.goal_id) {
           goal_result_available_ = true;
           result_ = result;
+          emitWakeUpSignal();
         }
       };
     send_goal_options.feedback_callback =
       [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
         const std::shared_ptr<const typename ActionT::Feedback> feedback) {
         feedback_ = feedback;
+        emitWakeUpSignal();
       };
 
     future_goal_handle_ = std::make_shared<
@@ -434,9 +438,9 @@ class BtActionNode : public BT::ActionNodeBase
   void increment_recovery_count()
   {
     int recovery_count = 0;
-    config().blackboard->template get<int>("number_recoveries", recovery_count);  // NOLINT
+    [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count);  // NOLINT
     recovery_count += 1;
-    config().blackboard->template set<int>("number_recoveries", recovery_count);  // NOLINT
+    config().blackboard->set("number_recoveries", recovery_count);  // NOLINT
   }
 
   std::string action_name_;

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
@@ -237,7 +237,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   // Create the Behavior Tree from the XML input
   try {
     tree_ = bt_->createTreeFromFile(filename, blackboard_);
-    for (auto & blackboard : tree_.blackboard_stack) {
+    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_);

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
@@ -19,7 +19,7 @@
 #include <string>
 #include <chrono>
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -19,7 +19,7 @@
 #include <memory>
 #include <chrono>
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
@@ -157,7 +157,7 @@ class BtServiceNode : public BT::ActionNodeBase
   void halt() override
   {
     request_sent_ = false;
-    setStatus(BT::NodeStatus::IDLE);
+    resetStatus();
   }
 
   /**
@@ -230,9 +230,9 @@ class BtServiceNode : public BT::ActionNodeBase
   void increment_recovery_count()
   {
     int recovery_count = 0;
-    config().blackboard->template get<int>("number_recoveries", recovery_count);  // NOLINT
+    [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count);  // NOLINT
     recovery_count += 1;
-    config().blackboard->template set<int>("number_recoveries", recovery_count);  // NOLINT
+    config().blackboard->set("number_recoveries", recovery_count);  // NOLINT
   }
 
   std::string service_name_, service_node_name_;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp
@@ -20,7 +20,7 @@
 
 #include "rclcpp/time.hpp"
 #include "rclcpp/node.hpp"
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -132,20 +132,27 @@ inline std::set<int> convertFromString(StringView key)
 }
 
 /**
- * @brief Return parameter value from behavior tree node or ros2 parameter file
+ * @brief Return parameter value from behavior tree node or ros2 parameter file.
  * @param node rclcpp::Node::SharedPtr
  * @param param_name std::string
  * @param behavior_tree_node T2
  * @return <T1>
  */
-template<typename T1, typename T2>
+template<typename T1, typename T2 = BT::TreeNode>
 T1 deconflictPortAndParamFrame(
   rclcpp::Node::SharedPtr node,
   std::string param_name,
-  T2 * behavior_tree_node)
+  const T2 * behavior_tree_node)
 {
   T1 param_value;
-  if (!behavior_tree_node->getInput(param_name, param_value)) {
+  bool param_from_input = behavior_tree_node->getInput(param_name, param_value).has_value();
+
+  if constexpr (std::is_same_v<T1, std::string>) {
+    // not valid if port doesn't exist or it is an empty string
+    param_from_input &= !param_value.empty();
+  }
+
+  if (!param_from_input) {
     RCLCPP_DEBUG(
       node->get_logger(),
       "Parameter '%s' not provided by behavior tree xml file, "
@@ -162,6 +169,37 @@ T1 deconflictPortAndParamFrame(
   }
 }
 
+/**
+ * @brief Try reading an import port first, and if that doesn't work
+ * fallback to reading directly the blackboard.
+ * The blackboard must be passed explitly because config() is private in BT.CPP 4.X
+ *
+ * @param bt_node node
+ * @param blackboard the blackboard ovtained with node->config().blackboard
+ * @param param_name std::string
+ * @param behavior_tree_node the node
+ * @return <T>
+ */
+template<typename T> inline
+bool getInputPortOrBlackboard(
+  const BT::TreeNode & bt_node,
+  const BT::Blackboard & blackboard,
+  const std::string & param_name,
+  T & value)
+{
+  if (bt_node.getInput<T>(param_name, value)) {
+    return true;
+  }
+  if (blackboard.get<T>(param_name, value)) {
+    return true;
+  }
+  return false;
+}
+
+// Macro to remove boiler plate when using getInputPortOrBlackboard
+#define getInputOrBlackboard(name, value) \
+  getInputPortOrBlackboard(*this, *(this->config().blackboard), name, value);
+
 }  // namespace BT
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_UTILS_HPP_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/...ehavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/...ehavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
@@ -20,7 +20,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
@@ -22,7 +22,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp
@@ -21,7 +21,7 @@
 
 #include "nav_msgs/msg/path.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
@@ -22,7 +22,7 @@
 
 #include "nav_msgs/msg/path.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "tf2_ros/buffer.h"
 
 namespace nav2_behavior_tree

diff --git a/...r_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp b/...r_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp
@@ -21,7 +21,7 @@
 #include <set>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
@@ -19,7 +19,7 @@
 #include <string>
 #include <memory>
 
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"

diff --git a/...ior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/...ior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp
@@ -20,7 +20,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 
@@ -57,7 +57,12 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
-    return {};
+    return {
+      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+        "goals", "Vector of navigation goals"),
+      BT::InputPort<geometry_msgs::msg::PoseStamped>(
+        "goal", "Navigation goal"),
+    };
   }
 
 private:

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
@@ -19,7 +19,7 @@
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "tf2_ros/buffer.h"
 
 namespace nav2_behavior_tree