diff --git a/waypoint_saver/CMakeLists.txt b/waypoint_saver/CMakeLists.txt
index b10b611..4134cad 100644
--- a/waypoint_saver/CMakeLists.txt
+++ b/waypoint_saver/CMakeLists.txt
@@ -5,6 +5,7 @@
 
 find_package(catkin REQUIRED
   geometry_msgs
+  visualization_msgs
   roscpp
   tf
 )
diff --git a/waypoint_saver/launch/waypoint_saver.launch b/waypoint_saver/launch/waypoint_saver.launch
index 332d94d..a03161e 100644
--- a/waypoint_saver/launch/waypoint_saver.launch
+++ b/waypoint_saver/launch/waypoint_saver.launch
@@ -1,4 +1,5 @@
 <?xml version="1.0"?>
+
 <launch>
     <arg name="scan_topic" default="scan"/>
     <arg name="joy_config" default="elecom_joy"/>
@@ -19,4 +20,4 @@
         <remap from="finish_pose" to="move_base_simple/goal"/>
     </node>
 
-</launch>
\ No newline at end of file
+</launch>
diff --git a/waypoint_saver/package.xml b/waypoint_saver/package.xml
index ad87480..1ce6999 100644
--- a/waypoint_saver/package.xml
+++ b/waypoint_saver/package.xml
@@ -20,10 +20,12 @@
 
 
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>tf</build_depend>
   
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>tf</exec_depend>
 
diff --git a/waypoint_saver/src/waypoint_saver.cpp b/waypoint_saver/src/waypoint_saver.cpp
index 5181e03..b7e2de7 100644
--- a/waypoint_saver/src/waypoint_saver.cpp
+++ b/waypoint_saver/src/waypoint_saver.cpp
@@ -4,6 +4,8 @@
 #include <sensor_msgs/Joy.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/Marker.h>
 
 #include <vector>
 #include <fstream>
@@ -22,6 +24,7 @@
         waypoints_viz_sub_ = nh_.subscribe("waypoints_viz", 1, &WaypointsSaver::waypointsVizCallback, this);
         waypoints_joy_sub_ = nh_.subscribe("waypoints_joy", 1, &WaypointsSaver::waypointsJoyCallback, this);
         finish_pose_sub_ = nh_.subscribe("finish_pose", 1, &WaypointsSaver::finishPoseCallback, this);
+        markers_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("waypoints", 10);
 
         ros::NodeHandle private_nh("~");
         private_nh.param("filename", filename_, filename_);
@@ -46,12 +49,14 @@
                 point.set_z(robot_gl.getOrigin().z());
                 point.set_rad(default_rad_);
                 waypoints_.push_back(point);
+                addWaypointMarker(point);
                 saved_time = ros::Time::now();
             }
             catch(tf::TransformException &e) {
                 ROS_WARN_STREAM("tf::TransformException: " << e.what());
             }
         }
+        markers_pub_.publish(marker_array_);
     }
     
     
@@ -76,6 +81,36 @@
         waypoints_.clear();
     }
     
+    
+    
+    void addWaypointMarker(Waypoint point)
+    {
+        double scale = 0.2;
+        int number = waypoints_.size();
+        std::stringstream name;
+        name << "Waypoint " << number;
+        
+        visualization_msgs::Marker marker;
+        marker.header.frame_id = world_frame_;
+        marker.header.stamp = ros::Time::now();
+        marker.ns = name.str();
+        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.z = scale/2;
+        marker.scale.x = scale;
+        marker.scale.y = scale;
+        marker.scale.z = scale;
+        marker.color.r=0.8f;
+        marker.color.g=0.2f;
+        marker.color.b=0.2f;
+        marker.color.a = 1.0f;
+        
+        marker_array_.markers.push_back(marker);
+    }
+    
 
 
     void save()
@@ -136,7 +171,9 @@
     ros::Subscriber waypoints_viz_sub_;
     ros::Subscriber waypoints_joy_sub_;
     ros::Subscriber finish_pose_sub_;
+    ros::Publisher markers_pub_;
     std::vector<Waypoint> waypoints_;
+    visualization_msgs::MarkerArray marker_array_;
     geometry_msgs::PoseStamped finish_pose_;
     tf::TransformListener tf_listener_;
     int save_joy_button_;
@@ -158,4 +195,4 @@
     saver.run();
 
     return 0;
-}
\ No newline at end of file
+}