diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
index b917cbffc33..5258fef1f7b 100644
--- a/nav2_rviz_plugins/CMakeLists.txt
+++ b/nav2_rviz_plugins/CMakeLists.txt
@@ -12,7 +12,6 @@ find_package(nav2_util REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(nav2_route REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rviz_common REQUIRED)
@@ -26,6 +25,37 @@ find_package(nav2_ros_common REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
+# In Kilted and older distros, rviz with Qt6 is not supported
+if(TARGET Qt5::Core)
+  set(QT_VERSION_MAJOR 5)
+  set(QT_VERSION ${Qt5Core_VERSION})
+  # In Qt5, state machine is part of core
+  find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
+elseif(TARGET Qt6::Core)
+  set(QT_VERSION_MAJOR 6)
+  set(QT_VERSION ${Qt6Core_VERSION})
+  find_package(Qt6 REQUIRED COMPONENTS Core Gui Widgets StateMachine Test Concurrent)
+endif()
+if(${QT_VERSION} VERSION_LESS 5.15.0)
+  function(qt_wrap_cpp out)
+    qt5_wrap_cpp(_sources ${ARGN})
+    set("${out}" ${_sources} PARENT_SCOPE)
+  endfunction()
+  function(qt_wrap_ui out)
+      qt5_wrap_ui(_uis_hdrs ${ARGN})
+      set("${out}" ${_uis_hdrs} PARENT_SCOPE)
+  endfunction()
+endif()
+
+set(QT_LIBRARIES
+  Qt${QT_VERSION_MAJOR}::Core
+  Qt${QT_VERSION_MAJOR}::Gui
+  Qt${QT_VERSION_MAJOR}::Widgets
+)
+if(QT_VERSION_MAJOR EQUAL 6)
+  list(APPEND QT_LIBRARIES Qt6::StateMachine)
+endif()
+
 nav2_package()
 
 # We specifically don't turn on CMAKE_AUTOMOC, since it generates one huge
@@ -43,10 +73,10 @@ set(nav2_rviz_plugins_headers_to_moc
 )
 
 foreach(header "${nav2_rviz_plugins_headers_to_moc}")
-  qt5_wrap_cpp(nav2_rviz_plugins_moc_files "${header}")
+  qt_wrap_cpp(nav2_rviz_plugins_moc_files "${header}")
 endforeach()
 
-qt5_wrap_ui(route_tool_UIS_H resource/route_tool.ui)
+qt_wrap_ui(route_tool_UIS_H resource/route_tool.ui)
 
 # Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
@@ -70,7 +100,6 @@ add_library(${library_name} SHARED
   ${route_tool_UIS_H}
 )
 target_include_directories(${library_name} PUBLIC
-  ${Qt5Widgets_INCLUDE_DIRS}
   ${OGRE_INCLUDE_DIRS}
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
@@ -90,6 +119,7 @@ target_link_libraries(${library_name} PUBLIC
   ${std_msgs_TARGETS}
   tf2_geometry_msgs::tf2_geometry_msgs
   ${visualization_msgs_TARGETS}
+  ${QT_LIBRARIES}
 )
 target_link_libraries(${library_name} PRIVATE
   ament_index_cpp::ament_index_cpp
@@ -132,7 +162,7 @@ ament_export_dependencies(
   nav2_msgs
   nav2_route
   nav2_util
-  Qt5
+  Qt${QT_VERSION_MAJOR}
   rclcpp
   rclcpp_action
   rviz_common
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 fcbc1d1b11a..b425a307368 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp
@@ -18,6 +18,9 @@
 // QT
 #include <QtWidgets>
 #include <QBasicTimer>
+#include <QStateMachine>
+#include <QState>
+#include <QSignalTransition>
 
 #include <memory>
 #include <string>
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 8c99a270d64..57dd701b1a2 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
@@ -17,6 +17,8 @@
 
 #include <QtWidgets>
 #include <QBasicTimer>
+#include <QStateMachine>
+#include <QSignalTransition>
 #undef NO_ERROR
 
 #include <memory>
diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp
index 8da22379965..5a0b3b9acc1 100644
--- a/nav2_rviz_plugins/src/nav2_panel.cpp
+++ b/nav2_rviz_plugins/src/nav2_panel.cpp
@@ -870,12 +870,12 @@ Nav2Panel::startThread()
 void
 Nav2Panel::onPause()
 {
-  QFuture<void> futureNav =
+  QFuture<bool> futureNav =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::pause,
       client_nav_.get(), std::placeholders::_1), server_timeout_);
-  QFuture<void> futureLoc =
+  QFuture<bool> futureLoc =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::pause,
@@ -885,12 +885,12 @@ Nav2Panel::onPause()
 void
 Nav2Panel::onResume()
 {
-  QFuture<void> futureNav =
+  QFuture<bool> futureNav =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::resume,
       client_nav_.get(), std::placeholders::_1), server_timeout_);
-  QFuture<void> futureLoc =
+  QFuture<bool> futureLoc =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::resume,
@@ -900,12 +900,12 @@ Nav2Panel::onResume()
 void
 Nav2Panel::onStartup()
 {
-  QFuture<void> futureNav =
+  QFuture<bool> futureNav =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::startup,
       client_nav_.get(), std::placeholders::_1), server_timeout_);
-  QFuture<void> futureLoc =
+  QFuture<bool> futureLoc =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::startup,
@@ -915,12 +915,12 @@ Nav2Panel::onStartup()
 void
 Nav2Panel::onShutdown()
 {
-  QFuture<void> futureNav =
+  QFuture<bool> futureNav =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::reset,
       client_nav_.get(), std::placeholders::_1), server_timeout_);
-  QFuture<void> futureLoc =
+  QFuture<bool> futureLoc =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::reset,