diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
index c5f5c788aa0..bd9059d6989 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 197fe2b6550..b70529f11a7 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_cost.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::GetCost>::SharedPtr local_cost_client_;
   rclcpp::Client<nav2_msgs::srv::GetCost>::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 68380cb43e3..868c644e4b1 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 ce4212a4b88..471dce5ce5a 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 90c4bdfa4d7..c90944df774 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 f25a71cd80e..5873b54f457 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::GetCost>("/local_costmap/get_cost_local_costmap");
+    node->create_client<nav2_msgs::srv::GetCost>("/local_costmap/get_cost_local_costmap");
   global_cost_client_ =
-    node_->create_client<nav2_msgs::srv::GetCost>("/global_costmap/get_cost_global_costmap");
+    node->create_client<nav2_msgs::srv::GetCost>("/global_costmap/get_cost_global_costmap");
 }
 
 void CostmapCostTool::activate() {}
@@ -106,22 +115,24 @@ void CostmapCostTool::callCostService(float x, float y)
 void CostmapCostTool::handleLocalCostResponse(
   rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture future)
 {
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
   if (response->cost != -1) {
-    RCLCPP_INFO(node_->get_logger(), "Local costmap cost: %.1f", response->cost);
+    RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->cost);
   } 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::GetCost>::SharedFuture future)
 {
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
   if (response->cost != -1) {
-    RCLCPP_INFO(node_->get_logger(), "Global costmap cost: %.1f", response->cost);
+    RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->cost);
   } 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 909e9a74fd5..7daa327079c 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 a3742685f3b..e10f1eccdb4 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")));