Waypoint Travel

From KameRider
Jump to: navigation, search
  • user should know how to build a map and save map, for those who do not know, here is the tutorial website: Map Building and Navigation via Map
    • user can choose where to save the map file, just change the saving location before saving the map.
  • to allow the robot travel to a predefined location, user need to know the coordinate of that location.
  • user can estimate the location via the map, 1 square in the map equivalent to 1 meter square.
  • the heading of the robot can be set using quaternion value, NOT yaw value.
    • websites related to yaw to quaternion conversion: [1] and [2]
  • next, user need to send the information to a topic so that the robot will know that that information is its destination.
  • however, the method so send information is slightly different. Actionlib is used to send the information to the robot.
  • website related to actionlib is given here: [3]
  • the information to be sent will be x-coordinate and y-coordinate of destination, and robot final heading.
  • An example of source code with explanation:

include section:

 1 # Include actionlib library
 2 import actionlib
 3 
 4 # Include all type of messages presents in actionlib
 5 from actionlib_msgs.msg import *
 6 
 7 # Include geometry type messages 
 8 from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
 9 
10 # Include kobuki base navigation status message type
11 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
12 
13 # Include quaternion from euler convertion
14 from tf.transformations import quaternion_from_euler
15 
16 # Global variable to enable initial position record only once
17 original = 0

initiation section:

 1 # Subscribe to the move_base action server
 2 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
 3 
 4 # Wait for the action server for 120 seconds before it becomes available
 5 self.move_base.wait_for_server(rospy.Duration(120))
 6 
 7 # A variable used to hold the initial pose of the robot to be set by the user in RViz
 8 initial_pose = PoseWithCovarianceStamped()
 9 
10 # Subscribe the /initialpose topic to acquire the initial position of the robot
11 rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
12 
13 # Wait for the user to point out the initial position
14 rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
15         
16 # Make sure we have the initial pose
17 while initial_pose.header.stamp == "":
18     rospy.sleep(1)
19 
20 # Assume the location is predefined
21 
22 # Variable "locations" can be any type of message
23 locations = dict()
24 
25 # "quaternion_from_euler()"will convert yaw value (euler) to quaternion
26 
27 # Sentence below will convert 90 degree to quaternion value
28 quaternion = quaternion_from_euler(0.0, 0.0, 1.5708)
29 
30 # Assume location A is at X = 0.5m, Y = 0.5m from the origin, and the robot final heading will be 90 degree.
31 locations['A'] = Pose(Point(0.5, 0.5, 0.000), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]))
32 
33 # An user defined variable which is same type as MoveBaseGoal()
34 self.goal = MoveBaseGoal()

callback function section:

 1 def update_initial_pose(self, initial_pose):
 2 
 3     # Initial position will be stored into user generated variable.
 4     self.initial_pose = initial_pose
 5 
 6     # Initial position only record once
 7     if original == 0:
 8 
 9         # Only the heading of robot, x and y coordiantes are needed, that is why we only access the pose data
10 	self.origin = self.initial_pose.pose.pose
11 
12 	global original
13 	original = 1

main (loop) section:

 1     # Stores the final coordinate into "self.goal"
 2     self.goal.target_pose.pose = locations['A']
 3 
 4     # Stores the type of frame which will be used during path calculation
 5     self.goal.target_pose.header.frame_id = 'map'
 6 
 7     # Stores travelling time, used in path calculation
 8     self.goal.target_pose.header.stamp = rospy.Time.now()
 9 
10     # Publish all information to robot via actionlib
11     self.move_base.send_goal(self.goal)
12 
13     # The path will takes maximum 5 minutes to travel
14     # If robot reached destination in 5 minutes, variable "waiting" will be 1.
15     waiting = self.move_base.wait_for_result(rospy.Duration(300))
16     if waiting == 1:
17 
18         # Print something on terminal
19         rospy.loginfo("Reached destination")
  • source code of the above example can be found here: [4]
  • source code of integration of navigation with voice command can be found here: [5]