diff --git a/waypoint_nav/waypoints_cfg/test.yaml b/waypoint_nav/waypoints_cfg/test.yaml
new file mode 100644
index 0000000..c555a51
--- /dev/null
+++ b/waypoint_nav/waypoints_cfg/test.yaml
@@ -0,0 +1,8 @@
+waypoints:
+- point: {x: -0.555141, y: -0.439525, z: 0.00183678, vel: 1, rad: 0.8, stop: false}
+
+finish_pose:
+  header: {seq: 0, stamp: 226.652000000, frame_id: map}
+  pose: 
+    position: {x: -0.517291, y: 0.622856, z: 0}
+    orientation: {x: 0, y: 0, z: 0.69261, w: 0.721313}
diff --git a/waypoint_saver/include/waypoint_saver.h b/waypoint_saver/include/waypoint_saver.h
index aad7dbd..9cf23da 100644
--- a/waypoint_saver/include/waypoint_saver.h
+++ b/waypoint_saver/include/waypoint_saver.h
@@ -1,3 +1,6 @@
+#include <string>
+#include <sstream>
+
 class Waypoint {
 public:
     double x, y, z;
@@ -15,4 +18,19 @@
     {
     }
 
+
+    std::string getYAMLstr()
+    {
+        std::stringstream ss;
+        ss << "{";
+        ss << "x: "    << x    << ", " ;
+        ss << "y: "    << y    << ", " ;
+        ss << "z: "    << z    << ", " ;
+        ss << "vel: "  << vel  << ", " ;
+        ss << "rad: "  << rad  << ", " ;
+        ss << "stop: " << (stop ? "true" : "false") << "}" ;
+        return ss.str();
+        // {x: *, y: *, z: *, vel: *, rad: *, stop: *}
+    }
+
 };
\ No newline at end of file
diff --git a/waypoint_saver/launch/waypoint_saver.launch b/waypoint_saver/launch/waypoint_saver.launch
index a03161e..9873494 100644
--- a/waypoint_saver/launch/waypoint_saver.launch
+++ b/waypoint_saver/launch/waypoint_saver.launch
@@ -11,13 +11,14 @@
     
     
     <node name="waypoint_saver" pkg="waypoint_saver" type="waypoint_saver" output="screen">
-        <param name="save_joy_button" value="$(arg save_joy_button)"/>
-        <param name="save_joy_pose_button" value="$(arg save_joy_pose_button)"/>
-        <param name="filename" value="$(arg waypoints_file)"/>
-        <param name="default_rad" value="$(arg default_rad)"/>
-        <remap from="waypoints_joy" to="joy"/>
-        <remap from="waypoints_viz" to="clicked_point"/>
-        <remap from="finish_pose" to="move_base_simple/goal"/>
+        <param name="save_joy_button"       value="$(arg save_joy_button)"/>
+        <param name="save_joy_pose_button"  value="$(arg save_joy_pose_button)"/>
+        <param name="filename"              value="$(arg waypoints_file)"/>
+        <param name="default_rad"           value="$(arg default_rad)"/>
+        
+        <remap from="waypoints_joy"     to="joy"/>
+        <remap from="waypoints_viz"     to="clicked_point"/>
+        <remap from="finish_pose"       to="move_base_simple/goal"/>
     </node>
 
 </launch>
diff --git a/waypoint_saver/src/waypoint_saver.cpp b/waypoint_saver/src/waypoint_saver.cpp
index b0264a4..9faad27 100644
--- a/waypoint_saver/src/waypoint_saver.cpp
+++ b/waypoint_saver/src/waypoint_saver.cpp
@@ -156,17 +156,11 @@
         std::ofstream ofs(filename_.c_str(), std::ios::out);
         /*
         waypoints:
-        - point: {x: *, y: *, z: *, vel: *, rad: *}
+        - point: {x: *, y: *, z: *, ...}
         */
         ofs << "waypoints:" << std::endl;
         for(int i=0; i < waypoints_.size(); i++) {
-            ofs << "- point: {" ;
-            ofs << "x: "    << waypoints_[i].x    << ", " ;
-            ofs << "y: "    << waypoints_[i].y    << ", " ;
-            ofs << "z: "    << waypoints_[i].z    << ", " ;
-            ofs << "vel: "  << waypoints_[i].vel  << ", " ;
-            ofs << "rad: "  << waypoints_[i].rad  << ", " ;
-            ofs << "stop: " << waypoints_[i].stop << "}"  ;
+            ofs << "- point: " << waypoints_[i].getYAMLstr() << std::endl;
             ofs << std::endl;
         }
         /*