diff --git a/waypoint_nav/param/test.yaml b/waypoint_nav/param/test.yaml
deleted file mode 100644
index 2a2d2a1..0000000
--- a/waypoint_nav/param/test.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-waypoints:
-- point: {x: 0, y: 0, z: 0, vel: 1, rad: 0.3}
-finish_pose:
-  header: {seq: 0, stamp: 0.000000000, frame_id: }
-  pose: 
-    position: {x: 0, y: 0, z: 0}
-    orientation: {x: 0, y: 0, z: 0, w: 0}
diff --git a/waypoint_saver/src/waypoint_saver.cpp b/waypoint_saver/src/waypoint_saver.cpp
index 566ea05..afe356b 100644
--- a/waypoint_saver/src/waypoint_saver.cpp
+++ b/waypoint_saver/src/waypoint_saver.cpp
@@ -35,7 +35,8 @@
 class WaypointsSaver{
 public:
     WaypointsSaver() : 
-        filename_("waypoints.yaml")
+        filename_("waypoints.yaml"), 
+        default_rad_(0.8)
     {
         waypoints_viz_sub_ = nh_.subscribe("waypoints_viz", 1, &WaypointsSaver::waypointsVizCallback, this);
         waypoints_joy_sub_ = nh_.subscribe("waypoints_joy", 1, &WaypointsSaver::waypointsJoyCallback, this);
@@ -46,52 +47,58 @@
         private_nh.param("save_joy_button", save_joy_button_, 0);
         private_nh.param("robot_frame", robot_frame_, std::string("base_link"));
         private_nh.param("world_frame", world_frame_, std::string("map"));
-        private_nh.param("default_rad", default_rad_, 0.8);
+        private_nh.param("default_rad", default_rad_, default_rad_);
     }
     
     
-    void waypointsJoyCallback(const sensor_msgs::Joy &msg) {
+    void waypointsJoyCallback(const sensor_msgs::Joy &msg)
+    {
         static ros::Time saved_time(0.0);
-        //ROS_INFO_STREAM("joy = " << msg);
-        if(msg.buttons[save_joy_button_] == 1 && (ros::Time::now() - saved_time).toSec() > 3.0){
+        if(msg.buttons[save_joy_button_] == 1 && (ros::Time::now() - saved_time).toSec() > 3.0) {
             tf::StampedTransform robot_gl;
-            try{
+            try {
                 tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
                 Waypoint point;
-                point.set_x(robot_gl.getOrigin().x()); // point.point.x = robot_gl.getOrigin().x();
-                point.set_y(robot_gl.getOrigin().y()); //point.point.y = robot_gl.getOrigin().y();
-                point.set_z(robot_gl.getOrigin().z()); //point.point.z = robot_gl.getOrigin().z();
+                point.set_x(robot_gl.getOrigin().x());
+                point.set_y(robot_gl.getOrigin().y());
+                point.set_z(robot_gl.getOrigin().z());
+                point.set_rad(default_rad_);
                 waypoints_.push_back(point);
                 saved_time = ros::Time::now();
-            }catch(tf::TransformException &e){
+            }
+            catch(tf::TransformException &e) {
                 ROS_WARN_STREAM("tf::TransformException: " << e.what());
             }
         }
     }
     
     
-    void waypointsVizCallback(const geometry_msgs::PointStamped &msg) {
+    void waypointsVizCallback(const geometry_msgs::PointStamped &msg)
+    {
         Waypoint point;
         point.set_x(msg.point.x);
         point.set_y(msg.point.y);
         point.set_z(msg.point.z);
+        point.set_rad(default_rad_);
         ROS_INFO_STREAM("point = " << point);
         waypoints_.push_back(point);
     }
     
     
-    void finishPoseCallback(const geometry_msgs::PoseStamped &msg) {
+    void finishPoseCallback(const geometry_msgs::PoseStamped &msg)
+    {
         finish_pose_ = msg;
         save2();
         waypoints_.clear();
     }
     
     
-    void save() {
+    void save()
+    {
         std::ofstream ofs(filename_.c_str(), std::ios::out);
         
         ofs << "waypoints:" << std::endl;
-        for(int i=0; i < waypoints_.size(); i++){
+        for(int i=0; i < waypoints_.size(); i++) {
             ofs << "    " << "- point:" << std::endl;
             ofs << "        x: " << waypoints_[i].point.x << std::endl;
             ofs << "        y: " << waypoints_[i].point.y << std::endl;
@@ -120,13 +127,8 @@
     }
 
 
-    void save2() {
-        Waypoint point;
-        point.set_x(0.0);
-        point.set_y(0.0);
-        point.set_z(0.0);
-        waypoints_.push_back(point);
-
+    void save2()
+    {
         std::ofstream ofs(filename_.c_str(), std::ios::out);
         /*
         waypoints:
@@ -169,7 +171,8 @@
     }
     
 
-    void run() {
+    void run()
+    {
         ros::spin();
     }
 
@@ -191,7 +194,8 @@
 
 
 
-int main(int argc, char *argv[]) {
+int main(int argc, char *argv[])
+{
     ros::init(argc, argv, "waypoints_saver");
     WaypointsSaver saver;
     saver.save2();