diff --git a/rchomeedu_mapping/README.md b/rchomeedu_mapping/README.md
index 7af6546..755f9b2 100644
--- a/rchomeedu_mapping/README.md
+++ b/rchomeedu_mapping/README.md
@@ -11,6 +11,13 @@
### Specific instructions for Turtlebot
+**Command-line interface**
+
+ cd $CATKIN_WS/src/robot_bringup
+ roslaunch ./start_turtle.launch
+
+ cd $CATKIN_WS/src/robocup_nav_tutorial
+ roslaunch ./map_setup.launch
### Specific instructions for MARRtino
diff --git a/rchomeedu_navigation/README.md b/rchomeedu_navigation/README.md
index 9526214..96bc48f 100644
--- a/rchomeedu_navigation/README.md
+++ b/rchomeedu_navigation/README.md
@@ -9,8 +9,23 @@
* path planner (e.g., move_base)
### Specific instructions for Turtlebot
-
-
+**Command-line interface**
+```
+ cd ~/catkin/src/robocup_nav_tutorial/launch
+```
+modify the launch file named navigation.launch, change the path with your map path
+
+```
+
+```
+start your robot
+```
+roslaunch robot_bringup start_turtle.launch
+```
+start navigation
+```
+roslaunch robocup_nav_tutorial navigation_turtle.launch
+```
### Specific instructions for MARRtino
diff --git a/rchomeedu_navigation/launch/navigation_turtle.launch b/rchomeedu_navigation/launch/navigation_turtle.launch
new file mode 100644
index 0000000..5d086f8
--- /dev/null
+++ b/rchomeedu_navigation/launch/navigation_turtle.launch
@@ -0,0 +1,41 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rchomeedu_navigation/scripts/save_waypoint.py b/rchomeedu_navigation/scripts/save_waypoint.py
new file mode 100644
index 0000000..ff791db
--- /dev/null
+++ b/rchomeedu_navigation/scripts/save_waypoint.py
@@ -0,0 +1,82 @@
+#!/urs/bin/env python
+# -*- encoding:UTF-8 -*-
+
+import rospy
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+
+class get_point():
+
+ def __init__(self):
+
+ rospy.init_node("save_point")
+ self.point_dataset = []
+ self.point_num = []
+ self.switch=True
+ con = True
+ print 'use keyboard to control your robot'
+ print 'please input point_id if you want to exit please input stop'
+ while con:
+ print 'Get the waypoint : '
+ str = raw_input()
+ if self.switch == True:
+ if str != 'stop':
+ self.switch = False
+ self.point_num.append(str)
+ self.save_waypoint()
+ else:
+ con=False
+ self.write2txt()
+ rospy.sleep(2)
+
+
+ def save_waypoint(self):
+
+ rospy.loginfo("save_point")
+ amcl_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.amcl_callback)
+
+ def amcl_callback(self, msg):
+
+ my_point = MoveBaseGoal()
+ my_point.target_pose.header.frame_id = '/map'
+ my_point.target_pose.header.seq = msg.header.seq
+ my_point.target_pose.header.stamp = msg.header.stamp
+ my_point.target_pose.pose.position.x = msg.pose.pose.position.x
+ my_point.target_pose.pose.position.y = msg.pose.pose.position.y
+ my_point.target_pose.pose.position.z = msg.pose.pose.position.z
+ my_point.target_pose.pose.orientation.x = msg.pose.pose.orientation.x
+ my_point.target_pose.pose.orientation.y = msg.pose.pose.orientation.y
+ my_point.target_pose.pose.orientation.z = msg.pose.pose.orientation.z
+ my_point.target_pose.pose.orientation.w = msg.pose.pose.orientation.w
+ self.point_dataset.append(my_point)
+ rospy.loginfo("Point saved successfully!")
+ self.switch = True
+
+ def write2txt(self):
+
+ f = open('/home/hts/ROS_test/src/nav_test/src/waypoints.txt','w')
+ for point in range (len(self.point_dataset)):
+ f.write(self.point_num[point])
+ f.write(' ')
+ f.write('position ')
+ f.write('x '+str(self.point_dataset[point].target_pose.pose.position.x)+' ')
+ f.write('y '+str(self.point_dataset[point].target_pose.pose.position.y)+' ')
+ f.write('z '+str(self.point_dataset[point].target_pose.pose.position.z)+' ')
+ f.write('orientation ')
+ f.write('x '+str(self.point_dataset[point].target_pose.pose.orientation.x)+' ')
+ f.write('y '+str(self.point_dataset[point].target_pose.pose.orientation.y)+' ')
+ f.write('z '+str(self.point_dataset[point].target_pose.pose.orientation.z)+' ')
+ f.write('w '+str(self.point_dataset[point].target_pose.pose.orientation.w)+' ')
+ f.write('\n')
+
+
+if __name__ == "__main__":
+
+ try :
+ get_point()
+ except rospy.ROSInterruptException:
+ print "------save_point_error!------"
+
+
+
+