diff --git a/.circleci/config.yml b/.circleci/config.yml
index e5006d10779c32214d287e1cb8911803da485b83..8a53a5ac194f257a168abd07ea4944adf11aa416 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -193,7 +193,6 @@ references:
     run:
       name: Post Checkout
       command: |
-        rm $OVERLAY_WS/src/navigation2/nav2_system_tests/COLCON_IGNORE
         if ! cmp \
           $OVERLAY_WS/src/navigation2/tools/ros2_dependencies.repos \
           $UNDERLAY_WS/ros2_dependencies.repos >/dev/null 2>&1
diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md
index 99c04039fa3456e89ef174df994b8ec0b9e80484..d2d3108e77b2ceb608a33ac7d2a8f2ca1f4facd3 100644
--- a/.github/ISSUE_TEMPLATE.md
+++ b/.github/ISSUE_TEMPLATE.md
@@ -11,14 +11,12 @@ For Bug report or feature requests, please fill out the relevant category below
 
 - Operating System:
   - <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
-- Installation type:
-  - <!-- binaries or from source  -->
 - Version or commit hash:
-  - <!-- Output of git rev-parse HEAD, release version, or repos file  -->
+  - <!-- from source: output of `git -C navigation2 rev-parse HEAD
+         apt binaries: output of: dpkg-query --show "ros-$ROS_DISTRO-navigation2"
+                              or: dpkg-query --show "ros-$ROS_DISTRO-nav2-*" -->
 - DDS implementation:
-  - <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc -->
-- Client library (if applicable):
-  - <!-- e.g. rclcpp, rclpy, or N/A -->
+  - <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc.) -->
 
 #### Steps to reproduce issue
 <!-- Detailed instructions on how to reliably reproduce this issue http://sscce.org/
diff --git a/Dockerfile b/Dockerfile
index e5c8f82b6f915da430422cdffa857f69c29e1b2a..f56ed02530b41b6c3c40dddd9825ec21ad1f45c4 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -72,7 +72,6 @@ RUN . $UNDERLAY_WS/install/setup.sh && \
 
 # build overlay source
 ARG OVERLAY_MIXINS="release ccache"
-RUN rm $OVERLAY_WS/src/navigation2/nav2_system_tests/COLCON_IGNORE
 RUN . $UNDERLAY_WS/install/setup.sh && \
     colcon build \
       --symlink-install \
diff --git a/Dockerfile.full_ros_build b/Dockerfile.full_ros_build
index 923fccf058b8a82480cf34b5bb088e03fef0c9e8..945cf0844a9c8489a519c94551a9bba85cb3a5e6 100644
--- a/Dockerfile.full_ros_build
+++ b/Dockerfile.full_ros_build
@@ -3,7 +3,6 @@
 #
 # Example build command:
 # export CMAKE_BUILD_TYPE=Debug
-# export BUILD_SYSTEM_TESTS=False
 #
 # This determines which version of the ROS2 code base to pull
 # export ROS2_BRANCH=master
@@ -13,15 +12,16 @@
 # has been removed that an older codebase depends on. In those cases, using
 # ros:DISTRO-ros-core would be a good choice where distro is crystal, dashing, etc.
 # export ROS2_DOCKER_IMAGE=osrf/ros2:nightly
-# docker build -t nav2:full_ros_build --build-arg BUILD_SYSTEM_TESTS
-#   --build-arg CMAKE_BUILD_TYPE -f Dockerfile.full_ros_build ./
+# docker build -t nav2:full_ros_build
+#   --build-arg CMAKE_BUILD_TYPE \
+#   -f Dockerfile.full_ros_build ./
 
 # We're only building on top of a ROS docker image to get the basics
 # prerequisites installed such as the apt source, rosdep, etc. We don't want to
 # actually use any of the ros dashing packages. Instead we are going to build
 # everything from source in one big workspace.
-ARG ROS2_DOCKER_BRANCH=osrf/ros2:nightly
-FROM $ROS2_DOCKER_BRANCH
+ARG FROM_IMAGE=osrf/ros2:nightly
+FROM $FROM_IMAGE
 
 # install ROS2 dependencies
 RUN apt-get update && apt-get install -q -y \
@@ -49,15 +49,6 @@ ENV NAV2_DEPS_DIR $ROS2_WS/src/navigation2_dependencies
 RUN mkdir -p $NAV2_DEPS_DIR
 RUN vcs import src < $NAV2_DIR/tools/ros2_dependencies.repos
 
-# delete nav2_system_tests/COLCON_IGNORE before running rosdep, otherwise it
-# doesn't install nav2_system_tests dependencies.
-ARG BUILD_SYSTEM_TESTS=True
-RUN if [ "$BUILD_SYSTEM_TESTS" = "True" ] ; then \
-        rm $NAV2_DIR/nav2_system_tests/COLCON_IGNORE ; \
-        echo "Building of system tests enabled" ; \
-    fi
-
-
 # install package dependencies
 RUN apt-get update && \
     rosdep install -q -y \
diff --git a/Dockerfile.release_branch b/Dockerfile.release_branch
index af09df638b135dbff2d57cb87171622ada0319ff..4be39e3462a7504b88b8abb0b080fa0e06dfcf65 100644
--- a/Dockerfile.release_branch
+++ b/Dockerfile.release_branch
@@ -3,14 +3,13 @@
 #
 # Example build command:
 # export CMAKE_BUILD_TYPE=Debug
-# export BUILD_SYSTEM_TESTS=False
-# docker build -t nav2:release_branch --build-arg ROS2_BRANCH=dashing \
-#  --build-arg BUILD_SYSTEM_TESTS #   --build-arg CMAKE_BUILD_TYPE \
-#  -f Dockerfile.release_branch ./
+# docker build -t nav2:release_branch \
+#   --build-arg FROM_IMAGE=dashing \
+#   --build-arg CMAKE_BUILD_TYPE \
+#   -f Dockerfile.release_branch ./
 
-ARG ROS2_BRANCH=dashing
-
-FROM ros:$ROS2_BRANCH
+ARG FROM_IMAGE=dashing
+FROM ros:$FROM_IMAGE
 
 # copy ros package repo
 ENV NAV2_WS /opt/nav2_ws
@@ -18,14 +17,6 @@ RUN mkdir -p $NAV2_WS/src
 WORKDIR $NAV2_WS/src
 COPY ./ navigation2/
 
-# delete nav2_system_tests/COLCON_IGNORE before running rosdep, otherwise it
-# doesn't install nav2_system_tests dependencies.
-ARG BUILD_SYSTEM_TESTS=True
-RUN if [ "$BUILD_SYSTEM_TESTS" = "True" ] ; then \
-        rm $NAV2_WS/src/navigation2/nav2_system_tests/COLCON_IGNORE ; \
-        echo "Building of system tests enabled" ; \
-    fi
-
 # install navigation2 package dependencies
 WORKDIR $NAV2_WS
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
diff --git a/doc/BUILD.md b/doc/BUILD.md
index 8fc7b11a309aa893f7c2ca5c01766e80c18c8db4..609ba6a537c7e3ed46c579c49b2830cbb40a9131 100644
--- a/doc/BUILD.md
+++ b/doc/BUILD.md
@@ -5,18 +5,9 @@ Quickstart
 ----------
 
 ### Steps
-First, install all ROS2 dependencies from the ROS2 Installation page: https://github.com/ros2/ros2/wiki/Linux-Development-Setup
+- Install all ROS2 dependencies from the ROS2 Installation page: https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Development-Setup/
 
-Second, install the dependencies for this repository:
-```sh
-RUN apt-get install -y \
-    libsdl-image1.2 \
-    libsdl-image1.2-dev \
-    libsdl1.2debian \
-    libsdl1.2-dev
-```
-
-Third, ensure there are no ROS environment variables set in your terminal or `.bashrc` file before taking the steps below.*
+- Ensure there are no ROS environment variables set in your terminal or `.bashrc` file before taking the steps below.*
 
 ```sh
 mkdir <directory_for_workspaces>
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index f16647dd357249bd5e22f6a5e2464e63ad2022aa..e46d344f0d1c06ca92759eb56b4aa066dc9b8867 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -14,7 +14,6 @@ find_package(nav_msgs REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
@@ -43,7 +42,6 @@ set(dependencies
   tf2
   tf2_ros
   tf2_geometry_msgs
-  rosidl_default_generators
   std_msgs
   std_srvs
   nav2_util
diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
index 52fea6ddc22a85a09ffcf0bb90bc1e978c5ea105..43d1d095f6634ee174fa3eb1eba68bba2064d6c2 100644
--- a/nav2_behavior_tree/package.xml
+++ b/nav2_behavior_tree/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <build_export_depend>tf2_geometry_msgs</build_export_depend>
+  <build_export_depend>std_srvs</build_export_depend>
 
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_action</build_depend>
@@ -19,7 +20,6 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>behaviortree_cpp</build_depend>
   <build_depend>builtin_interfaces</build_depend>
-  <build_depend>rosidl_default_generators</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav2_msgs</build_depend>
   <build_depend>nav_msgs</build_depend>
@@ -38,7 +38,6 @@
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>behaviortree_cpp</exec_depend>
   <exec_depend>builtin_interfaces</exec_depend>
-  <exec_depend>rosidl_default_runtime</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
@@ -55,8 +54,6 @@
   <test_depend>launch</test_depend>
   <test_depend>launch_testing</test_depend>
 
-  <member_of_group>rosidl_interface_packages</member_of_group>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/nav2_bringup/launch/nav2_navigation_launch.py b/nav2_bringup/launch/nav2_navigation_launch.py
index a588634e6aad0681f843dfbaa39e5a83849fce26..3be794f0f116cfac64ac894fc2b5231296f4e03c 100644
--- a/nav2_bringup/launch/nav2_navigation_launch.py
+++ b/nav2_bringup/launch/nav2_navigation_launch.py
@@ -31,12 +31,12 @@ def generate_launch_description():
     params_file = launch.substitutions.LaunchConfiguration('params')
     bt_xml_file = launch.substitutions.LaunchConfiguration('bt_xml_file')
 
-
     # Create our own temporary YAML files that include substitutions
     param_substitutions = {
         'use_sim_time': use_sim_time,
         'bt_xml_filename': bt_xml_file,
-        'autostart': autostart
+        'autostart': autostart,
+        'map_subscribe_transient_local': 'False'
     }
 
     configured_params = RewrittenYaml(
diff --git a/nav2_bringup/launch/nav2_params.yaml b/nav2_bringup/launch/nav2_params.yaml
index 6e68463d4e1a6c4fd5cccd9103c4c803cfb92679..bd19d8632ff2d3f373ab2abfc3ac8f4577519ce5 100644
--- a/nav2_bringup/launch/nav2_params.yaml
+++ b/nav2_bringup/launch/nav2_params.yaml
@@ -104,6 +104,8 @@ local_costmap:
       inflation_layer.cost_scaling_factor: 3.0
       obstacle_layer:
         enabled: True
+      static_layer:
+        map_subscribe_transient_local: True
       always_send_full_costmap: True
       observation_sources: scan
       scan:
@@ -125,6 +127,8 @@ global_costmap:
       robot_radius: 0.22
       obstacle_layer:
         enabled: True
+      static_layer:
+        map_subscribe_transient_local: True
       always_send_full_costmap: True
       observation_sources: scan
       scan:
diff --git a/nav2_bringup/launch/nav2_simulation_launch.py b/nav2_bringup/launch/nav2_simulation_launch.py
index 76ddedebe0d6484b93957abf4b47df236c8a0856..9f3045136e4f9bae25444ee446e3dcb359d6c88a 100644
--- a/nav2_bringup/launch/nav2_simulation_launch.py
+++ b/nav2_bringup/launch/nav2_simulation_launch.py
@@ -116,7 +116,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_share_directory('turtlebot3_description'),
                 'urdf', 'turtlebot3_waffle.urdf'),
-            ['__params:=', configured_params]],
+            '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 
     start_rviz_cmd = launch.actions.ExecuteProcess(
@@ -134,7 +134,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_map_server'),
                 'lib/nav2_map_server/map_server'),
-            ['__params:=', configured_params]],
+            '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 
     start_localizer_cmd = launch.actions.ExecuteProcess(
@@ -142,7 +142,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_amcl'),
                 'lib/nav2_amcl/amcl'),
-            ['__params:=', configured_params]],
+            '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 
     start_planner_cmd = launch.actions.ExecuteProcess(
@@ -150,7 +150,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_planner'),
                 'lib/nav2_planner/planner_server'),
-            ['__params:=', configured_params]],
+            '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 
     start_dwb_cmd = launch.actions.ExecuteProcess(
@@ -158,7 +158,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('dwb_controller'),
                 'lib/dwb_controller/dwb_controller'),
-            ['__params:=', configured_params]],
+            '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 
     start_navigator_cmd = launch.actions.ExecuteProcess(
@@ -166,7 +166,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_bt_navigator'),
                 'lib/nav2_bt_navigator/bt_navigator'),
-            ['__params:=', configured_params]],
+            '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 
     start_recovery_cmd = launch.actions.ExecuteProcess(
@@ -174,7 +174,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_recoveries'),
                 'lib/nav2_recoveries/recoveries_node'),
-            ['__params:=', configured_params]],
+            '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 
     start_lifecycle_manager_cmd = launch.actions.ExecuteProcess(
@@ -182,7 +182,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_lifecycle_manager'),
                 'lib/nav2_lifecycle_manager/lifecycle_manager'),
-            ['__params:=', configured_params]],
+            '--ros-args', '--params-file', configured_params],
         cwd=[launch_dir], output='screen')
 
     # Create the launch description and populate
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index e9751ee15cf51b251b049a2ad1f3b3c03a69139f..ff03494f3fe87b3ea3b147f23c5956c54011c56b 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -47,7 +47,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   auto options = rclcpp::NodeOptions().arguments(
     {"--ros-args",
-      std::string("__node:=") + get_name() + "_client_node",
+      "-r", std::string("__node:=") + get_name() + "_client_node",
       "--"});
   // Support for handling the topic-based goal pose from rviz
   client_node_ = std::make_shared<rclcpp::Node>("_", options);
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
index 6ce5b043fca3525b79685ceea546f43f39a167a5..05a518558361023ab18edeee127d4bd366e155fa 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
@@ -109,7 +109,7 @@ private:
   unsigned char lethal_threshold_;
   unsigned char unknown_cost_value_;
   bool trinary_costmap_;
-  bool map_received_;
+  bool map_received_{false};
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index 932c062a2df434d7177ce5b9f31e0a34d3fb8053..347a139bb9c30b39dea6a94e06f42692505fcb6a 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -75,11 +75,11 @@ void ObstacleLayer::onInitialize()
   std::string topics_string;
 
   // TODO(mjeronimo): these four are candidates for dynamic update
-  node_->declare_parameter(name_ + "." + "enabled", rclcpp::ParameterValue(true));
-  node_->declare_parameter(name_ + "." + "footprint_clearing_enabled",
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("footprint_clearing_enabled",
     rclcpp::ParameterValue(true));
-  node_->declare_parameter(name_ + "." + "max_obstacle_height", rclcpp::ParameterValue(2.0));
-  node_->declare_parameter(name_ + "." + "combination_method", rclcpp::ParameterValue(1));
+  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
+  declareParameter("combination_method", rclcpp::ParameterValue(1));
 
   node_->get_parameter(name_ + "." + "enabled", enabled_);
   node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
@@ -114,31 +114,31 @@ void ObstacleLayer::onInitialize()
     std::string topic, sensor_frame, data_type;
     bool inf_is_valid, clearing, marking;
 
-    node_->declare_parameter(source + "." + "topic", rclcpp::ParameterValue(source));
-    node_->declare_parameter(source + "." + "sensor_frame",
-      rclcpp::ParameterValue(std::string("")));
-    node_->declare_parameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0));
-    node_->declare_parameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0));
-    node_->declare_parameter(source + "." + "data_type",
-      rclcpp::ParameterValue(std::string("LaserScan")));
-    node_->declare_parameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));
-    node_->declare_parameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(0.0));
-    node_->declare_parameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));
-    node_->declare_parameter(source + "." + "marking", rclcpp::ParameterValue(true));
-    node_->declare_parameter(source + "." + "clearing", rclcpp::ParameterValue(false));
-    node_->declare_parameter(source + "." + "obstacle_range", rclcpp::ParameterValue(2.5));
-    node_->declare_parameter(source + "." + "raytrace_range", rclcpp::ParameterValue(3.0));
-
-    node_->get_parameter(source + "." + "topic", topic);
-    node_->get_parameter(source + "." + "sensor_frame", sensor_frame);
-    node_->get_parameter(source + "." + "observation_persistence", observation_keep_time);
-    node_->get_parameter(source + "." + "expected_update_rate", expected_update_rate);
-    node_->get_parameter(source + "." + "data_type", data_type);
-    node_->get_parameter(source + "." + "min_obstacle_height", min_obstacle_height);
-    node_->get_parameter(source + "." + "max_obstacle_height", max_obstacle_height);
-    node_->get_parameter(source + "." + "inf_is_valid", inf_is_valid);
-    node_->get_parameter(source + "." + "marking", marking);
-    node_->get_parameter(source + "." + "clearing", clearing);
+    declareParameter(source + "." + "topic", rclcpp::ParameterValue(source));
+    declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));
+    declareParameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "data_type", rclcpp::ParameterValue(std::string("LaserScan")));
+    declareParameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));
+    declareParameter(source + "." + "marking", rclcpp::ParameterValue(true));
+    declareParameter(source + "." + "clearing", rclcpp::ParameterValue(false));
+    declareParameter(source + "." + "obstacle_range", rclcpp::ParameterValue(2.5));
+    declareParameter(source + "." + "raytrace_range", rclcpp::ParameterValue(3.0));
+
+    node_->get_parameter(name_ + "." + source + "." + "topic", topic);
+    node_->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame);
+    node_->get_parameter(name_ + "." + source + "." + "observation_persistence",
+      observation_keep_time);
+    node_->get_parameter(name_ + "." + source + "." + "expected_update_rate",
+      expected_update_rate);
+    node_->get_parameter(name_ + "." + source + "." + "data_type", data_type);
+    node_->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height);
+    node_->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height);
+    node_->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
+    node_->get_parameter(name_ + "." + source + "." + "marking", marking);
+    node_->get_parameter(name_ + "." + source + "." + "clearing", clearing);
 
     if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
       RCLCPP_FATAL(node_->get_logger(),
@@ -149,11 +149,11 @@ void ObstacleLayer::onInitialize()
 
     // get the obstacle range for the sensor
     double obstacle_range;
-    node_->get_parameter(source + "." + "obstacle_range", obstacle_range);
+    node_->get_parameter(name_ + "." + source + "." + "obstacle_range", obstacle_range);
 
     // get the raytrace range for the sensor
     double raytrace_range;
-    node_->get_parameter(source + "." + "raytrace_range", raytrace_range);
+    node_->get_parameter(name_ + "." + source + "." + "raytrace_range", raytrace_range);
 
     RCLCPP_DEBUG(node_->get_logger(),
       "Creating an observation buffer for source %s, topic %s, frame %s",
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index 8738e50c54ca9bb7d37d8096edf8084ccb048d9a..a33c4bcc372897f2ba6a0a19e8d7fe4b4e0c1f84 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -72,12 +72,14 @@ StaticLayer::onInitialize()
 
   getParameters();
 
-  rclcpp::QoS map_qos(1);
+  rclcpp::QoS map_qos(10); // initialize to default
   if (map_subscribe_transient_local_) {
     map_qos.transient_local();
+    map_qos.reliable();
+    map_qos.keep_last(1);
   }
 
-  RCLCPP_DEBUG(node_->get_logger(),
+  RCLCPP_INFO(node_->get_logger(),
     "Subscribing to the map topic (%s) with %s durability",
     map_topic_.c_str(),
     map_subscribe_transient_local_ ? "transient local" : "volatile");
@@ -121,8 +123,7 @@ StaticLayer::getParameters()
 
   declareParameter("enabled", rclcpp::ParameterValue(true));
   declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false));
-  declareParameter("map_subscribe_transient_local",
-    rclcpp::ParameterValue(true));
+  declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true));
 
   node_->get_parameter(name_ + "." + "enabled", enabled_);
   node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
@@ -326,7 +327,16 @@ StaticLayer::updateCosts(
   nav2_costmap_2d::Costmap2D & master_grid,
   int min_i, int min_j, int max_i, int max_j)
 {
-  if (!enabled_ || !map_received_) {
+  if (!enabled_) {
+    return;
+  }
+  if (!map_received_) {
+    static int count = 0;
+    // throttle warning down to only 1/10 message rate
+    if (++count == 10) {
+      RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received");
+      count = 0;
+    }
     return;
   }
 
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 919dc03af359eaa9e975bebde37026efe1f9f523..105b88a1492b500b3ab43a811c819cee6b1cd89e 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -59,17 +59,16 @@ void VoxelLayer::onInitialize()
 {
   ObstacleLayer::onInitialize();
 
-  node_->declare_parameter(name_ + "." + "enabled", rclcpp::ParameterValue(true));
-  node_->declare_parameter(name_ + "." + "footprint_clearing_enabled",
-    rclcpp::ParameterValue(true));
-  node_->declare_parameter(name_ + "." + "max_obstacle_height", rclcpp::ParameterValue(2.0));
-  node_->declare_parameter(name_ + "." + "z_voxels", rclcpp::ParameterValue(10));
-  node_->declare_parameter(name_ + "." + "origin_z", rclcpp::ParameterValue(0.0));
-  node_->declare_parameter(name_ + "." + "z_resolution", rclcpp::ParameterValue(0.2));
-  node_->declare_parameter(name_ + "." + "unknown_threshold", rclcpp::ParameterValue(15));
-  node_->declare_parameter(name_ + "." + "mark_threshold", rclcpp::ParameterValue(0));
-  node_->declare_parameter(name_ + "." + "combination_method", rclcpp::ParameterValue(1));
-  node_->declare_parameter(name_ + "." + "publish_voxel_map", rclcpp::ParameterValue(false));
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
+  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
+  declareParameter("z_voxels", rclcpp::ParameterValue(10));
+  declareParameter("origin_z", rclcpp::ParameterValue(0.0));
+  declareParameter("z_resolution", rclcpp::ParameterValue(0.2));
+  declareParameter("unknown_threshold", rclcpp::ParameterValue(15));
+  declareParameter("mark_threshold", rclcpp::ParameterValue(0));
+  declareParameter("combination_method", rclcpp::ParameterValue(1));
+  declareParameter("publish_voxel_map", rclcpp::ParameterValue(false));
 
   node_->get_parameter(name_ + "." + "enabled", enabled_);
   node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
index b5341e5b82b0bc4b011356d523ecec5e037f1729..c1b994aa5d56f7617c2dff2e5503b12443b19f4f 100644
--- a/nav2_costmap_2d/src/clear_costmap_service.cpp
+++ b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -38,9 +38,6 @@ ClearCostmapService::ClearCostmapService(
 {
   reset_value_ = costmap_.getCostmap()->getDefaultValue();
 
-  std::vector<std::string> clearable_layers{"obstacle_layer"};
-  node_->declare_parameter("clearable_layers", rclcpp::ParameterValue(clearable_layers));
-
   node_->get_parameter("clearable_layers", clearable_layers_);
 
   clear_except_service_ = node_->create_service<ClearExceptRegion>(
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index ffb38e4929a5a5ee432a0c3f8baed96551d144b3..9b2647b49c45335609b7b74a41d5330386b30b48 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -59,17 +59,19 @@ Costmap2DROS::Costmap2DROS(const std::string & name, const std::string & absolut
 : nav2_util::LifecycleNode(name, "", true,
     // NodeOption arguments take precedence over the ones provided on the command line
     // use this to make sure the node is placed on the provided namespace
-    rclcpp::NodeOptions().arguments({std::string("__ns:=") + absolute_namespace})),
+    rclcpp::NodeOptions().arguments({"--ros-args", "-r", std::string(
+        "__ns:=") + absolute_namespace})),
   name_(name)
 {
   RCLCPP_INFO(get_logger(), "Creating Costmap");
   auto options = rclcpp::NodeOptions().arguments(
-    {"--ros-args", std::string("__node:=") + get_name() + "_client", "--"});
+    {"--ros-args", "-r", std::string("__node:=") + get_name() + "_client", "--"});
   client_node_ = std::make_shared<rclcpp::Node>("_", options);
 
   std::vector<std::string> plugin_names{"static_layer", "obstacle_layer", "inflation_layer"};
   std::vector<std::string> plugin_types{"nav2_costmap_2d::StaticLayer",
     "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"};
+  std::vector<std::string> clearable_layers{"obstacle_layer"};
 
   declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false));
   declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f));
@@ -95,6 +97,7 @@ Costmap2DROS::Costmap2DROS(const std::string & name, const std::string & absolut
   declare_parameter("update_frequency", rclcpp::ParameterValue(5.0));
   declare_parameter("use_maximum", rclcpp::ParameterValue(false));
   declare_parameter("width", rclcpp::ParameterValue(10));
+  declare_parameter("clearable_layers", rclcpp::ParameterValue(clearable_layers));
 }
 
 Costmap2DROS::~Costmap2DROS()
@@ -384,7 +387,7 @@ Costmap2DROS::mapUpdateLoop(double frequency)
       if ((last_publish_ + publish_cycle_ < current_time) ||  // publish_cycle_ is due
         (current_time < last_publish_))      // time has moved backwards, probably due to a switch to sim_time // NOLINT
       {
-        RCLCPP_INFO(get_logger(), "Publish costmap at %s", name_.c_str());
+        RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
         costmap_publisher_->publishCostmap();
         last_publish_ = current_time;
       }
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index 9e381f99a68d4aee01085234e2bedf58fd3a203c..fc07b7e7771690abc62703d7e1e19e5bf339fdd5 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -1,5 +1,5 @@
 #include <nav2_costmap_2d/costmap_layer.hpp>
-
+#include <stdexcept>
 #include <algorithm>
 
 namespace nav2_costmap_2d
@@ -86,6 +86,11 @@ void CostmapLayer::updateWithTrueOverwrite(
   if (!enabled_) {
     return;
   }
+
+  if (costmap_ == nullptr) {
+    throw std::runtime_error("Can't update costmap layer: It has't been initialized yet!");
+  }
+
   unsigned char * master = master_grid.getCharMap();
   unsigned int span = master_grid.getSizeInCellsX();
 
diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp
index e995846c7197aab81d8c2c3d9eb69e43413b535d..228c060f70d10ac93b04e73c824cc22269567004 100644
--- a/nav2_costmap_2d/src/layer.cpp
+++ b/nav2_costmap_2d/src/layer.cpp
@@ -31,6 +31,7 @@
 
 #include <string>
 #include <vector>
+#include "nav2_util/node_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -71,7 +72,7 @@ Layer::declareParameter(
   const std::string & param_name, const rclcpp::ParameterValue & value)
 {
   local_params_.insert(param_name);
-  node_->declare_parameter(getFullName(param_name), value);
+  nav2_util::declare_parameter_if_not_declared(node_, getFullName(param_name), value);
 }
 
 bool
diff --git a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
index 96ea343f318d8edba7797409c64653448bade7dc..ad057ca0d60a725f775d4486e8ac842cdbe0b6a3 100644
--- a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
+++ b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
@@ -76,7 +76,7 @@ DwbController::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("controller_frequency", controller_frequency_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
-  odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(*this);
+  odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(node);
   vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
 
   // Create the action server that we implement with our followPath method
diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
index cb8881c8fedb33644003fccb1428a6a1a0c1ecfd..a9214244c588632a406349da88dbe2512df74f3b 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -47,8 +47,11 @@
 #include "nav_2d_utils/parameters.hpp"
 #include "nav_2d_utils/tf_help.hpp"
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
 #include "pluginlib/class_list_macros.hpp"
 
+using nav2_util::declare_parameter_if_not_declared;
+
 namespace dwb_core
 {
 
@@ -62,16 +65,17 @@ DWBLocalPlanner::DWBLocalPlanner(
   goal_checker_loader_("dwb_core", "dwb_core::GoalChecker"),
   critic_loader_("dwb_core", "dwb_core::TrajectoryCritic")
 {
-  node_->declare_parameter("critics");
-  node_->declare_parameter("prune_plan", rclcpp::ParameterValue(true));
-  node_->declare_parameter("prune_distance", rclcpp::ParameterValue(1.0));
-  node_->declare_parameter("debug_trajectory_details", rclcpp::ParameterValue(false));
-  node_->declare_parameter("trajectory_generator_name",
+  declare_parameter_if_not_declared(node_, "critics");
+  declare_parameter_if_not_declared(node_, "prune_plan", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(node_, "prune_distance", rclcpp::ParameterValue(1.0));
+  declare_parameter_if_not_declared(node_, "debug_trajectory_details",
+    rclcpp::ParameterValue(false));
+  declare_parameter_if_not_declared(node_, "trajectory_generator_name",
     rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator")));
-  node_->declare_parameter("goal_checker_name",
+  declare_parameter_if_not_declared(node_, "goal_checker_name",
     rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker")));
-  node_->declare_parameter("use_dwa", rclcpp::ParameterValue(false));
-  node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1));
+  declare_parameter_if_not_declared(node_, "use_dwa", rclcpp::ParameterValue(false));
+  declare_parameter_if_not_declared(node_, "transform_tolerance", rclcpp::ParameterValue(0.1));
 }
 
 nav2_util::CallbackReturn
@@ -173,7 +177,8 @@ DWBLocalPlanner::loadCritics()
     std::string plugin_name = critic_names[i];
     std::string plugin_class;
 
-    node_->declare_parameter(plugin_name + "/class", rclcpp::ParameterValue(plugin_name));
+    declare_parameter_if_not_declared(node_, plugin_name + "/class",
+      rclcpp::ParameterValue(plugin_name));
     node_->get_parameter(plugin_name + "/class", plugin_class);
 
     plugin_class = resolveCriticClassName(plugin_class);
@@ -211,18 +216,18 @@ DWBLocalPlanner::loadBackwardsCompatibleParameters()
                                                 //   (local) goal, based on wave propagation
   node_->set_parameters({rclcpp::Parameter("critics", critic_names)});
 
-  node_->declare_parameter("path_distance_bias");
-  node_->declare_parameter("goal_distance_bias");
-  node_->declare_parameter("occdist_scale");
-  node_->declare_parameter("max_scaling_factor");
-  node_->declare_parameter("scaling_speed");
-  node_->declare_parameter("PathAlign.scale");
-  node_->declare_parameter("GoalAlign.scale");
-  node_->declare_parameter("PathDist.scale");
-  node_->declare_parameter("GoalDist.scale");
-  node_->declare_parameter("ObstacleFootprint.scale");
-  node_->declare_parameter("ObstacleFootprint.max_scaling_factor");
-  node_->declare_parameter("ObstacleFootprint.scaling_speed");
+  declare_parameter_if_not_declared(node_, "path_distance_bias");
+  declare_parameter_if_not_declared(node_, "goal_distance_bias");
+  declare_parameter_if_not_declared(node_, "occdist_scale");
+  declare_parameter_if_not_declared(node_, "max_scaling_factor");
+  declare_parameter_if_not_declared(node_, "scaling_speed");
+  declare_parameter_if_not_declared(node_, "PathAlign.scale");
+  declare_parameter_if_not_declared(node_, "GoalAlign.scale");
+  declare_parameter_if_not_declared(node_, "PathDist.scale");
+  declare_parameter_if_not_declared(node_, "GoalDist.scale");
+  declare_parameter_if_not_declared(node_, "ObstacleFootprint.scale");
+  declare_parameter_if_not_declared(node_, "ObstacleFootprint.max_scaling_factor");
+  declare_parameter_if_not_declared(node_, "ObstacleFootprint.scaling_speed");
 
   /* *INDENT-OFF* */
   nav_2d_utils::moveParameter(node_, "path_distance_bias", "PathAlign.scale", 32.0, false);
diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp
index 60aafa658942ec447612605f64e89f3bce775acf..245958708af12741f755bc66ce9bfd69a150ae52 100644
--- a/nav2_dwb_controller/dwb_core/src/publisher.cpp
+++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp
@@ -40,12 +40,14 @@
 #include <vector>
 
 #include "nav_2d_utils/conversions.hpp"
+#include "nav2_util/node_utils.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 #include "visualization_msgs/msg/marker.hpp"
 
 using std::max;
 using std::string;
+using nav2_util::declare_parameter_if_not_declared;
 
 namespace dwb_core
 {
@@ -53,12 +55,13 @@ namespace dwb_core
 DWBPublisher::DWBPublisher(nav2_util::LifecycleNode::SharedPtr node)
 : node_(node)
 {
-  node_->declare_parameter("publish_evaluation", rclcpp::ParameterValue(true));
-  node_->declare_parameter("publish_global_plan", rclcpp::ParameterValue(true));
-  node_->declare_parameter("publish_transformed_plan", rclcpp::ParameterValue(true));
-  node_->declare_parameter("publish_local_plan", rclcpp::ParameterValue(true));
-  node_->declare_parameter("publish_trajectories", rclcpp::ParameterValue(true));
-  node_->declare_parameter("publish_cost_grid_pc", rclcpp::ParameterValue(false));
+  declare_parameter_if_not_declared(node_, "publish_evaluation", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(node_, "publish_global_plan", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(node_, "publish_transformed_plan",
+    rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(node_, "publish_local_plan", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(node_, "publish_trajectories", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(node_, "publish_cost_grid_pc", rclcpp::ParameterValue(false));
 }
 
 nav2_util::CallbackReturn
diff --git a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
index 1dd7c2023f3fca5cd2713696908174f1b9276662..03cde45f17d59054a6534bdee73093d88471ed5b 100644
--- a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
@@ -36,6 +36,7 @@
 #include "dwb_core/exceptions.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_util/node_utils.hpp"
 
 PLUGINLIB_EXPORT_CLASS(dwb_critics::BaseObstacleCritic, dwb_core::TrajectoryCritic)
 
@@ -46,7 +47,8 @@ void BaseObstacleCritic::onInit()
 {
   costmap_ = costmap_ros_->getCostmap();
 
-  nh_->declare_parameter(name_ + ".sum_scores", rclcpp::ParameterValue(false));
+  nav2_util::declare_parameter_if_not_declared(nh_,
+    name_ + ".sum_scores", rclcpp::ParameterValue(false));
   nh_->get_parameter(name_ + ".sum_scores", sum_scores_);
 }
 
diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
index 4dfa9fab4230989867d65c8282209ce5109173af..e7218fd8f8dfebfa081fefdc98ea8426dce8faa1 100644
--- a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
@@ -39,6 +39,7 @@
 #include <memory>
 #include "dwb_core/exceptions.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_util/node_utils.hpp"
 
 using std::abs;
 using costmap_queue::CellData;
@@ -60,7 +61,8 @@ void MapGridCritic::onInit()
   // Always set to true, but can be overriden by subclasses
   stop_on_failure_ = true;
 
-  nh_->declare_parameter(name_ + ".aggregation_type", rclcpp::ParameterValue(std::string("last")));
+  nav2_util::declare_parameter_if_not_declared(nh_,
+    name_ + ".aggregation_type", rclcpp::ParameterValue(std::string("last")));
 
   std::string aggro_str;
   nh_->get_parameter(name_ + ".aggregation_type", aggro_str);
diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
index 1dd8d742b1268a5f223718efc508e19a46ee28dc..9d6f8edc091eaf615d7d45b35ff697b8b2feae99 100644
--- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
@@ -38,6 +38,7 @@
 #include <string>
 #include <vector>
 #include "nav_2d_utils/parameters.hpp"
+#include "nav2_util/node_utils.hpp"
 #include "dwb_core/exceptions.hpp"
 #include "pluginlib/class_list_macros.hpp"
 
@@ -96,7 +97,8 @@ void OscillationCritic::onInit()
   oscillation_reset_time_ = rclcpp::Duration::from_seconds(
     nav_2d_utils::searchAndGetParam(nh_, "oscillation_reset_time", -1.0));
 
-  nh_->declare_parameter(name_ + ".x_only_threshold", rclcpp::ParameterValue(0.05));
+  nav2_util::declare_parameter_if_not_declared(nh_,
+    name_ + ".x_only_threshold", rclcpp::ParameterValue(0.05));
 
   /**
    * Historical Parameter Loading
diff --git a/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp b/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp
index 82b08ce2e9242643abb37f993f9c379e98e08f65..fb52d94e17d92544f3bf5c2e6772d8bb70b2e7b6 100644
--- a/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp
@@ -35,18 +35,21 @@
 #include "dwb_critics/prefer_forward.hpp"
 #include <math.h>
 #include "pluginlib/class_list_macros.hpp"
+#include "nav2_util/node_utils.hpp"
 
 PLUGINLIB_EXPORT_CLASS(dwb_critics::PreferForwardCritic, dwb_core::TrajectoryCritic)
 
+using nav2_util::declare_parameter_if_not_declared;
+
 namespace dwb_critics
 {
 
 void PreferForwardCritic::onInit()
 {
-  nh_->declare_parameter(name_ + ".penalty", rclcpp::ParameterValue(1.0));
-  nh_->declare_parameter(name_ + ".strafe_x", rclcpp::ParameterValue(0.1));
-  nh_->declare_parameter(name_ + ".strafe_theta", rclcpp::ParameterValue(0.2));
-  nh_->declare_parameter(name_ + ".theta_scale", rclcpp::ParameterValue(10.0));
+  declare_parameter_if_not_declared(nh_, name_ + ".penalty", rclcpp::ParameterValue(1.0));
+  declare_parameter_if_not_declared(nh_, name_ + ".strafe_x", rclcpp::ParameterValue(0.1));
+  declare_parameter_if_not_declared(nh_, name_ + ".strafe_theta", rclcpp::ParameterValue(0.2));
+  declare_parameter_if_not_declared(nh_, name_ + ".theta_scale", rclcpp::ParameterValue(10.0));
 
   nh_->get_parameter(name_ + ".penalty", penalty_);
   nh_->get_parameter(name_ + ".strafe_x", strafe_x_);
diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
index 63eb16471f15b2b790685adc8c6582f34b211549..6129257e9e85bb328732093a9e1fa0b855c9120e 100644
--- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
@@ -39,10 +39,12 @@
 #include <string>
 
 #include "nav_2d_utils/parameters.hpp"
+#include "nav2_util/node_utils.hpp"
 
 using std::fabs;
-
+using nav2_util::declare_parameter_if_not_declared;
 using nav_2d_utils::moveDeprecatedParameter;
+
 namespace dwb_plugins
 {
 
@@ -58,20 +60,20 @@ void KinematicParameters::initialize(const nav2_util::LifecycleNode::SharedPtr &
   moveDeprecatedParameter<double>(nh, "max_speed_xy", "max_trans_vel");
   moveDeprecatedParameter<double>(nh, "min_speed_theta", "min_rot_vel");
 
-  nh->declare_parameter("min_vel_x", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("min_vel_y", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("max_vel_x", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("max_vel_y", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("max_vel_theta", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("min_speed_xy", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("max_speed_xy", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("min_speed_theta", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("acc_lim_x", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("acc_lim_y", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("acc_lim_theta", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("decel_lim_x", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("decel_lim_y", rclcpp::ParameterValue(0.0));
-  nh->declare_parameter("decel_lim_theta", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "min_vel_x", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "min_vel_y", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "max_vel_x", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "max_vel_y", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "max_vel_theta", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "min_speed_xy", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "max_speed_xy", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "min_speed_theta", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "acc_lim_x", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "acc_lim_y", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "acc_lim_theta", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "decel_lim_x", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "decel_lim_y", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(nh, "decel_lim_theta", rclcpp::ParameterValue(0.0));
 
   nh->get_parameter("min_vel_x", min_vel_x_);
   nh->get_parameter("min_vel_y", min_vel_y_);
diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
index 4d6b8e4205cff3723e65132af91a059ab1f55f38..c9efd36f0aa50fc55b084f5a3f050800fccfb451 100644
--- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
@@ -38,6 +38,7 @@
 #include "nav_2d_utils/parameters.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "dwb_core/exceptions.hpp"
+#include "nav2_util/node_utils.hpp"
 
 namespace dwb_plugins
 {
@@ -46,7 +47,7 @@ void LimitedAccelGenerator::initialize(const nav2_util::LifecycleNode::SharedPtr
 {
   StandardTrajectoryGenerator::initialize(nh);
 
-  nh->declare_parameter("sim_period");
+  nav2_util::declare_parameter_if_not_declared(nh, "sim_period");
 
   if (nh->get_parameter("sim_period", acceleration_time_)) {
   } else {
diff --git a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
index 50ff8f03a66255bd554d8b8b507466eaf7b688f7..8e6387eed1f880f609a965ae1611e744b84bc499 100644
--- a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
@@ -36,6 +36,7 @@
 #include <memory>
 #include "pluginlib/class_list_macros.hpp"
 #include "angles/angles.h"
+#include "nav2_util/node_utils.hpp"
 
 namespace dwb_plugins
 {
@@ -47,8 +48,10 @@ SimpleGoalChecker::SimpleGoalChecker()
 
 void SimpleGoalChecker::initialize(const nav2_util::LifecycleNode::SharedPtr & nh)
 {
-  nh->declare_parameter("xy_goal_tolerance", rclcpp::ParameterValue(0.25));
-  nh->declare_parameter("yaw_goal_tolerance", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(nh,
+    "xy_goal_tolerance", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(nh,
+    "yaw_goal_tolerance", rclcpp::ParameterValue(0.25));
 
   nh->get_parameter("xy_goal_tolerance", xy_goal_tolerance_);
   nh->get_parameter("yaw_goal_tolerance", yaw_goal_tolerance_);
diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
index 5fe5eb985bfd59fc883be6cab287103fd71f2da2..46fae624067e8625f971b53766207b4c60248616 100644
--- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
@@ -41,6 +41,7 @@
 #include "nav_2d_utils/parameters.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "dwb_core/exceptions.hpp"
+#include "nav2_util/node_utils.hpp"
 
 using nav_2d_utils::loadParameterWithDeprecation;
 
@@ -53,8 +54,9 @@ void StandardTrajectoryGenerator::initialize(const nav2_util::LifecycleNode::Sha
   kinematics_->initialize(nh);
   initializeIterator(nh);
 
-  nh->declare_parameter("sim_time", rclcpp::ParameterValue(1.7));
-  nh->declare_parameter("discretize_by_time", rclcpp::ParameterValue(false));
+  nav2_util::declare_parameter_if_not_declared(nh, "sim_time", rclcpp::ParameterValue(1.7));
+  nav2_util::declare_parameter_if_not_declared(nh,
+    "discretize_by_time", rclcpp::ParameterValue(false));
 
   nh->get_parameter("sim_time", sim_time_);
   checkUseDwaParam(nh);
diff --git a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
index 21180ea5fe17053fac8d498e0bf02711caedd50a..cf5d29346c95d26b23148d202f182447ab407533 100644
--- a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
@@ -36,6 +36,7 @@
 #include <cmath>
 #include <memory>
 #include "pluginlib/class_list_macros.hpp"
+#include "nav2_util/node_utils.hpp"
 
 using std::fabs;
 
@@ -51,8 +52,10 @@ void StoppedGoalChecker::initialize(const nav2_util::LifecycleNode::SharedPtr &
 {
   SimpleGoalChecker::initialize(nh);
 
-  nh->declare_parameter("rot_stopped_velocity", rclcpp::ParameterValue(0.25));
-  nh->declare_parameter("trans_stopped_velocity", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(nh,
+    "rot_stopped_velocity", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(nh,
+    "trans_stopped_velocity", rclcpp::ParameterValue(0.25));
 
   nh->get_parameter("rot_stopped_velocity", rot_stopped_velocity_);
   nh->get_parameter("trans_stopped_velocity", trans_stopped_velocity_);
diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
index 5c3e2c468854dc35b7acd59c5040a0bbd666dbf4..95ed47471fdbff10e3dbaeb3e66893a268be8df4 100644
--- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
@@ -35,6 +35,7 @@
 #include "dwb_plugins/xy_theta_iterator.hpp"
 #include <memory>
 #include "nav_2d_utils/parameters.hpp"
+#include "nav2_util/node_utils.hpp"
 
 namespace dwb_plugins
 {
@@ -44,8 +45,8 @@ void XYThetaIterator::initialize(
 {
   kinematics_ = kinematics;
 
-  nh->declare_parameter("vx_samples", rclcpp::ParameterValue(20));
-  nh->declare_parameter("vy_samples", rclcpp::ParameterValue(5));
+  nav2_util::declare_parameter_if_not_declared(nh, "vx_samples", rclcpp::ParameterValue(20));
+  nav2_util::declare_parameter_if_not_declared(nh, "vy_samples", rclcpp::ParameterValue(5));
 
   nh->get_parameter("vx_samples", vx_samples_);
   nh->get_parameter("vy_samples", vy_samples_);
diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
index 1d5aa0e25a7691ddcd5ebe71212d444dc00443dd..10f55c835a607d890beba664d1fffe0ab3b9552f 100644
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
+++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
@@ -43,6 +43,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "nav2_util/node_utils.hpp"
 
 namespace nav_2d_utils
 {
@@ -60,22 +61,27 @@ public:
    * @param nh NodeHandle for creating subscriber
    * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
    */
-  explicit OdomSubscriber(nav2_util::LifecycleNode & nh, std::string default_topic = "odom")
+  explicit OdomSubscriber(
+    nav2_util::LifecycleNode::SharedPtr & nh,
+    std::string default_topic = "odom")
   {
     std::string odom_topic;
-    nh.get_parameter_or("odom_topic", odom_topic, default_topic);
+    nh->get_parameter_or("odom_topic", odom_topic, default_topic);
     odom_sub_ =
-      nh.create_subscription<nav_msgs::msg::Odometry>(odom_topic,
+      nh->create_subscription<nav_msgs::msg::Odometry>(odom_topic,
         rclcpp::SystemDefaultsQoS(),
         std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1));
 
-    nh.declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001));
-    nh.declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001));
-    nh.declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001));
+    nav2_util::declare_parameter_if_not_declared(nh,
+      "min_x_velocity_threshold", rclcpp::ParameterValue(0.0001));
+    nav2_util::declare_parameter_if_not_declared(nh,
+      "min_y_velocity_threshold", rclcpp::ParameterValue(0.0001));
+    nav2_util::declare_parameter_if_not_declared(nh,
+      "min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001));
 
-    nh.get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_);
-    nh.get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_);
-    nh.get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
+    nh->get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_);
+    nh->get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_);
+    nh->get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
   }
 
   inline nav_2d_msgs::msg::Twist2D getTwist() {return odom_vel_.velocity;}
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
index 743a5a57fb4c6d553b9d3c246015133d75c72725..89f483d7f7a6542fdc7873e85a4b367a8719fa25 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp
+++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -52,7 +52,7 @@ LifecycleManager::LifecycleManager()
       std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3));
 
   auto options = rclcpp::NodeOptions().arguments(
-    {"--ros-args", std::string("__node:=") + get_name() + "service_client", "--"});
+    {"--ros-args", "-r", std::string("__node:=") + get_name() + "service_client", "--"});
   service_client_node_ = std::make_shared<rclcpp::Node>("_", options);
 
   transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
index 91ec54a746c32f6f020c10002518d0e7fbd84130..c295a1c9ebc5ac6955e7bb1fcb0d4ca0e5b466a5 100644
--- a/nav2_planner/include/nav2_planner/planner_server.hpp
+++ b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -72,7 +72,6 @@ protected:
 
   // TF buffer
   std::shared_ptr<tf2_ros::Buffer> tf_;
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
   // Global Costmap
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 06c90b0f650f5133b15a1ec6d5247208fb4960cd..e74bb4f05f2f2177ad4ff73d37ecb674a36c912a 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -79,7 +79,6 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
     costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
 
   tf_ = costmap_ros_->getTfBuffer();
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
 
   get_parameter("planner_plugin", planner_plugin_name_);
   auto node = shared_from_this();
@@ -91,7 +90,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
     planner_->configure(node,
       gp_loader_.getName(planner_plugin_name_), tf_.get(), costmap_ros_.get());
   } catch (const pluginlib::PluginlibException & ex) {
-    RCLCPP_INFO(get_logger(), "Failed to create global planner. Exception: %s",
+    RCLCPP_FATAL(get_logger(), "Failed to create global planner. Exception: %s",
       ex.what());
     exit(-1);
   }
@@ -139,7 +138,6 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state)
 
   action_server_.reset();
   plan_publisher_.reset();
-  tf_listener_.reset();
   tf_.reset();
   costmap_ros_->on_cleanup(state);
   costmap_ros_.reset();
@@ -233,7 +231,9 @@ PlannerServer::computePathToPose()
     action_server_->terminate_goals();
     return;
   } catch (...) {
-    RCLCPP_WARN(get_logger(), "Plan calculation failed");
+    RCLCPP_WARN(get_logger(), "Plan calculation failed, "
+      "An unexpected error has occurred. The planner server"
+      " may not be able to continue operating correctly.");
 
     // TODO(orduno): provide information about the failure to the parent task,
     //               for example: couldn't get costmap update
diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
index 4269d72828dda72fa10572cfa8c26354979ae1f0..4dc5900b27ada69de7a79ec6d50df54d0f46807c 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
@@ -88,7 +88,7 @@ private:
   QStateMachine state_machine_;
 
   QState * initial_{nullptr};
-  QState * starting_{nullptr};
+  QState * idle_{nullptr};
   QState * stopping_{nullptr};
   // The following states are added to allow for the state of the button to only expose shutdown
   // while the NavigateToPose action is not active. While running, the user will be allowed to
@@ -98,7 +98,6 @@ private:
   // but do nothing upon entrance.
   QState * running_{nullptr};
   QState * canceled_{nullptr};
-  QState * completed_{nullptr};
 };
 
 }  // namespace nav2_rviz_plugins
diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp
index cdf84ce3c264775fde3aa4ca45848ee30d52071c..07e5e33192079f8bac6d3603740df348334fed0e 100644
--- a/nav2_rviz_plugins/src/nav2_panel.cpp
+++ b/nav2_rviz_plugins/src/nav2_panel.cpp
@@ -49,22 +49,15 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   initial_->assignProperty(start_stop_button_, "text", "Startup");
   initial_->assignProperty(start_stop_button_, "toolTip", startup_msg);
 
-  starting_ = new QState();
-  starting_->setObjectName("starting");
-  starting_->assignProperty(start_stop_button_, "text", "Shutdown");
-  starting_->assignProperty(start_stop_button_, "toolTip", shutdown_msg);
+  // State entered when NavigateToPose is not active
+  idle_ = new QState();
+  idle_->setObjectName("idle");
+  idle_->assignProperty(start_stop_button_, "text", "Shutdown");
+  idle_->assignProperty(start_stop_button_, "toolTip", shutdown_msg);
 
   // State entered after NavigateToPose has been canceled
   canceled_ = new QState();
   canceled_->setObjectName("canceled");
-  canceled_->assignProperty(start_stop_button_, "text", "Shutdown");
-  canceled_->assignProperty(start_stop_button_, "toolTip", shutdown_msg);
-
-  // State entered after the NavigateToPose action has completed
-  completed_ = new QState();
-  completed_->setObjectName("succesful");
-  completed_->assignProperty(start_stop_button_, "text", "Shutdown");
-  completed_->assignProperty(start_stop_button_, "toolTip", shutdown_msg);
 
   // State entered while the NavigateToPose action is active
   running_ = new QState();
@@ -72,42 +65,33 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   running_->assignProperty(start_stop_button_, "text", "Cancel");
   running_->assignProperty(start_stop_button_, "toolTip", cancel_msg);
 
+  // State entered when shutdown is requested
   stopping_ = new QState();
   stopping_->setObjectName("stopping");
   stopping_->assignProperty(start_stop_button_, "enabled", false);
 
-  QObject::connect(starting_, SIGNAL(entered()), this, SLOT(onStartup()));
+  QObject::connect(initial_, SIGNAL(exited()), this, SLOT(onStartup()));
   QObject::connect(stopping_, SIGNAL(entered()), this, SLOT(onShutdown()));
-  QObject::connect(canceled_, SIGNAL(entered()), this, SLOT(onCancel()));
+  QObject::connect(canceled_, SIGNAL(exited()), this, SLOT(onCancel()));
 
-  initial_->addTransition(start_stop_button_, SIGNAL(clicked()), starting_);
-  starting_->addTransition(start_stop_button_, SIGNAL(clicked()), stopping_);
+  initial_->addTransition(start_stop_button_, SIGNAL(clicked()), idle_);
+  idle_->addTransition(start_stop_button_, SIGNAL(clicked()), stopping_);
   running_->addTransition(start_stop_button_, SIGNAL(clicked()), canceled_);
-  canceled_->addTransition(start_stop_button_, SIGNAL(clicked()), stopping_);
-  completed_->addTransition(start_stop_button_, SIGNAL(clicked()), stopping_);
-
-  ROSActionQTransition * startupTransition = new ROSActionQTransition(QActionState::INACTIVE);
-  startupTransition->setTargetState(running_);
-  starting_->addTransition(startupTransition);
+  canceled_->addTransition(canceled_, SIGNAL(entered()), idle_);
 
-  ROSActionQTransition * canceledTransition = new ROSActionQTransition(QActionState::INACTIVE);
-  canceledTransition->setTargetState(running_);
-  canceled_->addTransition(canceledTransition);
+  ROSActionQTransition * idleTransition = new ROSActionQTransition(QActionState::INACTIVE);
+  idleTransition->setTargetState(running_);
+  idle_->addTransition(idleTransition);
 
   ROSActionQTransition * runningTransition = new ROSActionQTransition(QActionState::ACTIVE);
-  runningTransition->setTargetState(completed_);
+  runningTransition->setTargetState(idle_);
   running_->addTransition(runningTransition);
 
-  ROSActionQTransition * completedTransition = new ROSActionQTransition(QActionState::INACTIVE);
-  completedTransition->setTargetState(running_);
-  completed_->addTransition(completedTransition);
-
   state_machine_.addState(initial_);
-  state_machine_.addState(starting_);
+  state_machine_.addState(idle_);
   state_machine_.addState(stopping_);
   state_machine_.addState(running_);
   state_machine_.addState(canceled_);
-  state_machine_.addState(completed_);
 
   state_machine_.setInitialState(initial_);
   state_machine_.start();
diff --git a/nav2_system_tests/COLCON_IGNORE b/nav2_system_tests/COLCON_IGNORE
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/nav2_system_tests/src/system/README.md b/nav2_system_tests/src/system/README.md
index b18fd3d993e3d5b76357cf040ac42e3db139e5bc..d78e92846506e2bed092031e3cd8bf9aa403d96f 100644
--- a/nav2_system_tests/src/system/README.md
+++ b/nav2_system_tests/src/system/README.md
@@ -3,11 +3,7 @@
 This is a 'top level' system test which will use Gazebo to simulate a Robot moving from an known initial starting position to a goal pose. 
 
 ## To run the test
-First, remove the COLCON_IGNORE; this exists so the system tests won't be in the debian release package
-```
-rm nav2_system_tests/COLCON_IGNORE
-```
-Then, you must build navigation2 including this package:
+First, you must build navigation2 including this package:
 ```
 colcon build --symlink-install
 ```
diff --git a/nav2_system_tests/src/system/test_system_node.py b/nav2_system_tests/src/system/test_system_node.py
index 35ecc24c206279939d34b1cc1e5cc1574a488cc4..e453169972c784d8e3781348df36dc329fe5a4ab 100755
--- a/nav2_system_tests/src/system/test_system_node.py
+++ b/nav2_system_tests/src/system/test_system_node.py
@@ -23,6 +23,8 @@ from geometry_msgs.msg import PoseWithCovarianceStamped
 from lifecycle_msgs.srv import GetState
 import rclpy
 from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
 
 
 class NavTester(Node):
@@ -30,11 +32,17 @@ class NavTester(Node):
     def __init__(self):
         super().__init__('nav2_tester')
         self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
-                                                      '/initialpose')
-        self.goal_pub = self.create_publisher(PoseStamped, '/goal_pose')
+                                                      '/initialpose', 10)
+        self.goal_pub = self.create_publisher(PoseStamped, '/goal_pose', 10)
+
+        pose_qos = QoSProfile(
+          durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+          reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+          history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+          depth=1)
 
         self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
-                                                       '/amcl_pose', self.poseCallback)
+                                                       '/amcl_pose', self.poseCallback, pose_qos)
         self.initial_pose_received = False
 
     def setInitialPose(self, pose):
diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py
index 7e41b813473a4bdb8043404803f883d3744c2c2a..15e322b39cc0449f5c748111a7bbb16b64d2fcd5 100755
--- a/nav2_system_tests/src/updown/test_updown_launch.py
+++ b/nav2_system_tests/src/updown/test_updown_launch.py
@@ -51,7 +51,7 @@ def generate_launch_description():
     start_gazebo_cmd = launch.actions.ExecuteProcess(
         cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
              world,
-             '--ros-args', ['__params:=', params_file]],
+             '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     start_robot_state_publisher_cmd = launch.actions.ExecuteProcess(
@@ -60,7 +60,7 @@ def generate_launch_description():
                 get_package_prefix('robot_state_publisher'),
                 'lib/robot_state_publisher/robot_state_publisher'),
             urdf,
-            '--ros-args', ['__params:=', params_file]],
+            '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     start_map_server_cmd = launch.actions.ExecuteProcess(
@@ -68,7 +68,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_map_server'),
                 'lib/nav2_map_server/map_server'),
-            '--ros-args', ['__params:=', params_file]],
+            '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     start_localizer_cmd = launch.actions.ExecuteProcess(
@@ -76,7 +76,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_amcl'),
                 'lib/nav2_amcl/amcl'),
-            '--ros-args', ['__params:=', params_file]],
+            '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     start_dwb_cmd = launch.actions.ExecuteProcess(
@@ -84,7 +84,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('dwb_controller'),
                 'lib/dwb_controller/dwb_controller'),
-            '--ros-args', ['__params:=', params_file]],
+            '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     start_planner_cmd = launch.actions.ExecuteProcess(
@@ -92,7 +92,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_planner'),
                 'lib/nav2_planner/planner_server'),
-            '--ros-args', ['__params:=', params_file]],
+            '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     start_navigator_cmd = launch.actions.ExecuteProcess(
@@ -100,7 +100,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_bt_navigator'),
                 'lib/nav2_bt_navigator/bt_navigator'),
-            '--ros-args', ['__params:=', params_file]],
+            '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     start_controller_cmd = launch.actions.ExecuteProcess(
@@ -108,7 +108,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_lifecycle_manager'),
                 'lib/nav2_lifecycle_manager/lifecycle_manager'),
-            '--ros-args', ['__params:=', params_file]],
+            '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     startup_cmd = launch.actions.ExecuteProcess(
@@ -116,7 +116,7 @@ def generate_launch_description():
             os.path.join(
                 get_package_prefix('nav2_system_tests'),
                 'lib/nav2_system_tests/test_updown'),
-            '--ros-args', ['__params:=', params_file]],
+            '--ros-args', '--params-file', params_file],
         cwd=[launch_dir], output='screen')
 
     startup_exit_event_handler = launch.actions.RegisterEventHandler(
diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world
index 05d0a61aeea55638d2f05e555a22aac3ba7c8185..b47308dfad01ab2e455550712e12344dc0db39f0 100644
--- a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world
+++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world
@@ -153,6 +153,8 @@
           <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so">
             <ros>
               <!-- <namespace>/tb3</namespace> -->
+              <argument>--ros-args</argument>
+              <argument>-r</argument>
               <argument>~/out:=imu</argument>
             </ros>
           </plugin>
@@ -221,6 +223,8 @@
             <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
               <ros>
                 <!-- <namespace>/tb3</namespace> -->
+                <argument>--ros-args</argument>
+                <argument>-r </argument>
                 <argument>~/out:=scan</argument>
               </ros>
               <output_type>sensor_msgs/LaserScan</output_type>
@@ -456,6 +460,8 @@
         <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
           <ros>
             <!-- <namespace>/tb3</namespace> -->
+            <argument>--ros-args</argument>
+            <argument>-r </argument>
             <argument>~/out:=joint_states</argument>
           </ros>
           <update_rate>250</update_rate>
diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp
index 5bb76e56267bbcaf7ba190b1003defa780de6051..adc22413f98737f59ded94fb1f7883c64d7b2ff6 100644
--- a/nav2_util/include/nav2_util/node_utils.hpp
+++ b/nav2_util/include/nav2_util/node_utils.hpp
@@ -80,6 +80,19 @@ std::string time_to_string(size_t len);
 rclcpp::NodeOptions
 get_node_options_default(bool allow_undeclared = true, bool declare_initial_params = true);
 
+template<typename NodeT>
+void declare_parameter_if_not_declared(
+  NodeT node,
+  const std::string & param_name,
+  const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+  rcl_interfaces::msg::ParameterDescriptor())
+{
+  if (!node->has_parameter(param_name)) {
+    node->declare_parameter(param_name, default_value, parameter_descriptor);
+  }
+}
+
 }  // namespace nav2_util
 
 #endif  // NAV2_UTIL__NODE_UTILS_HPP_
diff --git a/nav2_util/package.xml b/nav2_util/package.xml
index 67dd3c27127aa7bd3c77f5c98c826d84aea04a1d..66280dc5874e898592a62c1b3ce043b0faf26856 100644
--- a/nav2_util/package.xml
+++ b/nav2_util/package.xml
@@ -10,6 +10,7 @@
   <license>BSD-3-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>libboost-program-options-dev</build_depend>
   <depend>nav2_common</depend>
 
   <depend>geometry_msgs</depend>
@@ -26,6 +27,8 @@
   <depend>test_msgs</depend>
   <depend>rclcpp_lifecycle</depend>
 
+  <exec_depend>libboost-program-options</exec_depend>
+
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>
diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp
index f186ff6ffa0b582be52aefff30b35ad267294d56..2d2c4edb0e791d35c42de41dd8abb659fdcbcf51 100644
--- a/nav2_util/src/lifecycle_node.cpp
+++ b/nav2_util/src/lifecycle_node.cpp
@@ -50,6 +50,7 @@ LifecycleNode::LifecycleNode(
   if (use_rclcpp_node_) {
     std::vector<std::string> new_args = options.arguments();
     new_args.push_back("--ros-args");
+    new_args.push_back("-r");
     new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node");
     new_args.push_back("--");
     rclcpp_node_ = std::make_shared<rclcpp::Node>(
diff --git a/nav2_util/src/node_utils.cpp b/nav2_util/src/node_utils.cpp
index 5344c8b39a4ca7574bee92f2e6bfea97542737c7..c7bf4c4c93d1ee54eefd241660292be5db898454 100644
--- a/nav2_util/src/node_utils.cpp
+++ b/nav2_util/src/node_utils.cpp
@@ -82,7 +82,7 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix)
     rclcpp::NodeOptions()
     .start_parameter_services(false)
     .start_parameter_event_publisher(false)
-    .arguments({"--ros-args", "__node:=" + generate_internal_node_name(prefix), "--"});
+    .arguments({"--ros-args", "-r", "__node:=" + generate_internal_node_name(prefix), "--"});
   return rclcpp::Node::make_shared("_", options);
 }