diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..dbe9c82
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+.vscode/
\ No newline at end of file
diff --git a/waypoint_nav/CMakeLists.txt b/waypoint_nav/CMakeLists.txt
index 54f30f4..ba766d3 100644
--- a/waypoint_nav/CMakeLists.txt
+++ b/waypoint_nav/CMakeLists.txt
@@ -31,6 +31,7 @@
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
+  DEPENDS waypoint_saver
 )
 
 include_directories(
diff --git a/waypoint_nav/package.xml b/waypoint_nav/package.xml
index cdeeed2..74eb2df 100644
--- a/waypoint_nav/package.xml
+++ b/waypoint_nav/package.xml
@@ -27,6 +27,7 @@
   <build_depend>actionlib</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>std_srvs</build_depend>
+  <build_depend>waypoint_saver</build_depend>
   
   <exec_depend>move_base</exec_depend>
   <exec_depend>yaml-cpp</exec_depend>
diff --git a/waypoint_nav/rviz_cfg/nav.rviz b/waypoint_nav/rviz_cfg/nav.rviz
index a0b5c61..8c94ae3 100644
--- a/waypoint_nav/rviz_cfg/nav.rviz
+++ b/waypoint_nav/rviz_cfg/nav.rviz
@@ -1,11 +1,11 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 78
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 383
+    Tree Height: 595
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -301,7 +301,7 @@
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 6.003083229064941
+      Distance: 10.579483032226562
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -309,8 +309,8 @@
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -0.36166632175445557
-        Y: 0.0590318888425827
+        X: -0.2993233799934387
+        Y: 0.4264318346977234
         Z: 0
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
@@ -319,7 +319,7 @@
       Near Clip Distance: 0.009999999776482582
       Pitch: 1.5697963237762451
       Target Frame: base_link
-      Yaw: 3.2353971004486084
+      Yaw: 3.115396738052368
     Saved: ~
 Window Geometry:
   Displays:
@@ -327,7 +327,7 @@
   Height: 847
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000193000002b3fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e0000020a000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c010000024e000000a30000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650100000000000005e00000026c00fffffffb0000000800540069006d0065010000000000000450000000000000000000000447000002b300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000193000002f4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000290000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c01000002d40000005e0000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000026c00fffffffb0000000800540069006d0065010000000000000450000000000000000000000447000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   StateTriggerPanel:
@@ -339,5 +339,5 @@
   Views:
     collapsed: true
   Width: 1504
-  X: 0
-  Y: 0
+  X: 25
+  Y: 1