diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index 0569c58bc94..b1f7da25373 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(angles REQUIRED)
 
 nav2_package()
 
@@ -41,6 +42,7 @@ set(dependencies
   std_msgs
   std_srvs
   nav2_util
+  angles
 )
 
 add_library(${library_name} SHARED
@@ -199,6 +201,8 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
+install(FILES test/test_action_server.hpp test/test_service.hpp DESTINATION include/nav2_behavior_tree)
+
 install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
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
index 06fc6dd8103..141a3157bce 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -93,9 +93,9 @@ class BtActionNode : public BT::ActionNodeBase
 
     // Make sure the server is actually there before continuing
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
-    if (!action_client_->wait_for_action_server(1s)) {
+    if (!action_client_->wait_for_action_server(10s)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
+        node_->get_logger(), "\"%s\" action server not available after waiting for 10 s",
         action_name.c_str());
       throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
     }
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
index 75580803476..d91cfec2b80 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -169,7 +169,7 @@ class BtActionServer
    * @brief Getter function for the current BT tree
    * @return BT::Tree Current behavior tree
    */
-  const BT::Tree & getTree() const
+  const BT::Tree* getTree() const
   {
     return tree_;
   }
@@ -180,9 +180,11 @@ class BtActionServer
    */
   void haltTree()
   {
-    tree_.rootNode()->halt();
+    tree_->rootNode()->halt();
   }
 
+  std::map<std::size_t, BT::Tree> cached_trees;
+  std::vector<std::size_t> cached_tree_hashes;
 protected:
   /**
    * @brief Action server callback
@@ -196,7 +198,7 @@ class BtActionServer
   std::shared_ptr<ActionServer> action_server_;
 
   // Behavior Tree to be executed when goal is received
-  BT::Tree tree_;
+  BT::Tree* tree_;
 
   // The blackboard shared by all of the nodes in the tree
   BT::Blackboard::Ptr blackboard_;
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
index 592d1257a27..50494fd856c 100644
--- 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
@@ -21,6 +21,7 @@
 #include <set>
 #include <exception>
 #include <vector>
+#include <algorithm>
 
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_behavior_tree/bt_action_server.hpp"
@@ -142,7 +143,7 @@ bool BtActionServer<ActionT>::on_cleanup()
   plugin_lib_names_.clear();
   current_bt_xml_filename_.clear();
   blackboard_.reset();
-  bt_->haltAllActions(tree_.rootNode());
+  bt_->haltAllActions(tree_->rootNode());
   bt_.reset();
   return true;
 }
@@ -153,11 +154,17 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   // Empty filename is default for backward compatibility
   auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
 
+  // This is removed as part of the changes about BT hashing as we still want to check for
+  // changes in the xml file even if current_bt_xml_filename_ == filename
+
+  /*
   // Use previous BT if it is the existing one
   if (current_bt_xml_filename_ == filename) {
     RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded");
     return true;
   }
+  */
+ 
 
   // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(filename);
@@ -171,17 +178,30 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     std::istreambuf_iterator<char>(xml_file),
     std::istreambuf_iterator<char>());
 
-  // Create the Behavior Tree from the XML input
-  try {
-    tree_ = bt_->createTreeFromText(xml_string, blackboard_);
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
-    return false;
+  std::hash<std::string> hasher;
+
+  std::size_t tree_hash = hasher(xml_string);
+
+  // if a tree was used before we fetch it from the cached trees not to create it one more time
+  if (std::find(cached_tree_hashes.begin(), cached_tree_hashes.end(), tree_hash) != cached_tree_hashes.end())
+  {
+    RCLCPP_DEBUG(logger_, "BT will not be loaded as it exists in cache");
+    tree_ = &cached_trees[tree_hash];
+  }
+  else
+  {
+    RCLCPP_DEBUG(logger_, "BT will be loaded as it doesn't exist in cache");
+
+    // Create the Behavior Tree from the XML input
+    cached_trees[tree_hash] = bt_->createTreeFromText(xml_string, blackboard_);
+    cached_tree_hashes.push_back(tree_hash);
+    tree_ = &cached_trees[tree_hash];
   }
 
-  topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
+  topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, *tree_);
 
   current_bt_xml_filename_ = filename;
+
   return true;
 }
 
@@ -192,7 +212,7 @@ void BtActionServer<ActionT>::executeCallback()
     action_server_->terminate_current();
     return;
   }
-
+  RCLCPP_INFO(logger_, "Action server name is %s", action_name_.c_str());
   auto is_canceling = [&]() {
       if (action_server_ == nullptr) {
         RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling.");
@@ -214,11 +234,11 @@ void BtActionServer<ActionT>::executeCallback()
     };
 
   // Execute the BT that was previously created in the configure step
-  nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling, bt_loop_duration_);
+  nav2_behavior_tree::BtStatus rc = bt_->run(tree_, on_loop, is_canceling, bt_loop_duration_);
 
   // Make sure that the Bt is not in a running state from a previous execution
   // note: if all the ControlNodes are implemented correctly, this is not needed.
-  bt_->haltAllActions(tree_.rootNode());
+  bt_->haltAllActions(tree_->rootNode());
 
   // Give server an opportunity to populate the result message or simple give
   // an indication that the action is complete.
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
index c1970e38b18..126d1c4cd9c 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -71,7 +71,7 @@ class BtServiceNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(
       node_->get_logger(), "Waiting for \"%s\" service",
       service_name_.c_str());
-    service_client_->wait_for_service();
+    service_client_->wait_for_service(std::chrono::seconds(10));
 
     RCLCPP_DEBUG(
       node_->get_logger(), "\"%s\" BtServiceNode initialized",
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
index 4481198972f..2747ac23b12 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
@@ -55,7 +55,9 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
       {
         BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
+        BT::InputPort<bool>("free_goal_vel", false, "Don't stop when goal reached"),
+        BT::InputPort<bool>("check_local_costmap", true, "Check local costmap for collisions")
       });
   }
 };
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
index a97993c7738..8093798639f 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
@@ -64,6 +64,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
       {
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
         BT::InputPort<std::string>("controller_id", ""),
+        BT::InputPort<std::string>("free_goal_vel", "Don't stop when goal reached"),
         BT::InputPort<std::string>("goal_checker_id", ""),
       });
   }
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
index a4d41437d84..c06529d0313 100644
--- 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
@@ -74,6 +74,8 @@ class GoalReachedCondition : public BT::ConditionNode
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
       BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
+      BT::InputPort<double>("xy_goal_tolerance", 0.1, "xy goal tolerance"),
+      BT::InputPort<double>("yaw_goal_tolerance", 0.1, "yaw goal tolerance"),
       BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
     };
   }
@@ -91,6 +93,7 @@ class GoalReachedCondition : public BT::ConditionNode
 
   bool initialized_;
   double goal_reached_tol_;
+  double goal_reached_tol_yaw_;
   std::string global_frame_;
   std::string robot_base_frame_;
   double transform_tolerance_;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp
index d71a57bc046..549a96efc6e 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp
@@ -46,7 +46,7 @@ class RosTopicLogger : public BT::StatusChangeLogger
     clock_ = node->get_clock();
     logger_ = node->get_logger();
     log_pub_ = node->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
-      "behavior_tree_log",
+      "~/behavior_tree_log",
       rclcpp::QoS(10));
   }
 
@@ -69,16 +69,18 @@ class RosTopicLogger : public BT::StatusChangeLogger
     // before converting to a msg.
     event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp));
     event.node_name = node.name();
+    event.node_uid = node.UID();
     event.previous_status = toStr(prev_status, false);
     event.current_status = toStr(status, false);
     event_log_.push_back(std::move(event));
 
-    RCLCPP_DEBUG(
-      logger_, "[%.3f]: %25s %s -> %s",
-      std::chrono::duration<double>(timestamp).count(),
-      node.name().c_str(),
-      toStr(prev_status, true).c_str(),
-      toStr(status, true).c_str() );
+    // AMRFM-2554 Remove BT messages about node state transitioning
+    // RCLCPP_DEBUG(
+    //   logger_, "[%.3f]: %25s %s -> %s",
+    //   std::chrono::duration<double>(timestamp).count(),
+    //   node.name().c_str(),
+    //   toStr(prev_status, true).c_str(),
+    //   toStr(status, true).c_str() );
   }
 
   /**
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index 3f5fa4b522f..57731beec59 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -13,6 +13,8 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="free_goal_vel" default="false">Don't stop when goal reached</input_port>
+      <input_port name="check_local_costmap" default="true">Check if there is an obstacle on path</input_port>
     </Action>
 
     <Action ID="DriveOnHeading">
@@ -169,6 +171,8 @@
         <input_port name="goal">Destination</input_port>
         <input_port name="global_frame">Reference frame</input_port>
         <input_port name="robot_base_frame">Robot base frame</input_port>
+        <input_port name="xy_goal_tolerance">xy goal tolerance</input_port>
+        <input_port name="yaw_goal_tolerance">yaw goal tolerance</input_port>
     </Condition>
 
     <Condition ID="IsStuck"/>
diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
index ce98becbc12..e9f79dd78e0 100644
--- a/nav2_behavior_tree/package.xml
+++ b/nav2_behavior_tree/package.xml
@@ -31,6 +31,7 @@
   <build_depend>nav2_util</build_depend>
   <build_depend>lifecycle_msgs</build_depend>
   <build_depend>nav2_common</build_depend>
+  <build_depend>angles</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_action</exec_depend>
diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp
index b72afaf8106..8b30ead7869 100644
--- a/nav2_behavior_tree/plugins/action/back_up_action.cpp
+++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp
@@ -32,6 +32,10 @@ BackUpAction::BackUpAction(
   getInput("backup_speed", speed);
   double time_allowance;
   getInput("time_allowance", time_allowance);
+  bool free_goal_vel;
+  getInput("free_goal_vel", free_goal_vel);
+  bool check_local_costmap;
+  getInput("check_local_costmap", check_local_costmap);
 
   // Populate the input message
   goal_.target.x = dist;
@@ -39,6 +43,8 @@ BackUpAction::BackUpAction(
   goal_.target.z = 0.0;
   goal_.speed = speed;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+  goal_.free_goal_vel = free_goal_vel;
+  goal_.check_local_costmap = check_local_costmap;
 }
 
 void BackUpAction::on_tick()
diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
index 0fc0dc5b574..1a4e575d71a 100644
--- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp
+++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
@@ -32,6 +32,7 @@ void FollowPathAction::on_tick()
 {
   getInput("path", goal_.path);
   getInput("controller_id", goal_.controller_id);
+  getInput("free_goal_vel", goal_.free_goal_vel);
   getInput("goal_checker_id", goal_.goal_checker_id);
 }
 
diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
index 3a5b8d821c9..b4fd44c64c3 100644
--- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
@@ -18,6 +18,8 @@
 #include "nav2_util/robot_utils.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_util/node_utils.hpp"
+#include "tf2/utils.h"
+#include "angles/angles.h"
 
 #include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
 
@@ -34,6 +36,10 @@ GoalReachedCondition::GoalReachedCondition(
 {
   getInput("global_frame", global_frame_);
   getInput("robot_base_frame", robot_base_frame_);
+  getInput("xy_goal_tolerance", goal_reached_tol_);
+  getInput("yaw_goal_tolerance", goal_reached_tol_yaw_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 }
 
 GoalReachedCondition::~GoalReachedCondition()
@@ -57,10 +63,10 @@ void GoalReachedCondition::initialize()
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
-  nav2_util::declare_parameter_if_not_declared(
-    node_, "goal_reached_tol",
-    rclcpp::ParameterValue(0.25));
-  node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
+  //nav2_util::declare_parameter_if_not_declared(
+  //  node_, "goal_reached_tol",
+  //  rclcpp::ParameterValue(0.25));
+  //node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 
   node_->get_parameter("transform_tolerance", transform_tolerance_);
@@ -81,10 +87,14 @@ bool GoalReachedCondition::isGoalReached()
 
   geometry_msgs::msg::PoseStamped goal;
   getInput("goal", goal);
+
+  double current_yaw = tf2::getYaw(current_pose.pose.orientation);
+  double goal_yaw = tf2::getYaw(goal.pose.orientation);
+  double dangle = fabs(angles::shortest_angular_distance(goal_yaw, current_yaw));
   double dx = goal.pose.position.x - current_pose.pose.position.x;
   double dy = goal.pose.position.y - current_pose.pose.position.y;
 
-  return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_);
+  return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) && dangle <= goal_reached_tol_yaw_;
 }
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp
index 90a69a686ed..ce1b8e20cf3 100644
--- a/nav2_behaviors/plugins/back_up.cpp
+++ b/nav2_behaviors/plugins/back_up.cpp
@@ -26,9 +26,10 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
     return Status::FAILED;
   }
 
-  // Silently ensure that both the speed and direction are negative.
-  command_x_ = -std::fabs(command->target.x);
-  command_speed_ = -std::fabs(command->speed);
+  command_x_ = command->target.x;
+  command_speed_ = std::fabs(command->speed);
+  free_goal_vel = command->free_goal_vel;
+  check_local_costmap = command->check_local_costmap;
   command_time_allowance_ = command->time_allowance;
 
   end_time_ = steady_clock_.now() + command_time_allowance_;
diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp
index 79c8d81c719..3edfdb86403 100644
--- a/nav2_behaviors/plugins/drive_on_heading.hpp
+++ b/nav2_behaviors/plugins/drive_on_heading.hpp
@@ -118,22 +118,25 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     feedback_->distance_traveled = distance;
     this->action_server_->publish_feedback(feedback_);
 
-    if (distance >= std::fabs(command_x_)) {
-      this->stopRobot();
+    if (distance >= abs(command_x_)) {
+      if (!free_goal_vel)
+      {
+        this->stopRobot();
+      }
       return Status::SUCCEEDED;
     }
 
     auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
     cmd_vel->linear.y = 0.0;
     cmd_vel->angular.z = 0.0;
-    cmd_vel->linear.x = command_speed_;
+    cmd_vel->linear.x = command_x_ < 0 ? -command_speed_ : command_speed_;
 
     geometry_msgs::msg::Pose2D pose2d;
     pose2d.x = current_pose.pose.position.x;
     pose2d.y = current_pose.pose.position.y;
     pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
 
-    if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) {
+    if (check_local_costmap && !isCollisionFree(distance, cmd_vel.get(), pose2d)) {
       this->stopRobot();
       RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading");
       return Status::FAILED;
@@ -207,6 +210,8 @@ class DriveOnHeading : public TimedBehavior<ActionT>
   rclcpp::Duration command_time_allowance_{0, 0};
   rclcpp::Time end_time_;
   double simulate_ahead_time_;
+  bool free_goal_vel;
+  bool check_local_costmap;
 };
 
 }  // namespace nav2_behaviors
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index 3f4e71cc1cf..a6ff1732842 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -351,6 +351,7 @@ void ControllerServer::computeControl()
     std::string c_name = action_server_->get_current_goal()->controller_id;
     std::string current_controller;
     if (findControllerId(c_name, current_controller)) {
+      RCLCPP_INFO(get_logger(), "Selected controller: %s.", c_name.c_str());
       current_controller_ = current_controller;
     } else {
       action_server_->terminate_current();
@@ -371,6 +372,8 @@ void ControllerServer::computeControl()
 
     last_valid_cmd_time_ = now();
     rclcpp::WallRate loop_rate(controller_frequency_);
+    auto begin = std::chrono::steady_clock::now();
+    double real_frequency = controller_frequency_;
     while (rclcpp::ok()) {
       if (action_server_ == nullptr || !action_server_->is_server_active()) {
         RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
@@ -378,9 +381,15 @@ void ControllerServer::computeControl()
       }
 
       if (action_server_->is_cancel_requested()) {
-        RCLCPP_INFO(get_logger(), "Goal was canceled. Stopping the robot.");
+        bool free_goal_vel = action_server_->get_current_goal()->free_goal_vel;
         action_server_->terminate_all();
-        publishZeroVelocity();
+        if (!free_goal_vel){
+          RCLCPP_INFO(get_logger(), "Goal was canceled. Stopping the robot.");
+          publishZeroVelocity();
+        }
+        else {
+          RCLCPP_INFO(get_logger(), "Goal was canceled.");
+        }
         return;
       }
 
@@ -400,9 +409,13 @@ void ControllerServer::computeControl()
       }
 
       if (!loop_rate.sleep()) {
+        auto end = std::chrono::steady_clock::now();
+        double period = std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count();
+        begin = end;
+        real_frequency = 1.0e6 / period;
         RCLCPP_WARN(
-          get_logger(), "Control loop missed its desired rate of %.4fHz",
-          controller_frequency_);
+          get_logger(), "Control loop missed its desired rate of %.4fHz. Achieved rate: %.4fHz",
+          controller_frequency_, real_frequency);
       }
     }
   } catch (nav2_core::PlannerException & e) {
@@ -413,8 +426,10 @@ void ControllerServer::computeControl()
   }
 
   RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result");
-
-  publishZeroVelocity();
+  bool free_goal_vel = action_server_->get_current_goal()->free_goal_vel;
+  if (!free_goal_vel){
+    publishZeroVelocity();
+  }
 
   // TODO(orduno) #861 Handle a pending preemption and set controller name
   action_server_->succeeded_current();
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index 073109c2352..f285c13984f 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -195,7 +195,7 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
     !layered_costmap_->isSizeLocked()))
   {
     // Update the size of the layered costmap (and all layers, including this one)
-    RCLCPP_INFO(
+    RCLCPP_DEBUG(
       logger_,
       "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
       new_map.info.resolution);
diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action
index 9937302578a..85346321746 100644
--- a/nav2_msgs/action/BackUp.action
+++ b/nav2_msgs/action/BackUp.action
@@ -2,6 +2,8 @@
 geometry_msgs/Point target
 float32 speed
 builtin_interfaces/Duration time_allowance
+bool free_goal_vel
+bool check_local_costmap
 ---
 #result definition
 builtin_interfaces/Duration total_elapsed_time
diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action
index 5462faa2398..5f197d16c36 100644
--- a/nav2_msgs/action/FollowPath.action
+++ b/nav2_msgs/action/FollowPath.action
@@ -1,6 +1,7 @@
 #goal definition
 nav_msgs/Path path
 string controller_id
+bool free_goal_vel
 string goal_checker_id
 ---
 #result definition
diff --git a/nav2_msgs/msg/BehaviorTreeStatusChange.msg b/nav2_msgs/msg/BehaviorTreeStatusChange.msg
index 01cd812c6f7..96159cba474 100644
--- a/nav2_msgs/msg/BehaviorTreeStatusChange.msg
+++ b/nav2_msgs/msg/BehaviorTreeStatusChange.msg
@@ -1,4 +1,5 @@
 builtin_interfaces/Time timestamp  # internal behavior tree event timestamp. Typically this is wall clock time
 string node_name
+uint32 node_uid
 string previous_status              # IDLE, RUNNING, SUCCESS or FAILURE
 string current_status               # IDLE, RUNNING, SUCCESS or FAILURE