diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 392dbb9..d983e6d 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -118,23 +118,23 @@
             #endif
             geometry_msgs::Pose pose;
             Waypoint point;
-            float vel, rad;
-            bool stop;
+            // float vel, rad;
+            // bool stop;
             if (wp_node != NULL) {
                 for(int i=0; i < wp_node->size(); i++) {
                     (*wp_node)[i]["point"]["x"] >> pose.position.x;
                     (*wp_node)[i]["point"]["y"] >> pose.position.y;
                     (*wp_node)[i]["point"]["z"] >> pose.position.z;
-                    (*wp_node)[i]["point"]["vel"] >> vel;
-                    (*wp_node)[i]["point"]["rad"] >> rad;
-                    (*wp_node)[i]["point"]["stop"] >> stop;
                     waypoints_.poses.push_back(pose);
-                    point.set_x(pose.position.x);
-                    point.set_y(pose.position.y);
-                    point.set_z(pose.position.z);
-                    point.set_vel(vel);
-                    point.set_rad(rad);
-                    point.set_stop(stop);
+                    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;
+                    // point.vel = vel;
+                    // point.rad = rad;
+                    // point.stop = stop;
                     waypoint_list_.push_back(point);
                 }
             } else {
@@ -157,9 +157,9 @@
                 (*fp_node)["pose"]["orientation"]["z"] >> pose.orientation.z;
                 (*fp_node)["pose"]["orientation"]["w"] >> pose.orientation.w;
                 waypoints_.poses.push_back(pose);
-                point.set_x(pose.position.x);
-                point.set_y(pose.position.y);
-                point.set_z(pose.position.z);
+                point.x = pose.position.x;
+                point.y = pose.position.y;
+                point.z = pose.position.z;
                 waypoint_list_.push_back(point);
             } else {
                 return false;
@@ -269,7 +269,7 @@
     */
     bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8)
     {
-        if (waypoint_list_[current_wp_num_].get_stop()) {
+        if (waypoint_list_[current_wp_num_].stop) {
             return navigationFinished();
         }
         tf::StampedTransform robot_gl = getRobotPosGL();
@@ -343,7 +343,7 @@
     */
     void setMaxVel()
     {
-        float max_vel_rate = waypoint_list_[current_wp_num_].get_vel();
+        float max_vel_rate = waypoint_list_[current_wp_num_].vel;
         max_vel_msg_.data = max_vel_rate;
         max_vel_pub_.publish(max_vel_msg_);
     }
@@ -375,7 +375,7 @@
             startNavigationGL(*current_waypoint_);
             int resend_goal = 0;
             double start_nav_time = ros::Time::now().toSec();
-            dist_err_ = waypoint_list_[current_wp_num_].get_rad();
+            dist_err_ = waypoint_list_[current_wp_num_].rad;
             try {
                 // loop until reach waypoint
                 while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
@@ -400,7 +400,7 @@
                     sleep();
                 }
                 // if current waypoint is stop point
-                if (waypoint_list_[current_wp_num_].get_stop()) {
+                if (waypoint_list_[current_wp_num_].stop) {
                     has_activate_ = false;
                     move_base_action_.cancelAllGoals();
                     ROS_INFO("Waiting for navigation to resume...");
diff --git a/waypoint_saver/include/waypoint_saver.h b/waypoint_saver/include/waypoint_saver.h
index c386710..aad7dbd 100644
--- a/waypoint_saver/include/waypoint_saver.h
+++ b/waypoint_saver/include/waypoint_saver.h
@@ -1,30 +1,18 @@
-#include <geometry_msgs/PointStamped.h>
-
-
-class Waypoint : public geometry_msgs::PointStamped {
+class Waypoint {
 public:
+    double x, y, z;
     float vel;
     float rad;
     bool stop;
 
     Waypoint() : 
-        vel(1.0), 
-        rad(0.8), 
+        x(0.0),
+        y(0.0),
+        z(0.0),
+        vel(1.0),
+        rad(0.8),
         stop(false)
     {
     }
 
-    double get_x() { return point.x; }
-    double get_y() { return point.y; }
-    double get_z() { return point.z; }
-    float get_vel() { return vel; }
-    float get_rad() { return rad; }
-    bool get_stop() { return stop; }
-
-    void set_x(double x) { point.x = x; }
-    void set_y(double y) { point.y = y; }
-    void set_z(double z) { point.z = z; }
-    void set_vel(float v) { vel = v; }
-    void set_rad(float r) { rad = r; }
-    void set_stop(bool s) { stop = s; }
 };
\ No newline at end of file
diff --git a/waypoint_saver/src/waypoint_saver.cpp b/waypoint_saver/src/waypoint_saver.cpp
index 6d6f7c4..b0264a4 100644
--- a/waypoint_saver/src/waypoint_saver.cpp
+++ b/waypoint_saver/src/waypoint_saver.cpp
@@ -36,6 +36,9 @@
     
     
 
+    /*
+    ++++++++++ Save waypoint by joy stick button ++++++++++
+    */
     void waypointsJoyCallback(const sensor_msgs::Joy &msg)
     {
         static ros::Time saved_time(0.0);
@@ -48,14 +51,16 @@
     
     
 
+    /*
+    ++++++++++ Save waypoint by "Publish Point" on rviz ++++++++++
+    */
     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);
+        point.x = msg.point.x;
+        point.y = msg.point.y;
+        point.z = msg.point.z;
+        point.rad = default_rad_;
         waypoints_.push_back(point);
         addWaypointMarker(point);
         publishMarkerArray();
@@ -63,6 +68,9 @@
     
     
 
+    /*
+    ++++++++++ Save finish pose by "2D Nav Goal" on rviz ++++++++++
+    */
     void finishPoseCallback(const geometry_msgs::PoseStamped &msg)
     {
         finish_pose_ = msg;
@@ -72,16 +80,19 @@
 
 
 
+    /*
+    ++++++++++ Get robot's pose and add waypoint ++++++++++
+    */
     void pushbackWaypoint()
     {
         tf::StampedTransform robot_gl;
         try {
             tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
             Waypoint point;
-            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_);
+            point.x = robot_gl.getOrigin().x();
+            point.y = robot_gl.getOrigin().y();
+            point.z = robot_gl.getOrigin().z();
+            point.rad = default_rad_;
             waypoints_.push_back(point);
             addWaypointMarker(point);
         }
@@ -92,6 +103,9 @@
     
     
     
+    /*
+    ++++++++++ Add marker to display on rviz ++++++++++
+    */
     void addWaypointMarker(Waypoint point)
     {
         double scale = 0.2;
@@ -106,8 +120,8 @@
         marker.id = number;
         marker.type = visualization_msgs::Marker::SPHERE;
         marker.action = visualization_msgs::Marker::ADD;
-        marker.pose.position.x = point.get_x();
-        marker.pose.position.y = point.get_y();
+        marker.pose.position.x = point.x;
+        marker.pose.position.y = point.y;
         marker.pose.position.z = scale/2;
         marker.scale.x = scale;
         marker.scale.y = scale;
@@ -122,6 +136,9 @@
 
 
 
+    /*
+    ++++++++++ Publish markerArray to display waypoints on rviz ++++++++++
+    */
     void publishMarkerArray()
     {
         visualization_msgs::MarkerArray marker_array;
@@ -131,6 +148,9 @@
     
 
 
+    /*
+    ++++++++++ Save waypoints as yaml file ++++++++++
+    */
     void save()
     {
         std::ofstream ofs(filename_.c_str(), std::ios::out);
@@ -141,12 +161,12 @@
         ofs << "waypoints:" << std::endl;
         for(int i=0; i < waypoints_.size(); i++) {
             ofs << "- point: {" ;
-            ofs << "x: "    << waypoints_[i].get_x()    << ", " ;
-            ofs << "y: "    << waypoints_[i].get_y()    << ", " ;
-            ofs << "z: "    << waypoints_[i].get_z()    << ", " ;
-            ofs << "vel: "  << waypoints_[i].get_vel()  << ", " ;
-            ofs << "rad: "  << waypoints_[i].get_rad()  << ", " ;
-            ofs << "stop: " << waypoints_[i].get_stop() << "}"  ;
+            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 << std::endl;
         }
         /*
@@ -191,7 +211,6 @@
     ros::Subscriber finish_pose_sub_;
     ros::Publisher markers_pub_;
     std::vector<Waypoint> waypoints_;
-    // visualization_msgs::MarkerArray marker_array_;
     std::vector<visualization_msgs::Marker> markers_;
     geometry_msgs::PoseStamped finish_pose_;
     tf::TransformListener tf_listener_;