diff --git a/.circleci/config.yml b/.circleci/config.yml
index 5c6c2a06aad8570f5ff5771c65e72e0ed534142e..7fd1f17b61110d6f7779d36fa9eef3ddb99561e1 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -10,6 +10,8 @@ references:
     CCACHE_LOGFILE: "/tmp/ccache.log"
     CCACHE_MAXSIZE: "200M"
     MAKEFLAGS: "-j 1 -l 2"
+    RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED: "1"
+    RETEST_UNTIL_PASS: "2"
   common_commands: &common_commands
     restore_from_cache:
       description: "Restore From Cache"
@@ -152,6 +154,12 @@ references:
               colcon test \
                 --packages-select ${TEST_PACKAGES} \
                 --mixin << parameters.mixins >>
+              colcon test \
+                --packages-select ${TEST_PACKAGES} \
+                --packages-select-test-failures \
+                --mixin << parameters.mixins >> \
+                --retest-until-pass ${RETEST_UNTIL_PASS} \
+                --ctest-args --test-regex "test_.*"
               colcon test-result \
                 --verbose
         - run:
@@ -285,7 +293,10 @@ references:
       command: |
         cd src/navigation2/sphinx_doc
         make html
-
+  store_docs: &store_docs
+    store_artifacts:
+        path: /opt/overlay_ws/src/navigation2/sphinx_doc/_build/html
+        destination: html
 commands:
   <<: *common_commands
   checkout_source:
@@ -335,6 +346,7 @@ commands:
     description: "Build docs"
     steps:
       - *make_docs
+      - *store_docs
 
 executors:
   debug_exec:
diff --git a/Dockerfile b/Dockerfile
index 608fbe83ab436e443549fe0ac6b5b4ffafa6e70d..9ac07102cac6ef17ecc02c3a77e103d109df6384 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -34,8 +34,8 @@ RUN find ./ -name "package.xml" | \
 # multi-stage for building
 FROM $FROM_IMAGE AS build
 
-# install CI dependencies	
-RUN apt-get update && apt-get install -q -y \	
+# install CI dependencies
+RUN apt-get update && apt-get install -q -y \
       ccache \
       lcov \
     && rm -rf /var/lib/apt/lists/*
@@ -63,6 +63,7 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
       --symlink-install \
       --mixin \
         $UNDERLAY_MIXINS \
+      --event-handlers console_direct+ \
     || touch build_failed && \
     if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \
       exit 1; \
diff --git a/README.md b/README.md
index 3ab6d5d76896dea83eae1b5aace886cab10a8597..7bf6244c234e4e82ff534d67ac6d0dda2b7b56a9 100644
--- a/README.md
+++ b/README.md
@@ -17,14 +17,17 @@ ROS2 Navigation System
 # Overview
 The ROS 2 Navigation System is the control system that enables a robot to autonomously reach a goal state, such as a specific position and orientation relative to a specific map. Given a current pose, a map, and a goal, such as a destination pose, the navigation system generates a plan to reach the goal, and outputs commands to autonomously drive the robot, respecting any safety constraints and avoiding obstacles encountered along the way.
 
+# Documentation
+For detailed instructions on how to install and run the examples, please visit our [documentation site](https://ros-planning.github.io/navigation2/).
+
 # Contributing
-We are currently in the pre-release development phase, contributions are welcome. To contribute, see the [documentation README](doc/README.md).
+[Contributions are welcome!](doc/README.md#contributing). For more information, please review our [contribution guidelines](https://ros-planning.github.io/navigation2/contribute/contribute_guidelines.html).
 
 # Building the source
 For instructions on how to download and build this repo, see the [BUILD.md](doc/BUILD.md) file.
 
 # Creating a docker image
-To build an image from the Dockerfile in the navigation2 folder: 
+To build an image from the Dockerfile in the navigation2 folder:
 First, clone the repo to your local system (or see Building the source above)
 ```
 sudo docker build -t nav2/latest .
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
index abc421cbc197cc838d380f2e5e5c1df71f57e592..8eb8518cabf0d4fdfd50c0037e7965acdfba8d1a 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
@@ -52,10 +52,10 @@ public:
 
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::InputPort<double>("backup_dist", -0.15, "Distance to backup"),
-      BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup")
-    };
+    return providedBasicPorts({
+        BT::InputPort<double>("backup_dist", -0.15, "Distance to backup"),
+        BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup")
+      });
   }
 };
 
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 9208cd2284fa7092fc4b5a06d68fdb1dc3ffc3a3..e069794f104af7780c20630f48822bcf10bcc424 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
@@ -41,8 +41,9 @@ public:
     result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
 
     // Get the required items from the blackboard
-    node_loop_timeout_ =
-      config().blackboard->get<std::chrono::milliseconds>("node_loop_timeout");
+    server_timeout_ =
+      config().blackboard->get<std::chrono::milliseconds>("server_timeout");
+    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
 
     // Now that we have the ROS node to use, create the action client for this BT action
     action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name_);
@@ -61,8 +62,25 @@ public:
   {
   }
 
+  // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method
+  // and call providedBasicPorts in it.
+  static BT::PortsList providedBasicPorts(BT::PortsList addition)
+  {
+    BT::PortsList basic = {
+      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+    };
+    basic.insert(addition.begin(), addition.end());
+
+    return basic;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({});
+  }
+
   // Derived classes can override any of the following methods to hook into the
-  // processing for the action: on_tick, on_loop_timeout, and on_success
+  // processing for the action: on_tick, on_server_timeout, and on_success
 
   // Could do dynamic checks, such as getting updates to values on the blackboard
   virtual void on_tick()
@@ -71,7 +89,7 @@ public:
 
   // There can be many loop iterations per tick. Any opportunity to do something after
   // a timeout waiting for a result that hasn't been received yet
-  virtual void on_loop_timeout()
+  virtual void on_server_timeout()
   {
   }
 
@@ -106,9 +124,9 @@ new_goal_received:
     auto future_result = goal_handle_->async_result();
     rclcpp::executor::FutureReturnCode rc;
     do {
-      rc = rclcpp::spin_until_future_complete(node_, future_result, node_loop_timeout_);
+      rc = rclcpp::spin_until_future_complete(node_, future_result, server_timeout_);
       if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
-        on_loop_timeout();
+        on_server_timeout();
 
         // We can handle a new goal if we're still executing
         auto status = goal_handle_->get_status();
@@ -197,7 +215,7 @@ protected:
 
   // The timeout value while to use in the tick loop while waiting for
   // a result from the server
-  std::chrono::milliseconds node_loop_timeout_;
+  std::chrono::milliseconds server_timeout_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
index ead7ed686a3d74306265f60b076e079b131efd5d..6b7d783511b15876a0dffa14e9ae735f1748e5e6 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
@@ -59,6 +59,12 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
   }
 }
 
+template<>
+inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(const StringView key)
+{
+  return std::chrono::milliseconds(std::stoul(key.data()));
+}
+
 }  // namespace BT
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_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
index 8f40866bfcd40eef08652a71087140531efb807d..76fbdcdfb11235ac192cc63b8d947a7dc9f4beff 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
@@ -37,8 +37,9 @@ public:
     node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
     // Get the required items from the blackboard
-    node_loop_timeout_ =
-      config().blackboard->get<std::chrono::milliseconds>("node_loop_timeout");
+    server_timeout_ =
+      config().blackboard->get<std::chrono::milliseconds>("server_timeout");
+    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
 
     // Now that we have node_ to use, create the service client for this BT service
     getInput("service_name", service_name_);
@@ -59,12 +60,22 @@ public:
   {
   }
 
-  // Any BT node that accepts parameters must provide a requiredNodeParameters method
-  static BT::PortsList providedPorts()
+  // Any subclass of BtServiceNode that accepts parameters must provide a providedPorts method
+  // and call providedBasicPorts in it.
+  static BT::PortsList providedBasicPorts(BT::PortsList addition)
   {
-    return {
-      BT::InputPort<std::string>("service_name", "please_set_service_name_in_BT_Node")
+    BT::PortsList basic = {
+      BT::InputPort<std::string>("service_name", "please_set_service_name_in_BT_Node"),
+      BT::InputPort<std::chrono::milliseconds>("server_timeout")
     };
+    basic.insert(addition.begin(), addition.end());
+
+    return basic;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({});
   }
 
   // The main override required by a BT service
@@ -75,12 +86,15 @@ public:
 
     rclcpp::executor::FutureReturnCode rc;
     rc = rclcpp::spin_until_future_complete(node_,
-        future_result, node_loop_timeout_);
-    if (rc != rclcpp::executor::FutureReturnCode::SUCCESS) {
-      return BT::NodeStatus::FAILURE;
-    } else {
+        future_result, server_timeout_);
+    if (rc == rclcpp::executor::FutureReturnCode::SUCCESS) {
       return BT::NodeStatus::SUCCESS;
+    } else if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
+      RCLCPP_WARN(node_->get_logger(),
+        "Node timed out while executing service call to %s.", service_name_.c_str());
+      on_server_timeout();
     }
+    return BT::NodeStatus::FAILURE;
   }
 
   // Fill in service request with information if necessary
@@ -89,6 +103,12 @@ public:
     request_ = std::make_shared<typename ServiceT::Request>();
   }
 
+  // An opportunity to do something after
+  // a timeout waiting for a result that hasn't been received yet
+  virtual void on_server_timeout()
+  {
+  }
+
 protected:
   std::string service_name_, service_node_name_;
   typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
@@ -99,7 +119,7 @@ protected:
 
   // The timeout value while to use in the tick loop while waiting for
   // a result from the server
-  std::chrono::milliseconds node_loop_timeout_;
+  std::chrono::milliseconds server_timeout_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/compute_control_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/compute_control_action.hpp
index 524faed60d1d438502da399bc537456b7d6d4947..65532801bec844d8ab568f525f244d60dc33144d 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/compute_control_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/compute_control_action.hpp
@@ -40,7 +40,7 @@ public:
     getInput("path", goal_.path);
   }
 
-  void on_loop_timeout() override
+  void on_server_timeout() override
   {
     // Check if the goal has been updated
     if (config().blackboard->get<bool>("path_updated")) {
@@ -57,10 +57,10 @@ public:
 
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
-      BT::InputPort<std::string>("controller_name", "FollowPath"),
-    };
+    return providedBasicPorts({
+        BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
+        BT::InputPort<std::string>("controller_name", "FollowPath"),
+      });
   }
 };
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
index 91e8bc488274641e1dd8dece446c97dcbeb93cf4..d610563b0f41631878ace76c2c07efeff20ca54b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
@@ -53,10 +53,10 @@ public:
 
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
-      BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to")
-    };
+    return providedBasicPorts({
+        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to")
+      });
   }
 
 private:
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
index e3dca52f37c773cb67548144fe8da15899b8431b..46f40fec5530664ab1d1b306a79167a57ff19c30 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
@@ -58,10 +58,10 @@ public:
   // Any BT node that accepts parameters must provide a requiredNodeParameters method
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::InputPort<geometry_msgs::msg::Point>("position", "0;0;0", "Position"),
-      BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "0;0;0;0", "Orientation")
-    };
+    return providedBasicPorts({
+        BT::InputPort<geometry_msgs::msg::Point>("position", "0;0;0", "Position"),
+        BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "0;0;0;0", "Orientation")
+      });
   }
 };
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
index 38c11dc3619d2bb4dcf70544f4e1ba4b3daa193f..2324a917e434b2fce958116d58d780dc56f301dd 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
@@ -43,9 +43,9 @@ public:
 
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::InputPort<double>("spin_dist", 1.57, "Spin distance")
-    };
+    return providedBasicPorts({
+        BT::InputPort<double>("spin_dist", 1.57, "Spin distance")
+      });
   }
 };
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
index f31a8c28bf1e60935e0f9d0282968f8b95e54d08..1bd4997f72fb62994b3fa49d52cbd44fedff992a 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
@@ -47,9 +47,9 @@ public:
   // Any BT node that accepts parameters must provide a requiredNodeParameters method
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::InputPort<int>("wait_duration", 1, "Wait time")
-    };
+    return providedBasicPorts({
+        BT::InputPort<int>("wait_duration", 1, "Wait time")
+      });
   }
 };
 
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
index 7b40f6e50ca91d5522b3dc10b9503b8f00c0b9be..f016df80afded076f21c13721df747b90964003d 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -30,20 +30,61 @@
 
 namespace nav2_bt_navigator
 {
-
+/**
+ * @class nav2_bt_navigator::BtNavigator
+ * @brief An action server that uses behavior tree for navigating a robot to its
+ * goal position.
+ */
 class BtNavigator : public nav2_util::LifecycleNode
 {
 public:
+  /**
+   * @brief A constructor for nav2_bt_navigator::BtNavigator class
+   */
   BtNavigator();
+  /**
+   * @brief A destructor for nav2_bt_navigator::BtNavigator class
+   */
   ~BtNavigator();
 
 protected:
-  // The lifecycle node interface
+  /**
+   * @brief Configures member variables
+   *
+   * Initializes action server for "NavigationToPose"; subscription to
+   * "goal_sub"; and builds behavior tree from xml file.
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Activates action server
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Deactivates action server
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Resets member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in shutdown state
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in error state
+   * @param state Reference to LifeCycle node state
+   */
   nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
 
   using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::NavigateToPose>;
@@ -51,13 +92,20 @@ protected:
   // Our action server implements the NavigateToPose action
   std::unique_ptr<ActionServer> action_server_;
 
-  // The action server callback
+  /**
+   * @brief Action server callbacks
+   */
   void navigateToPose();
 
-  // Goal pose initialization on the blackboard
+  /**
+   * @brief Goal pose initialization on the blackboard
+   */
   void initializeGoalPose();
 
-  // A subscription and callback to handle the topic-based goal published from rviz
+  /**
+   * @brief A subscription and callback to handle the topic-based goal published
+   * from rviz
+   */
   void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
 
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index c06b2ea2206b67771f50db5960c27177e5a00f1a..9a9a0ad1e5221a93ffd45ff98a7f382475c3599f 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -82,7 +82,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Put items on the blackboard
   blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
   blackboard_->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_);  // NOLINT
-  blackboard_->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10));  // NOLINT
+  blackboard_->set<std::chrono::milliseconds>("server_timeout", std::chrono::milliseconds(10));  // NOLINT
   blackboard_->set<bool>("path_updated", false);  // NOLINT
   blackboard_->set<bool>("initial_pose_received", false);  // NOLINT
 
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
index 82a71a675ddda75df62853be08fbdecca55fec99..86b293cbfc76fd749c00076811c9c46f607962ca 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
@@ -60,6 +60,8 @@ public:
 
   virtual void matchSize();
 
+  virtual void clearArea(int start_x, int start_y, int end_x, int end_y);
+
   /**
    * If an external source changes values in the costmap,
    * it should call this method with the area that it changed
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index f878c8dba3b299e5bee5b77e5723eb2497cf8ce3..bded2edeb308aa5c2368a8fbeb34d3dae63e6918 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -60,6 +60,24 @@ void CostmapLayer::matchSize()
     master->getOriginX(), master->getOriginY());
 }
 
+void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y)
+{
+  unsigned char * grid = getCharMap();
+  for (int x = 0; x < static_cast<int>(getSizeInCellsX()); x++) {
+    bool xrange = x > start_x && x < end_x;
+
+    for (int y = 0; y < static_cast<int>(getSizeInCellsY()); y++) {
+      if (xrange && y > start_y && y < end_y) {
+        continue;
+      }
+      int index = getIndex(x, y);
+      if (grid[index] != NO_INFORMATION) {
+        grid[index] = NO_INFORMATION;
+      }
+    }
+  }
+}
+
 void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
 {
   extra_min_x_ = std::min(mx0, extra_min_x_);
diff --git a/nav2_dwb_controller/README.md b/nav2_dwb_controller/README.md
index e0a5aca5fe84f26cec6ebfc160d435ee9b7a9149..7b116230958a4ea190b0c9beae387634545dee41 100644
--- a/nav2_dwb_controller/README.md
+++ b/nav2_dwb_controller/README.md
@@ -75,7 +75,4 @@ TaskStatus execute(path)
 ```
 ## Future Plans
 
-* Add support for recovery behaviors https://github.com/ros-planning/navigation2/issues/49
-* Simplify parameters and remove obsolete parameters https://github.com/ros-planning/navigation2/issues/201
-* Remove the direct inclusion of `costmap_2d` and replace it with calls to the world model node. https://github.com/ros-planning/navigation2/issues/21
 * Port another planner to refine the interfaces to the planner and to the world model. There are not many local planners available for the navigation stack, it seems. However `Time Elastic Band` seems like an interesting possibility of the few that are out there, as it interprets the dynamic obstacles very differently. https://github.com/ros-planning/navigation2/issues/202
diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
index 873c3f199efc012730a15b9707d9e72efe959286..31e00bf0a5912e1badb0470ad975293eae31b8f9 100644
--- a/nav2_planner/include/nav2_planner/planner_server.hpp
+++ b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -38,20 +38,59 @@
 
 namespace nav2_planner
 {
-
+/**
+ * @class nav2_planner::PlannerServer
+ * @brief An action server implements the behavior tree's ComputePathToPose
+ * interface and hosts various plugins of different algorithms to compute plans.
+ */
 class PlannerServer : public nav2_util::LifecycleNode
 {
 public:
+  /**
+   * @brief A constructor for nav2_planner::PlannerServer
+   */
   PlannerServer();
+  /**
+   * @brief A destructor for nav2_planner::PlannerServer
+   */
   ~PlannerServer();
 
 protected:
-  // Implement the lifecycle interface
+  /**
+   * @brief Configure member variables and initializes planner
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Activate member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Deactivate member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Reset member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in shutdown state
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in error state
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
   nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
 
   using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>;
@@ -59,10 +98,15 @@ protected:
   // Our action server implements the ComputePathToPose action
   std::unique_ptr<ActionServer> action_server_;
 
-  // The action server callback
+  /**
+   * @brief The action server callback which calls planner to get the path
+   */
   void computePathToPose();
 
-  // Publish a path for visualization purposes
+  /**
+   * @brief Publish a path for visualization purposes
+   * @param path Reference to Global Path
+   */
   void publishPlan(const nav_msgs::msg::Path & path);
 
   // Planner
diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp
index 6785d63bd6452ecd7221fb8492137c98fdaeac05..bf543f192888b300e62eac236a3caac103a95c94 100644
--- a/nav2_system_tests/src/localization/test_localization_node.cpp
+++ b/nav2_system_tests/src/localization/test_localization_node.cpp
@@ -51,7 +51,7 @@ public:
     initial_pose_pub_ = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
       "initialpose", rclcpp::SystemDefaultsQoS());
     subscription_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-      "amcl_pose", rclcpp::SystemDefaultsQoS(),
+      "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
       std::bind(&TestAmclPose::amcl_pose_callback, this, _1));
     initial_pose_pub_->publish(testPose_);
   }
@@ -85,7 +85,7 @@ bool TestAmclPose::defaultAmclTest()
   while (!pose_callback_) {
     // TODO(mhpanah): Initial pose should only be published once.
     initial_pose_pub_->publish(testPose_);
-    std::this_thread::sleep_for(100ms);
+    std::this_thread::sleep_for(1s);
     rclcpp::spin_some(node);
   }
   if (std::abs(amcl_pose_x - testPose_.pose.pose.position.x) < tol_ &&
diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py
index 5d1b6212dcf22cd712a59748359fbe5aafa3168d..292b1d40f1afd295f706fb303dc23d78b0bd02a3 100755
--- a/nav2_system_tests/src/system/test_multi_robot_launch.py
+++ b/nav2_system_tests/src/system/test_multi_robot_launch.py
@@ -17,7 +17,7 @@
 import os
 import sys
 
-from ament_index_python.packages import get_package_prefix, get_package_share_directory
+from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription, LaunchService
 from launch.actions import (ExecuteProcess, GroupAction,
@@ -35,7 +35,7 @@ def generate_launch_description():
     urdf = os.getenv('TEST_URDF')
     sdf = os.getenv('TEST_SDF')
 
-    bt_xml_file = os.path.join(get_package_prefix('nav2_bt_navigator'),
+    bt_xml_file = os.path.join(get_package_share_directory('nav2_bt_navigator'),
                                'behavior_trees',
                                os.getenv('BT_NAVIGATOR_XML'))
 
diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py
index e3b86c9abb185ef160b29c527c3975cbbaa6509e..ee4b4387b8608a77d868b8f962b6242f4b20ca02 100755
--- a/nav2_system_tests/src/system/test_system_launch.py
+++ b/nav2_system_tests/src/system/test_system_launch.py
@@ -17,7 +17,7 @@
 import os
 import sys
 
-from ament_index_python.packages import get_package_prefix, get_package_share_directory
+from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch import LaunchService
@@ -33,7 +33,7 @@ def generate_launch_description():
     map_yaml_file = os.getenv('TEST_MAP')
     world = os.getenv('TEST_WORLD')
 
-    bt_navigator_xml = os.path.join(get_package_prefix('nav2_bt_navigator'),
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
                                     'behavior_trees',
                                     os.getenv('BT_NAVIGATOR_XML'))
 
diff --git a/sphinx_doc/README.md b/sphinx_doc/README.md
index 09c2c178b8ef6c8d8b9245c2f9c1000e2999b1f4..ff7c1c80994143f9c58dbb7f69d673abafdb0504 100644
--- a/sphinx_doc/README.md
+++ b/sphinx_doc/README.md
@@ -1,6 +1,6 @@
-# Project Documentation
+# Navigation2 Documentation
 
 This folder holds the source and configuration files used to generate the
-Project documentation web site.
+[Navigation2 documentation](https://ros-planning.github.io/navigation2/) web site.
 
-Learn how to setup and generate documentation by reading the howtos
+Learn how to setup and generate documentation by reading the [howtos](https://ros-planning.github.io/navigation2/howtos/index.html).
diff --git a/sphinx_doc/getting_started/index.rst b/sphinx_doc/getting_started/index.rst
index 8779a82d05646212e3e5efb705c957c9d53d1142..9b72a8b7a96c51c42d3a9920123fdafa3f4b98aa 100644
--- a/sphinx_doc/getting_started/index.rst
+++ b/sphinx_doc/getting_started/index.rst
@@ -3,5 +3,129 @@
 Getting Started
 ###############
 
-This website is under construction. Please visit the `project repo`_ in the
-meantime.
+This document will take you through the process of installing the |PN| binaries
+and navigating a simulated Turtlebot 3 in the Gazebo simulator. We'll use the
+|Distro| version of ROS 2, since it is the latest stable version at the time
+this was written. The instructions are written primarily for Ubuntu 18, using
+the standard installation options.
+
+.. note::
+
+  See the :ref:`howtos` for other situations such as building from source or
+  working with other types of robots.
+
+.. warning::
+
+  This is a simplified version of the Turtlebot 3 instructions. We highly
+  recommend you follow the `official Turtlebot 3 manual`_ if you intend to
+  continue working with this robot beyond the minimal example provided here.
+
+Installation
+************
+
+1. Install the `ROS 2 binary packages`_ as described in the official docs
+2. Install the |PN| packages using your operating system's package manager. For
+   Ubuntu 18, do this:
+
+   .. code-block:: bash
+
+      sudo apt install ros2-dashing-navigation2
+      sudo apt install ros2-dashing-nav2-bringup
+
+3. Install the Turtlebot 3 packages. Again, for Ubuntu 18, it looks like this:
+
+   .. code-block:: bash
+
+      sudo apt install ros2-dashing-turtlebot3*
+
+Running the Example
+*******************
+
+1. Start a terminal in your GUI
+2. Set key environment variables. Here's how to do it in Ubuntu.
+
+   .. code-block:: bash
+
+      source /opt/ros/dashing/setup.bash
+      export TURTLEBOT3_MODEL=waffle
+      export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/dashing/share/turtlebot3_gazebo/models
+
+3. In the same terminal, run
+
+   .. code-block:: bash
+
+      ros2 launch nav2_bringup nav2_tb3_simulation_launch.py
+
+
+   If everything has started correctly, you will see the RViz and Gazebo GUIs like
+   this.
+
+   .. figure:: /images/rviz/rviz-not-started.png
+      :scale: 50%
+      :alt: Initial appearance of RViz before hitting startup button
+
+      Initial appearance of RViz before hitting startup button. Nothing is
+      displayed at this point because |PN| is still in the unconfigured state
+
+   .. figure:: /images/gazebo/gazebo_turtlebot1.png
+      :scale: 50%
+      :alt: Initial appearance of Gazebo with Turtlebot 3 world
+
+      Initial appearance of Gazebo with Turtlebot 3 world
+
+4. Click the "Startup" button in the bottom left corner of RViz. This will
+   cause |PN| to change to the Active lifecycle state. It should
+   change appearance to show the map.
+
+   .. figure:: /images/rviz/rviz_initial.png
+      :scale: 50%
+      :alt: Initial appearance of RViz transitioning to the Active state
+
+      Initial appearance of RViz transitioning to the Active state
+
+Navigating
+**********
+
+1. After starting, the robot initially has no idea where it is. By default,
+   |PN| waits for you to give it an approximate starting position. Take a look
+   at where the robot is in the Gazebo world, and find that spot on the map. Set
+   the initial pose by clicking the "2D Pose Estimate" button in RViz, and then
+   down clicking on the map in that location. You set the orientation by dragging
+   forward from the down click.
+
+   If you are using the defaults so far, it should look like this.
+
+   .. figure:: /images/rviz/rviz-set-initial-pose.png
+      :scale: 50%
+      :alt: Approximate starting location of Turtlebot
+
+      Approximate starting location of Turtlebot
+
+   If you don't get the location exactly right, that's fine. |PN| will refine
+   the position as it navigates. You can also, click the "2D Pose
+   Estimate" button and try again, if you prefer.
+
+   Once you've set the initial pose, the trasform tree will be complete and
+   |PN| is fully active and ready to go.
+
+   .. figure:: /images/rviz/navstack-ready.png
+      :scale: 50%
+      :alt: |PN| is ready. Transforms and Costmap show in RViz.
+
+      |PN| is ready. Transforms and Costmap show in RViz.
+
+2. Click the "Navigaton2 Goal" button and choose a destination.
+
+   .. figure:: /images/rviz/navigate-to-pose.png
+      :scale: 50%
+      :alt: Setting the goal pose in RViz.
+
+      Setting the goal pose in RViz.
+
+   Watch the robot go!
+
+   .. figure:: /images/rviz/navigating.png
+      :scale: 50%
+      :alt: Turtlebot on its way to the goal.
+
+      Turtlebot on its way to the goal.
diff --git a/sphinx_doc/how_it_works/index.rst b/sphinx_doc/how_it_works/index.rst
new file mode 100644
index 0000000000000000000000000000000000000000..553b8adfc95d8f946416d905dee5b39fea26b1f8
--- /dev/null
+++ b/sphinx_doc/how_it_works/index.rst
@@ -0,0 +1,7 @@
+.. _how_it_works:
+
+How It Works
+############
+
+This website is under construction. Please visit the `project repo`_ in the
+meantime.
diff --git a/sphinx_doc/images/gazebo/gazebo_turtlebot1.png b/sphinx_doc/images/gazebo/gazebo_turtlebot1.png
new file mode 100644
index 0000000000000000000000000000000000000000..b5776b16f4079886429c01ecf31b5009f3d9be7c
Binary files /dev/null and b/sphinx_doc/images/gazebo/gazebo_turtlebot1.png differ
diff --git a/sphinx_doc/images/rviz/navigate-to-pose.png b/sphinx_doc/images/rviz/navigate-to-pose.png
new file mode 100644
index 0000000000000000000000000000000000000000..0634a46c8c1f5f954d86f97dbfe76e51b58f88a9
Binary files /dev/null and b/sphinx_doc/images/rviz/navigate-to-pose.png differ
diff --git a/sphinx_doc/images/rviz/navigating.png b/sphinx_doc/images/rviz/navigating.png
new file mode 100644
index 0000000000000000000000000000000000000000..3e84ce8098d024b57de12869886a76942131df9f
Binary files /dev/null and b/sphinx_doc/images/rviz/navigating.png differ
diff --git a/sphinx_doc/images/rviz/navstack-ready.png b/sphinx_doc/images/rviz/navstack-ready.png
new file mode 100644
index 0000000000000000000000000000000000000000..43e89ea6caffa881d8aed6f79951f86fc8e292cb
Binary files /dev/null and b/sphinx_doc/images/rviz/navstack-ready.png differ
diff --git a/sphinx_doc/images/rviz/rviz-not-started.png b/sphinx_doc/images/rviz/rviz-not-started.png
new file mode 100644
index 0000000000000000000000000000000000000000..733471141b68b902df8250104f5c2132fd0726ed
Binary files /dev/null and b/sphinx_doc/images/rviz/rviz-not-started.png differ
diff --git a/sphinx_doc/images/rviz/rviz-set-initial-pose.png b/sphinx_doc/images/rviz/rviz-set-initial-pose.png
new file mode 100644
index 0000000000000000000000000000000000000000..c732e2ecf537ad84a5d0e4b1c79bd2dba0b2d3bb
Binary files /dev/null and b/sphinx_doc/images/rviz/rviz-set-initial-pose.png differ
diff --git a/sphinx_doc/images/rviz/rviz_initial.png b/sphinx_doc/images/rviz/rviz_initial.png
new file mode 100644
index 0000000000000000000000000000000000000000..5a1a4e505b1111f8df03526bf722e1b26bfdf668
Binary files /dev/null and b/sphinx_doc/images/rviz/rviz_initial.png differ
diff --git a/sphinx_doc/index.rst b/sphinx_doc/index.rst
index 404bddfedaedaa56498021ed312e1f5f9e25432b..7b9d996c864812e14581d5e404ba3bbf7721477e 100644
--- a/sphinx_doc/index.rst
+++ b/sphinx_doc/index.rst
@@ -1,10 +1,36 @@
 .. _documentation_home:
 
+*****
 |LPN|
-#####
+*****
 
-This website is under construction. Please visit the `project repo`_ in the
-meantime.
+.. raw:: html
+
+    <div style="position: relative; padding-bottom: 0%; overflow: hidden; max-width: 100%; height: auto;">
+      <iframe width="560" height="315" src="https://www.youtube.com/embed/ZeCds7Sv-5Q" frameborder="0" allowfullscreen></iframe>
+    </div>
+
+Overview
+########
+
+The |PN| project is a set of ROS2 components that help you build a robot that
+can drive around.
+
+It has tools to:
+
+- load and store maps
+- localize the robot on the map
+- plan a path from A to B around obstacles
+- control the robot as it follows the path and avoid any obstacles along the way
+- convert sensor data into a costmap representation of the world
+- build complicated robot behaviors using behavior trees
+
+These all work seemlessly together, so that, with a bit of customization for your
+robot, you can just give it a destination and off it go.
+
+Here you will find documentation on how to install and use |PN| with a Turtlebot
+3, as well as how to customize it for other robots, tune the behavior for better
+performance, as well as customize the internals for advanced results.
 
 Sections
 ********
@@ -13,6 +39,7 @@ Sections
    :maxdepth: 1
 
    getting_started/index.rst
+   how_it_works/index.rst
    howtos/index.rst
    release_notes.rst
    contribute/index.rst
diff --git a/sphinx_doc/substitutions.txt b/sphinx_doc/substitutions.txt
index c534aab4c919c4dc999bc5ac461392f1dc811206..cfd56b99a7dcd57a789b1d29762752816f73207f 100644
--- a/sphinx_doc/substitutions.txt
+++ b/sphinx_doc/substitutions.txt
@@ -15,6 +15,14 @@
 
 .. |PP| replace:: https://github.com/ros-planning/navigation2/blob/master
 
+.. |Distro| replace:: Dashing
+
+.. |distro| replace:: dashing
+
+.. _`ROS 2 binary packages`: https://index.ros.org/doc/ros2/Installation/Dashing/#binary-packages
+
+.. _`official Turtlebot 3 manual`: http://emanual.robotis.com/docs/en/platform/turtlebot3/ros2/#ros2
+
 .. These are replacement strings for non-ASCII characters used within the project
    using the same name as the html entity names (e.g., &copy;) for that character
 
diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos
index 71a7cdf3dec23a7dc6de94f220ca3977302bfd10..6b58b64a308d4cdf94999189d41359c0ec0d73e9 100644
--- a/tools/ros2_dependencies.repos
+++ b/tools/ros2_dependencies.repos
@@ -2,7 +2,7 @@ repositories:
   BehaviorTree.CPP:
     type: git
     url: https://github.com/BehaviorTree/BehaviorTree.CPP.git
-    version: 3.0.9
+    version: ros2-dev
   angles:
     type: git
     url: https://github.com/ros/angles.git