diff --git a/waypoint_navigation_plugin/cmake/ROS2.cmake b/waypoint_navigation_plugin/cmake/ROS2.cmake
index 7ab3fb9..7f85f5e 100644
--- a/waypoint_navigation_plugin/cmake/ROS2.cmake
+++ b/waypoint_navigation_plugin/cmake/ROS2.cmake
@@ -21,6 +21,7 @@ find_package(rviz_common REQUIRED)
 find_package(rviz_default_plugins REQUIRED)
 find_package(rviz_rendering REQUIRED)
 find_package(rosbag2_cpp REQUIRED)
+find_package(rosbag2_storage REQUIRED)
 find_package(mav_manager_srv REQUIRED)
 find_package(rviz_ogre_vendor REQUIRED)
 find_package(std_srvs REQUIRED)
diff --git a/waypoint_navigation_plugin/package.xml b/waypoint_navigation_plugin/package.xml
index 45ba4c5..ec35723 100644
--- a/waypoint_navigation_plugin/package.xml
+++ b/waypoint_navigation_plugin/package.xml
@@ -24,6 +24,7 @@
   <depend condition="$ROS_VERSION == 2">rviz_rendering</depend>
   <depend condition="$ROS_VERSION == 2">sensor_msgs</depend>
   <depend condition="$ROS_VERSION == 2">rosbag2_cpp</depend>
+  <depend condition="$ROS_VERSION == 2">rosbag2_storage</depend>
   <depend condition="$ROS_VERSION == 2">mav_manager_srv</depend>
 
   <build_depend condition="$ROS_VERSION == 2">qtbase5-dev</build_depend>
diff --git a/waypoint_navigation_plugin/src/waypoint_nav_frame_ros2.hpp b/waypoint_navigation_plugin/src/waypoint_nav_frame_ros2.hpp
index 4701d69..e7a28bf 100644
--- a/waypoint_navigation_plugin/src/waypoint_nav_frame_ros2.hpp
+++ b/waypoint_navigation_plugin/src/waypoint_nav_frame_ros2.hpp
@@ -65,7 +65,7 @@
 #include <rosbag2_cpp/writer.hpp>
 #include <rosbag2_cpp/reader.hpp>
 #include <rclcpp/serialization.hpp>
-#include <rosbag2_cpp/storage_options.hpp>
+#include <rosbag2_storage/storage_options.hpp>
 #include <OgrePrerequisites.h>
 
 typedef struct {
diff --git a/waypoint_navigation_plugin/src/waypoint_nav_tool_ros2.cpp b/waypoint_navigation_plugin/src/waypoint_nav_tool_ros2.cpp
index 83b67be..885967f 100644
--- a/waypoint_navigation_plugin/src/waypoint_nav_tool_ros2.cpp
+++ b/waypoint_navigation_plugin/src/waypoint_nav_tool_ros2.cpp
@@ -72,6 +72,7 @@ WaypointNavTool::~WaypointNavTool()
 
   delete frame_;
   delete frame_dock_;
+  delete moving_flag_node_;
 }
 
 void WaypointNavTool::onInitialize()