diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index e69c3f6..2195d1f 100755
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -139,9 +139,15 @@
                     point.x = pose.position.x;
                     point.y = pose.position.y;
                     point.z = pose.position.z;
-                    (*wp_node)[i]["point"]["vel"] >> point.vel;
-                    (*wp_node)[i]["point"]["rad"] >> point.rad;
-                    (*wp_node)[i]["point"]["stop"] >> point.stop;
+                    try {
+                        (*wp_node)[i]["point"]["vel"]  >> point.vel;
+                    } catch(...) {} // Use default value
+                    try {
+                        (*wp_node)[i]["point"]["rad"]  >> point.rad;
+                    } catch(...) {}
+                    try {
+                        (*wp_node)[i]["point"]["stop"] >> point.stop;
+                    } catch(...) {}
                     waypoint_list_.push_back(point);
                 }
             } else {
diff --git a/waypoint_nav/waypoints_cfg/waypoints.yaml b/waypoint_nav/waypoints_cfg/waypoints.yaml
new file mode 100644
index 0000000..d9fe6cb
--- /dev/null
+++ b/waypoint_nav/waypoints_cfg/waypoints.yaml
@@ -0,0 +1,12 @@
+waypoints:
+- point: {x: 1.0, y: 0.0, z: 0.0, vel: 0.5, rad: 1.0, stop: false}
+- point: {x: 2.0, y: 0.0, z: 0.0, vel: 1.0, rad: 1.0, stop: true}
+- point: {x: 3.0, y: 0.0, z: 0.0, vel: 1.0, rad: 0.5, stop: false}
+- point: {x: 4.0, y: 0.0, z: 0.0, vel: 1.0, rad: 1.0, stop: false, tandem_start: 1}
+- point: {x: 5.0, y: 0.0, z: 0.0, vel: 0.5, rad: 0.5, stop: false}
+- point: {x: 6.0, y: 0.0, z: 0.0, vel: 0.5, rad: 0.5, stop: false, tandem_end: 1}
+finish_pose:
+  header: {seq: 0, stamp: 113.132, frame_id: map}
+  pose:
+    position: {x: 7.0, y: 0.0, z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
\ No newline at end of file
diff --git a/waypoint_saver/launch/waypoint_saver.launch b/waypoint_saver/launch/waypoint_saver.launch
index 9873494..023a6d0 100644
--- a/waypoint_saver/launch/waypoint_saver.launch
+++ b/waypoint_saver/launch/waypoint_saver.launch
@@ -4,10 +4,10 @@
     <arg name="scan_topic" default="scan"/>
     <arg name="joy_config" default="elecom_joy"/>
     <arg name="joy_dev" default="/dev/input/js0"/>
-    <arg name="waypoints_file" default="$(find waypoint_nav)/waypoints_cfg/test.yaml"/>
+    <arg name="waypoints_file" default="$(find waypoint_nav)/waypoints_cfg/waypoints.yaml"/>
     <arg name="save_joy_button" default="0"/>
     <arg name="save_joy_pose_button" default="1"/>
-    <arg name="default_rad" default="0.8"/>
+    <arg name="default_rad" default="1.0"/>
     
     
     <node name="waypoint_saver" pkg="waypoint_saver" type="waypoint_saver" output="screen">