diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index 5c6b165d59..595ffb7626 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -537,8 +537,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
       global_frame_id_.c_str());
     return;
   }
-  if (abs(msg->pose.pose.position.x) > map_->size_x ||
-    abs(msg->pose.pose.position.y) > map_->size_y)
+  if (first_map_received_ && (abs(msg->pose.pose.position.x) > map_->size_x ||
+    abs(msg->pose.pose.position.y) > map_->size_y))
   {
     RCLCPP_ERROR(
       get_logger(), "Received initialpose from message is out of the size of map. Rejecting.");
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
index bab26bef5a..08c3cc1f7c 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
@@ -19,6 +19,7 @@
 #include <memory>
 #include <string>
 #include <mutex>
+#include <functional>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp b/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp
index 341ad936dd..f12b92883e 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp
@@ -18,6 +18,7 @@
 #include <vector>
 #include <memory>
 #include <string>
+#include <functional>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
@@ -67,11 +68,14 @@ class Navigator
    * May throw exception if fails to navigate or communicate
    * Blocks until completion.
    * @param pose Pose to go to
-   * @param max_staging_duration Maximum time to get to the staging pose
+   * @param remaining_staging_duration Remaining time to get to the staging pose
+   * @param isPreempted Function to check if preempted
+   * @param recursed True if recursed (used to retry once)
    */
   void goToPose(
     const geometry_msgs::msg::PoseStamped & pose,
-    const rclcpp::Duration & max_staging_duration,
+    rclcpp::Duration remaining_staging_duration,
+    std::function<bool()> isPreempted,
     bool recursed = false);
 
 protected:
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
index e1b7c19279..a8bf62a6b2 100644
--- a/nav2_docking/opennav_docking/src/docking_server.cpp
+++ b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -253,8 +253,15 @@ void DockingServer::dockRobot()
     {
       RCLCPP_INFO(get_logger(), "Robot already within pre-staging pose tolerance for dock");
     } else {
+      std::function<bool()> isPreempted = [this]() {
+          return checkAndWarnIfCancelled(docking_action_server_, "dock_robot") ||
+                 checkAndWarnIfPreempted(docking_action_server_, "dock_robot");
+        };
+
       navigator_->goToPose(
-        initial_staging_pose, rclcpp::Duration::from_seconds(goal->max_staging_time));
+        initial_staging_pose,
+        rclcpp::Duration::from_seconds(goal->max_staging_time),
+        isPreempted);
       RCLCPP_INFO(get_logger(), "Successful navigation to staging pose");
     }
 
diff --git a/nav2_docking/opennav_docking/src/navigator.cpp b/nav2_docking/opennav_docking/src/navigator.cpp
index aae3ff9709..e792411a75 100644
--- a/nav2_docking/opennav_docking/src/navigator.cpp
+++ b/nav2_docking/opennav_docking/src/navigator.cpp
@@ -51,13 +51,16 @@ void Navigator::deactivate()
 
 void Navigator::goToPose(
   const geometry_msgs::msg::PoseStamped & pose,
-  const rclcpp::Duration & max_staging_duration,
+  rclcpp::Duration remaining_staging_duration,
+  std::function<bool()> isPreempted,
   bool recursed)
 {
+  auto node = node_.lock();
+
   Nav2Pose::Goal goal;
   goal.pose = pose;
   goal.behavior_tree = navigator_bt_xml_;
-  const auto timeout = max_staging_duration.to_chrono<std::chrono::milliseconds>();
+  const auto start_time = node->now();
 
   // Wait for server to be active
   nav_to_pose_client_->wait_for_action_server(1s);
@@ -66,19 +69,41 @@ void Navigator::goToPose(
       future_goal_handle, 2s) == rclcpp::FutureReturnCode::SUCCESS)
   {
     auto future_result = nav_to_pose_client_->async_get_result(future_goal_handle.get());
-    if (executor_.spin_until_future_complete(
-        future_result, timeout) == rclcpp::FutureReturnCode::SUCCESS)
-    {
-      auto result = future_result.get();
-      if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->error_code == 0) {
-        return;  // Success!
+
+    while (rclcpp::ok()) {
+      if (isPreempted()) {
+        auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get());
+        executor_.spin_until_future_complete(cancel_future, 1s);
+        throw opennav_docking_core::FailedToStage("Navigation request to staging pose preempted.");
+      }
+
+      if (node->now() - start_time > remaining_staging_duration) {
+        auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get());
+        executor_.spin_until_future_complete(cancel_future, 1s);
+        throw opennav_docking_core::FailedToStage("Navigation request to staging pose timed out.");
+      }
+
+      if (executor_.spin_until_future_complete(
+          future_result, 10ms) == rclcpp::FutureReturnCode::SUCCESS)
+      {
+        auto result = future_result.get();
+        if (result.code == rclcpp_action::ResultCode::SUCCEEDED &&
+          result.result->error_code == 0)
+        {
+          return;  // Success!
+        } else {
+          RCLCPP_WARN(node->get_logger(), "Navigation request to staging pose failed.");
+          break;
+        }
       }
     }
   }
 
   // Attempt to retry once using single iteration recursion
   if (!recursed) {
-    goToPose(pose, max_staging_duration, true);
+    auto elapsed_time = node->now() - start_time;
+    remaining_staging_duration = remaining_staging_duration - elapsed_time;
+    goToPose(pose, remaining_staging_duration, isPreempted, true);
     return;
   }
 
diff --git a/nav2_docking/opennav_docking/test/test_navigator.cpp b/nav2_docking/opennav_docking/test/test_navigator.cpp
index 80766f0782..8d81dec81b 100644
--- a/nav2_docking/opennav_docking/test/test_navigator.cpp
+++ b/nav2_docking/opennav_docking/test/test_navigator.cpp
@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <functional>
+
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/simple_action_server.hpp"
@@ -94,26 +96,40 @@ TEST(NavigatorTests, TestNavigator)
   auto navigator = std::make_unique<Navigator>(node);
   navigator->activate();
 
+  std::function<bool()> is_preempted_true = []() {return true;};
+  std::function<bool()> is_preempted_false = []() {return false;};
+
   // Should succeed, action server set to succeed
   dummy_navigator_node->setReturn(true);
   EXPECT_NO_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)));
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+      is_preempted_false));
 
   // Should fail, timeout exceeded
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0)),
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0),
+      is_preempted_false),
     opennav_docking_core::FailedToStage);
 
   // Should fail, action server set to succeed
   dummy_navigator_node->setReturn(false);
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)),
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+      is_preempted_false),
     opennav_docking_core::FailedToStage);
 
   // First should fail, recursion should succeed
   dummy_navigator_node->setToggle();
   EXPECT_NO_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)));
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+      is_preempted_false));
+
+  // Should fail, preempted
+  dummy_navigator_node->setReturn(true);
+  EXPECT_THROW(
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+      is_preempted_true),
+    opennav_docking_core::FailedToStage);
 
   navigator->deactivate();
   navigator.reset();
diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt
index 197150837c..a5bce30e77 100644
--- a/nav2_planner/CMakeLists.txt
+++ b/nav2_planner/CMakeLists.txt
@@ -2,71 +2,62 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_planner)
 
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(visualization_msgs REQUIRED)
-find_package(nav2_util REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
 find_package(nav_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(builtin_interfaces REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(nav2_costmap_2d REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(nav2_core REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
 
-include_directories(
-  include
-)
-
 set(executable_name planner_server)
 set(library_name ${executable_name}_core)
 
-set(dependencies
-  rclcpp
-  rclcpp_action
-  rclcpp_lifecycle
-  rclcpp_components
-  std_msgs
-  visualization_msgs
-  nav2_util
-  nav2_msgs
-  nav_msgs
-  geometry_msgs
-  builtin_interfaces
-  tf2_ros
-  nav2_costmap_2d
-  pluginlib
-  nav2_core
-)
-
 add_library(${library_name} SHARED
   src/planner_server.cpp
 )
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
+target_include_directories(${library_name}
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${rcl_interfaces_TARGETS}
+  tf2_ros::tf2_ros
+)
+target_link_libraries(${library_name} PRIVATE
+  ${lifecycle_msgs_TARGETS}
+  rclcpp_components::component
+  tf2::tf2
 )
 
 add_executable(${executable_name}
   src/main.cpp
 )
-
-target_link_libraries(${executable_name} ${library_name})
-
-ament_target_dependencies(${executable_name}
-  ${dependencies}
-)
+target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp)
 
 rclcpp_components_register_nodes(${library_name} "nav2_planner::PlannerServer")
 
 install(TARGETS ${library_name}
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -77,17 +68,31 @@ install(TARGETS ${executable_name}
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  ament_find_gtest()
   add_subdirectory(test)
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_libraries(${library_name})
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  geometry_msgs
+  nav2_core
+  nav2_costmap_2d
+  nav2_msgs
+  nav2_util
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  tf2_ros
+)
+ament_export_targets(${library_name})
 ament_package()
diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
index bdffda281a..12e364c710 100644
--- a/nav2_planner/include/nav2_planner/planner_server.hpp
+++ b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -31,7 +31,6 @@
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/simple_action_server.hpp"
-#include "visualization_msgs/msg/marker.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml
index 397dd8ae1d..26de6cfa13 100644
--- a/nav2_planner/package.xml
+++ b/nav2_planner/package.xml
@@ -8,22 +8,23 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
 
-  <depend>rclcpp</depend>
-  <depend>rclcpp_action</depend>
-  <depend>rclcpp_lifecycle</depend>
-  <depend>visualization_msgs</depend>
-  <depend>nav2_util</depend>
-  <depend>nav2_msgs</depend>
-  <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>builtin_interfaces</depend>
-  <depend>nav2_common</depend>
-  <depend>tf2_ros</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
-  <depend>nav2_core</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
 
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 6abeddd29b..d572360fc9 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -24,7 +24,6 @@
 #include <vector>
 #include <utility>
 
-#include "builtin_interfaces/msg/duration.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "nav2_util/costmap.hpp"
 #include "nav2_util/node_utils.hpp"
diff --git a/nav2_planner/test/CMakeLists.txt b/nav2_planner/test/CMakeLists.txt
index d415d906ef..a551d1208f 100644
--- a/nav2_planner/test/CMakeLists.txt
+++ b/nav2_planner/test/CMakeLists.txt
@@ -2,9 +2,9 @@
 ament_add_gtest(test_dynamic_parameters
   test_dynamic_parameters.cpp
 )
-ament_target_dependencies(test_dynamic_parameters
-  ${dependencies}
-)
 target_link_libraries(test_dynamic_parameters
   ${library_name}
+  nav2_util::nav2_util_core
+  rclcpp::rclcpp
+  ${rcl_interfaces_TARGETS}
 )
diff --git a/nav2_planner/test/test_dynamic_parameters.cpp b/nav2_planner/test/test_dynamic_parameters.cpp
index 8215e80e43..3dc3ab2b55 100644
--- a/nav2_planner/test/test_dynamic_parameters.cpp
+++ b/nav2_planner/test/test_dynamic_parameters.cpp
@@ -12,8 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#include <math.h>
-
+#include <cmath>
 #include <memory>
 #include <string>
 #include <vector>
@@ -22,6 +21,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_planner/planner_server.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 class PlannerShim : public nav2_planner::PlannerServer
 {
diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
index e0340a131c..91364c36bc 100644
--- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
+++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
@@ -3,57 +3,61 @@ project(nav2_regulated_pure_pursuit_controller)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_util REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
-set(CMAKE_CXX_STANDARD 17)
-
-include_directories(
-  include
-)
-
-set(dependencies
-  rclcpp
-  geometry_msgs
-  nav2_costmap_2d
-  pluginlib
-  nav_msgs
-  angles
-  nav2_util
-  nav2_core
-  tf2
-  tf2_geometry_msgs
-)
 
 set(library_name nav2_regulated_pure_pursuit_controller)
 
 add_library(${library_name} SHARED
-        src/regulated_pure_pursuit_controller.cpp
-        src/collision_checker.cpp
-        src/parameter_handler.cpp
-        src/path_handler.cpp)
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
+  src/regulated_pure_pursuit_controller.cpp
+  src/collision_checker.cpp
+  src/parameter_handler.cpp
+  src/path_handler.cpp)
+target_include_directories(${library_name}
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${rcl_interfaces_TARGETS}
+  ${std_msgs_TARGETS}
+  tf2::tf2
+  tf2_ros::tf2_ros
+)
+target_link_libraries(${library_name} PRIVATE
+  angles::angles
 )
 
 install(TARGETS ${library_name}
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
@@ -61,14 +65,31 @@ if(BUILD_TESTING)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
+  find_package(tf2_geometry_msgs REQUIRED)
+
+  ament_find_gtest()
   add_subdirectory(test)
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_libraries(${library_name})
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  geometry_msgs
+  nav2_core
+  nav2_costmap_2d
+  nav2_util
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  rcl_interfaces
+  std_msgs
+  tf2
+  tf2_ros
+)
+ament_export_targets(${library_name})
 
 pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml)
 
 ament_package()
-
diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml
index e690351e19..ff4105b62e 100644
--- a/nav2_regulated_pure_pursuit_controller/package.xml
+++ b/nav2_regulated_pure_pursuit_controller/package.xml
@@ -9,22 +9,26 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
 
   <depend>angles</depend>
-  <depend>nav2_common</depend>
+  <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
-  <depend>nav2_util</depend>
   <depend>nav2_costmap_2d</depend>
-  <depend>rclcpp</depend>
-  <depend>geometry_msgs</depend>
   <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
   <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>std_msgs</depend>
   <depend>tf2</depend>
-  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>tf2_geometry_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt
index 3e8d4c5b0a..81ccfc4d28 100644
--- a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt
+++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt
@@ -3,13 +3,22 @@ ament_add_gtest(test_regulated_pp
   test_regulated_pp.cpp
   path_utils/path_utils.cpp
 )
-ament_target_dependencies(test_regulated_pp
-  ${dependencies}
-)
 target_link_libraries(test_regulated_pp
   ${library_name}
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
 )
 
 # Path utils test
 ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp)
-ament_target_dependencies(test_path_utils nav_msgs geometry_msgs tf2_geometry_msgs)
+target_link_libraries(test_path_utils
+  ${nav_msgs_TARGETS}
+  ${geometry_msgs_TARGETS}
+  tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
+)
diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt
index 2e141d5f44..fafa98863c 100644
--- a/nav2_rotation_shim_controller/CMakeLists.txt
+++ b/nav2_rotation_shim_controller/CMakeLists.txt
@@ -2,53 +2,56 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_rotation_shim_controller)
 
 find_package(ament_cmake REQUIRED)
+find_package(angles REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_util REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 find_package(tf2 REQUIRED)
-find_package(angles REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
-set(CMAKE_CXX_STANDARD 17)
-
-include_directories(
-  include
-)
-
-set(dependencies
-  rclcpp
-  geometry_msgs
-  nav2_costmap_2d
-  pluginlib
-  nav_msgs
-  nav2_util
-  nav2_core
-  tf2
-  angles
-)
 
 set(library_name nav2_rotation_shim_controller)
 
 add_library(${library_name} SHARED
-        src/nav2_rotation_shim_controller.cpp)
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
+  src/nav2_rotation_shim_controller.cpp)
+target_include_directories(${library_name}
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${rcl_interfaces_TARGETS}
+  tf2_ros::tf2_ros
+)
+target_link_libraries(${library_name} PRIVATE
+  angles::angles
+  nav2_util::nav2_util_core
+  tf2::tf2
 )
 
 install(TARGETS ${library_name}
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
@@ -56,12 +59,24 @@ if(BUILD_TESTING)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
+  ament_find_gtest()
   add_subdirectory(test)
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_libraries(${library_name})
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  geometry_msgs
+  nav2_core
+  nav2_costmap_2d
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  rcl_interfaces
+  tf2_ros
+)
+ament_export_targets(${library_name})
 
 pluginlib_export_plugin_description_file(nav2_core nav2_rotation_shim_controller.xml)
 
diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
index 6d4e8b86b3..f0da5e0fba 100644
--- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
+++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
@@ -24,13 +24,9 @@
 #include "rclcpp/rclcpp.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
-#include "nav2_util/geometry_utils.hpp"
-#include "nav2_util/robot_utils.hpp"
 #include "nav2_core/controller.hpp"
 #include "nav2_core/controller_exceptions.hpp"
-#include "nav2_util/node_utils.hpp"
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
-#include "angles/angles.h"
 
 namespace nav2_rotation_shim_controller
 {
diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml
index 1fadbbe2c5..9e450f7573 100644
--- a/nav2_rotation_shim_controller/package.xml
+++ b/nav2_rotation_shim_controller/package.xml
@@ -8,17 +8,20 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
 
-  <depend>nav2_common</depend>
+  <depend>angles</depend>
+  <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
-  <depend>nav2_util</depend>
   <depend>nav2_costmap_2d</depend>
-  <depend>rclcpp</depend>
-  <depend>geometry_msgs</depend>
   <depend>nav2_msgs</depend>
-  <depend>angles</depend>
+  <depend>nav2_util</depend>
   <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rcl_interfaces</depend>
   <depend>tf2</depend>
+  <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
index afb74c357e..4e111edfda 100644
--- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
+++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
@@ -18,6 +18,11 @@
 #include <vector>
 #include <utility>
 
+#include "angles/angles.h"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+
 #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
 #include "nav2_rotation_shim_controller/tools/utils.hpp"
 
diff --git a/nav2_rotation_shim_controller/test/CMakeLists.txt b/nav2_rotation_shim_controller/test/CMakeLists.txt
index ae3161d1a7..e2d79806a3 100644
--- a/nav2_rotation_shim_controller/test/CMakeLists.txt
+++ b/nav2_rotation_shim_controller/test/CMakeLists.txt
@@ -4,10 +4,11 @@ find_package(nav2_controller REQUIRED)
 ament_add_gtest(test_shim_controller
   test_shim_controller.cpp
 )
-ament_target_dependencies(test_shim_controller
-  ${dependencies}
-  nav2_controller
-)
 target_link_libraries(test_shim_controller
   ${library_name}
+  rclcpp::rclcpp
+  nav2_costmap_2d::nav2_costmap_2d_core
+  nav2_controller::simple_goal_checker
+  nav2_util::nav2_util_core
+  tf2_ros::tf2_ros
 )
diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
index c5f5c788aa..bd9059d698 100644
--- a/nav2_rviz_plugins/CMakeLists.txt
+++ b/nav2_rviz_plugins/CMakeLists.txt
@@ -1,29 +1,17 @@
 cmake_minimum_required(VERSION 3.5)
 project(nav2_rviz_plugins)
 
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
-endif()
-
-# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html
-set(CMAKE_INCLUDE_CURRENT_DIR ON)
-set(CMAKE_AUTOMOC ON)
-
 find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package(nav2_util REQUIRED)
+find_package(nav2_common REQUIRED)
 find_package(nav2_lifecycle_manager REQUIRED)
 find_package(nav2_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rviz_common REQUIRED)
 find_package(rviz_default_plugins REQUIRED)
 find_package(rviz_ogre_vendor REQUIRED)
@@ -32,23 +20,25 @@ find_package(std_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+nav2_package()
 
+# We specifically don't turn on CMAKE_AUTOMOC, since it generates one huge
+# mocs_compilation.cpp file that takes a lot of memory to compile.  Instead
+# we create individual moc files that can be compiled separately.
 set(nav2_rviz_plugins_headers_to_moc
   include/nav2_rviz_plugins/costmap_cost_tool.hpp
   include/nav2_rviz_plugins/docking_panel.hpp
   include/nav2_rviz_plugins/goal_pose_updater.hpp
-  include/nav2_rviz_plugins/goal_common.hpp
   include/nav2_rviz_plugins/goal_tool.hpp
   include/nav2_rviz_plugins/nav2_panel.hpp
   include/nav2_rviz_plugins/selector.hpp
-  include/nav2_rviz_plugins/utils.hpp
-  include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp
   include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp
 )
-
-include_directories(
-  include
-)
+foreach(header "${nav2_rviz_plugins_headers_to_moc}")
+  qt5_wrap_cpp(nav2_rviz_plugins_moc_files "${header}")
+endforeach()
 
 set(library_name ${PROJECT_NAME})
 
@@ -61,45 +51,33 @@ add_library(${library_name} SHARED
   src/utils.cpp
   src/particle_cloud_display/flat_weighted_arrows_array.cpp
   src/particle_cloud_display/particle_cloud_display.cpp
-  ${nav2_rviz_plugins_headers_to_moc}
-)
-
-set(dependencies
-  geometry_msgs
-  nav2_util
-  nav2_lifecycle_manager
-  nav2_msgs
-  nav_msgs
-  pluginlib
-  Qt5
-  rclcpp
-  rclcpp_lifecycle
-  rviz_common
-  rviz_default_plugins
-  rviz_ogre_vendor
-  rviz_rendering
-  std_msgs
-  tf2_geometry_msgs
-  yaml_cpp_vendor
-)
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
+  ${nav2_rviz_plugins_moc_files}
 )
-
 target_include_directories(${library_name} PUBLIC
   ${Qt5Widgets_INCLUDE_DIRS}
   ${OGRE_INCLUDE_DIRS}
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
-
-target_link_libraries(${library_name}
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  nav2_lifecycle_manager::nav2_lifecycle_manager_core
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  rclcpp::rclcpp
+  rclcpp_action::rclcpp_action
   rviz_common::rviz_common
+  rviz_default_plugins::rviz_default_plugins
+  rviz_rendering::rviz_rendering
+  ${std_msgs_TARGETS}
+  tf2_geometry_msgs::tf2_geometry_msgs
+  ${visualization_msgs_TARGETS}
+)
+target_link_libraries(${library_name} PRIVATE
+  ament_index_cpp::ament_index_cpp
+  pluginlib::pluginlib
+  yaml-cpp::yaml-cpp
 )
-
-# Causes the visibility macros to use dllexport rather than dllimport,
-# which is appropriate when building the dll but not consuming it.
-# TODO: Make this specific to this project (not rviz default plugins)
-target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
 
 pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
 
@@ -109,12 +87,11 @@ install(
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
 )
 
 install(
   DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 install(
@@ -127,15 +104,22 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_targets(${library_name} HAS_LIBRARY_TARGET)
 ament_export_dependencies(
-  Qt5
-  rviz_common
   geometry_msgs
-  map_msgs
-  nav_msgs
+  nav2_lifecycle_manager
+  nav2_msgs
+  nav2_util
+  Qt5
   rclcpp
+  rclcpp_action
+  rviz_common
+  rviz_default_plugins
+  rviz_rendering
+  std_msgs
+  tf2_geometry_msgs
+  visualization_msgs
 )
 
 ament_package()
diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
index 3466e8831b..a14a9f98b5 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
@@ -15,7 +15,10 @@
 #ifndef NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
 #define NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
 
+#include <memory>
+
 #include <nav2_msgs/srv/get_costs.hpp>
+#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
 #include <rviz_common/tool.hpp>
 #include <rviz_default_plugins/tools/point/point_tool.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -47,7 +50,8 @@ private Q_SLOTS:
 private:
   rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedPtr local_cost_client_;
   rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedPtr global_cost_client_;
-  rclcpp::Node::SharedPtr node_;
+  // The Node pointer that we need to keep alive for the duration of this plugin.
+  std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
 
   QCursor std_cursor_;
   QCursor hit_cursor_;
diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp
index 68380cb43e..868c644e4b 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp
@@ -19,12 +19,14 @@
 #include <QtWidgets>
 #include <QBasicTimer>
 
+#include <memory>
 #include <string>
 
 // ROS
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rviz_common/panel.hpp"
+#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "nav2_msgs/action/dock_robot.hpp"
 #include "nav2_msgs/action/undock_robot.hpp"
@@ -125,6 +127,9 @@ private Q_SLOTS:
   DockGoalHandle::SharedPtr dock_goal_handle_;
   UndockGoalHandle::SharedPtr undock_goal_handle_;
 
+  // The Node pointer that we need to keep alive for the duration of this plugin.
+  std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
+
   // Docking / Undocking action feedback subscribers
   rclcpp::Subscription<Dock::Impl::FeedbackMessage>::SharedPtr docking_feedback_sub_;
   rclcpp::Subscription<Undock::Impl::FeedbackMessage>::SharedPtr undocking_feedback_sub_;
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 ce4212a4b8..471dce5ce5 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
@@ -31,6 +31,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rviz_common/panel.hpp"
+#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -92,6 +93,9 @@ private Q_SLOTS:
   std::string loop_no_ = "0";
   std::string base_frame_;
 
+  // The Node pointer that we need to keep alive for the duration of this plugin.
+  std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
+
   // Call to send NavigateToPose action request for goal poses
   geometry_msgs::msg::PoseStamped convert_to_msg(
     std::vector<double> pose,
diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml
index 90c4bdfa4d..c90944df77 100644
--- a/nav2_rviz_plugins/package.xml
+++ b/nav2_rviz_plugins/package.xml
@@ -8,24 +8,23 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
   <build_depend>qtbase5-dev</build_depend>
 
+  <depend>ament_index_cpp</depend>
   <depend>geometry_msgs</depend>
-  <depend>nav2_util</depend>
   <depend>nav2_lifecycle_manager</depend>
   <depend>nav2_msgs</depend>
-  <depend>nav_msgs</depend>
+  <depend>nav2_util</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
-  <depend>rclcpp_lifecycle</depend>
-  <depend>resource_retriever</depend>
+  <depend>rclcpp_action</depend>
   <depend>rviz_common</depend>
   <depend>rviz_default_plugins</depend>
   <depend>rviz_ogre_vendor</depend>
   <depend>rviz_rendering</depend>
   <depend>std_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>urdf</depend>
   <depend>visualization_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
 
diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp
index 7f1fa84daf..d5ef565874 100644
--- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp
+++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp
@@ -43,11 +43,20 @@ void CostmapCostTool::onInitialize()
   setName("Costmap Cost");
   setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/PointStamped.png"));
 
-  node_ = context_->getRosNodeAbstraction().lock()->get_raw_node();
+  node_ptr_ = context_->getRosNodeAbstraction().lock();
+  if (node_ptr_ == nullptr) {
+    // The node no longer exists, so just don't initialize
+    RCLCPP_ERROR(
+      rclcpp::get_logger("costmap_cost_tool"),
+      "Underlying ROS node no longer exists, initialization failed");
+    return;
+  }
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
+
   local_cost_client_ =
-    node_->create_client<nav2_msgs::srv::GetCosts>("/local_costmap/get_cost_local_costmap");
+    node->create_client<nav2_msgs::srv::GetCosts>("/local_costmap/get_cost_local_costmap");
   global_cost_client_ =
-    node_->create_client<nav2_msgs::srv::GetCosts>("/global_costmap/get_cost_global_costmap");
+    node->create_client<nav2_msgs::srv::GetCosts>("/global_costmap/get_cost_global_costmap");
 }
 
 void CostmapCostTool::activate() {}
@@ -109,22 +118,24 @@ void CostmapCostTool::callCostService(float x, float y)
 void CostmapCostTool::handleLocalCostResponse(
   rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedFuture future)
 {
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
   if (response->costs[0] != -1) {
-    RCLCPP_INFO(node_->get_logger(), "Local costmap cost: %.1f", response->costs[0]);
+    RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]);
   } else {
-    RCLCPP_ERROR(node_->get_logger(), "Failed to get local costmap cost");
+    RCLCPP_ERROR(node->get_logger(), "Failed to get local costmap cost");
   }
 }
 
 void CostmapCostTool::handleGlobalCostResponse(
   rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedFuture future)
 {
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
   if (response->costs[0] != -1) {
-    RCLCPP_INFO(node_->get_logger(), "Global costmap cost: %.1f", response->costs[0]);
+    RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]);
   } else {
-    RCLCPP_ERROR(node_->get_logger(), "Failed to get global costmap cost");
+    RCLCPP_ERROR(node->get_logger(), "Failed to get global costmap cost");
   }
 }
 }  // namespace nav2_rviz_plugins
diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp
index 909e9a74fd..7daa327079 100644
--- a/nav2_rviz_plugins/src/docking_panel.cpp
+++ b/nav2_rviz_plugins/src/docking_panel.cpp
@@ -12,14 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// C++
-#include <stdio.h>
-
 // QT
 #include <QLineEdit>
 #include <QVBoxLayout>
 #include <QHBoxLayout>
 #include <QLabel>
+
+// C++
+#include <chrono>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
 #include <rviz_common/display_context.hpp>
 
 #include "nav2_util/geometry_utils.hpp"
@@ -114,7 +119,15 @@ DockingPanel::DockingPanel(QWidget * parent)
 
 void DockingPanel::onInitialize()
 {
-  auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  node_ptr_ = getDisplayContext()->getRosNodeAbstraction().lock();
+  if (node_ptr_ == nullptr) {
+    // The node no longer exists, so just don't initialize
+    RCLCPP_ERROR(
+      rclcpp::get_logger("docking_panel"),
+      "Underlying ROS node no longer exists, initialization failed");
+    return;
+  }
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
 
   // Create action feedback subscriber
   docking_feedback_sub_ = node->create_subscription<Dock::Impl::FeedbackMessage>(
diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp
index a3742685f3..e10f1eccdb 100644
--- a/nav2_rviz_plugins/src/nav2_panel.cpp
+++ b/nav2_rviz_plugins/src/nav2_panel.cpp
@@ -29,6 +29,7 @@
 
 #include "nav2_rviz_plugins/goal_common.hpp"
 #include "nav2_rviz_plugins/utils.hpp"
+#include "rclcpp/rclcpp.hpp"
 #include "rviz_common/display_context.hpp"
 #include "rviz_common/load_resource.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
@@ -777,7 +778,15 @@ void Nav2Panel::handleGoalSaver()
 void
 Nav2Panel::onInitialize()
 {
-  auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  node_ptr_ = getDisplayContext()->getRosNodeAbstraction().lock();
+  if (node_ptr_ == nullptr) {
+    // The node no longer exists, so just don't initialize
+    RCLCPP_ERROR(
+      rclcpp::get_logger("nav2_panel"),
+      "Underlying ROS node no longer exists, initialization failed");
+    return;
+  }
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
 
   // declaring parameter to get the base frame
   node->declare_parameter("base_frame", rclcpp::ParameterValue(std::string("base_footprint")));
diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt
index 0ab7c83628..7e93a035c0 100644
--- a/nav2_smoother/CMakeLists.txt
+++ b/nav2_smoother/CMakeLists.txt
@@ -2,79 +2,108 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_smoother)
 
 find_package(ament_cmake REQUIRED)
-find_package(nav2_core REQUIRED)
-find_package(nav2_common REQUIRED)
 find_package(angles REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(nav2_util REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
 find_package(nav_2d_utils REQUIRED)
-find_package(nav_2d_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
-
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
 
-include_directories(
-  include
-)
-
 set(executable_name smoother_server)
 set(library_name ${executable_name}_core)
 
-set(dependencies
-  angles
-  rclcpp
-  rclcpp_components
-  rclcpp_action
-  rclcpp_components
-  std_msgs
-  nav2_msgs
-  nav_2d_utils
-  nav_2d_msgs
-  nav2_util
-  nav2_core
-  pluginlib
-)
-
 # Main library
 add_library(${library_name} SHARED
   src/nav2_smoother.cpp
 )
-ament_target_dependencies(${library_name}
-  ${dependencies}
+target_include_directories(${library_name}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${library_name} PUBLIC
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_client
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  tf2_ros::tf2_ros
+)
+target_link_libraries(${library_name} PRIVATE
+  nav_2d_utils::conversions
+  nav_2d_utils::tf_help
+  rclcpp_components::component
+  tf2::tf2
 )
 
 # Main executable
 add_executable(${executable_name}
   src/main.cpp
 )
-ament_target_dependencies(${executable_name}
-  ${dependencies}
-)
-target_link_libraries(${executable_name} ${library_name})
+target_include_directories(${executable_name}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${executable_name} PRIVATE rclcpp::rclcpp ${library_name})
 
 # Simple Smoother plugin
 add_library(simple_smoother SHARED
   src/simple_smoother.cpp
 )
-ament_target_dependencies(simple_smoother
-  ${dependencies}
+target_include_directories(simple_smoother
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(simple_smoother PUBLIC
+  angles::angles
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  nav2_costmap_2d::nav2_costmap_2d_client
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  tf2::tf2
+  tf2_ros::tf2_ros
+)
+target_link_libraries(simple_smoother PRIVATE
+  pluginlib::pluginlib
 )
 
 # Savitzky Golay Smoother plugin
 add_library(savitzky_golay_smoother SHARED
   src/savitzky_golay_smoother.cpp
 )
-ament_target_dependencies(savitzky_golay_smoother
-  ${dependencies}
+target_include_directories(savitzky_golay_smoother
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(savitzky_golay_smoother PUBLIC
+  angles::angles
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  nav2_costmap_2d::nav2_costmap_2d_client
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  tf2::tf2
+  tf2_ros::tf2_ros
+)
+target_link_libraries(savitzky_golay_smoother PRIVATE
+  pluginlib::pluginlib
 )
 
 pluginlib_export_plugin_description_file(nav2_core plugins.xml)
@@ -87,12 +116,18 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_index_cpp REQUIRED)
+  find_package(rclcpp_action REQUIRED)
+
+  ament_find_gtest()
+  add_subdirectory(test)
 endif()
 
 rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer")
 
 install(
   TARGETS ${library_name} simple_smoother savitzky_golay_smoother
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -103,17 +138,22 @@ install(TARGETS ${executable_name}
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-  find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
-endif()
-
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_libraries(${library_name} simple_smoother savitzky_golay_smoother)
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  angles
+  nav2_core
+  nav2_costmap_2d
+  nav2_msgs
+  nav2_util
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  tf2
+  tf2_ros
+)
+ament_export_targets(${library_name})
 ament_package()
diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml
index e4f7e5eca3..1e85b02a42 100644
--- a/nav2_smoother/package.xml
+++ b/nav2_smoother/package.xml
@@ -10,22 +10,25 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
+
   <depend>angles</depend>
-  <depend>rclcpp</depend>
-  <depend>rclcpp_components</depend>
-  <depend>rclcpp_action</depend>
-  <depend>std_msgs</depend>
-  <depend>nav2_util</depend>
+  <depend>nav2_core</depend>
+  <depend>nav2_costmap_2d</depend>
   <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
   <depend>nav_2d_utils</depend>
-  <depend>nav_2d_msgs</depend>
-  <depend>nav2_core</depend>
   <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_index_cpp</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>rclcpp_action</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/nav2_smoother/test/CMakeLists.txt b/nav2_smoother/test/CMakeLists.txt
index 623447b5f9..ce361ad9a4 100644
--- a/nav2_smoother/test/CMakeLists.txt
+++ b/nav2_smoother/test/CMakeLists.txt
@@ -1,36 +1,48 @@
 ament_add_gtest(test_smoother_server
   test_smoother_server.cpp
 )
-
 target_link_libraries(test_smoother_server
   ${library_name}
-)
-
-ament_target_dependencies(test_smoother_server
-  ${dependencies}
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_client
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_action::rclcpp_action
+  rclcpp_lifecycle::rclcpp_lifecycle
 )
 
 ament_add_gtest(test_simple_smoother
   test_simple_smoother.cpp
 )
-
 target_link_libraries(test_simple_smoother
   simple_smoother
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_client
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  tf2::tf2
 )
 
-ament_target_dependencies(test_simple_smoother
-  ${dependencies}
-)
-
-
 ament_add_gtest(test_savitzky_golay_smoother
   test_savitzky_golay_smoother.cpp
 )
-
 target_link_libraries(test_savitzky_golay_smoother
   savitzky_golay_smoother
-)
-
-ament_target_dependencies(test_savitzky_golay_smoother
-  ${dependencies}
+  ament_index_cpp::ament_index_cpp
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_client
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  tf2_ros::tf2_ros
 )
diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt
index 8cf8a28c35..de6778fe2b 100644
--- a/nav2_theta_star_planner/CMakeLists.txt
+++ b/nav2_theta_star_planner/CMakeLists.txt
@@ -2,80 +2,94 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_theta_star_planner)
 
 find_package(ament_cmake REQUIRED)
-find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
-find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 find_package(tf2_ros REQUIRED)
 
-nav2_package() #Calls the nav2_package.cmake file
-add_compile_options(-O3)
-
-include_directories(
-  include
-)
+nav2_package()
 
 set(library_name ${PROJECT_NAME})
 
-set(dependencies ament_cmake
-  builtin_interfaces
-  nav2_common
-  nav2_core
-  nav2_costmap_2d
-  nav2_msgs
-  nav2_util
-  pluginlib
-  rclcpp
-  rclcpp_action
-  rclcpp_lifecycle
-  tf2_ros
-)
-
-
 add_library(${library_name} SHARED
   src/theta_star.cpp
   src/theta_star_planner.cpp
 )
-
-ament_target_dependencies(${library_name} ${dependencies})
-
-target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS")
+target_include_directories(${library_name}
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${rcl_interfaces_TARGETS}
+  tf2_ros::tf2_ros
+)
+target_link_libraries(${library_name} PRIVATE
+  pluginlib::pluginlib
+)
+target_compile_options(${library_name} PRIVATE -O3)
 
 pluginlib_export_plugin_description_file(nav2_core theta_star_planner.xml)
 
 install(TARGETS ${library_name}
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 install(FILES theta_star_planner.xml
   DESTINATION share/${PROJECT_NAME}
 )
 
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(gtest_disable_pthreads OFF)
   ament_lint_auto_find_test_dependencies()
+
   find_package(ament_cmake_gtest REQUIRED)
+
+  ament_find_gtest()
+
   ament_add_gtest(test_theta_star test/test_theta_star.cpp)
-  ament_target_dependencies(test_theta_star ${dependencies})
-  target_link_libraries(test_theta_star ${library_name})
+  target_link_libraries(test_theta_star
+    ${library_name}
+    ${geometry_msgs_TARGETS}
+    nav2_costmap_2d::nav2_costmap_2d_core
+    rclcpp::rclcpp
+    rclcpp_lifecycle::rclcpp_lifecycle
+  )
 endif()
 
 
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_libraries(${library_name})
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  geometry_msgs
+  nav2_core
+  nav2_costmap_2dnav2_util
+  nav_msgs
+  rclcpp
+  rclcpp_lifecycle
+  rcl_interfaces
+  tf2_ros
+)
+ament_export_targets(${library_name})
 ament_package()
diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml
index 0fcbd0e288..0da357b581 100644
--- a/nav2_theta_star_planner/package.xml
+++ b/nav2_theta_star_planner/package.xml
@@ -9,26 +9,25 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
 
-  <depend>builtin_interfaces</depend>
-  <depend>nav2_common</depend>
+  <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
-  <depend>nav2_msgs</depend>
   <depend>nav2_util</depend>
+  <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
-  <depend>rclcpp_action</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>rcl_interfaces</depend>
   <depend>tf2_ros</depend>
-  
+
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-  <test_depend>ament_cmake_gtest</test_depend>
-  
+
   <export>
     <build_type>ament_cmake</build_type>
     <nav2_core plugin="${prefix}/global_planner_plugin.xml"/>
   </export>
 </package>
-
diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp
index ba3629bcbf..48a3f6c1bd 100644
--- a/nav2_theta_star_planner/src/theta_star.cpp
+++ b/nav2_theta_star_planner/src/theta_star.cpp
@@ -12,7 +12,11 @@
 //  See the License for the specific language governing permissions and
 //  limitations under the License.
 
+#include <functional>
 #include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
 #include "nav2_core/planner_exceptions.hpp"
 #include "nav2_theta_star_planner/theta_star.hpp"
 
diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp
index 24f2b81000..5d14368cc0 100644
--- a/nav2_theta_star_planner/src/theta_star_planner.cpp
+++ b/nav2_theta_star_planner/src/theta_star_planner.cpp
@@ -15,6 +15,10 @@
 #include <vector>
 #include <memory>
 #include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+
 #include "nav2_theta_star_planner/theta_star_planner.hpp"
 #include "nav2_theta_star_planner/theta_star.hpp"