摘要: 原创博客：转载请表明出处：http://www.cnblogs.com/zxouxuewei/
我们现在准备用move_base简单的移动机器人记住，一个“pose”在ros的意思是一个位置和方向。
1.首先启动turtlebot机器人。
roslaunch rbx1_bringup fake_turtlebot.launch
2.在另一个终端运行：
roslaunch rbx1_nav fake_move_base_blank_map.launch
3.打开rviz视图查看机器人。
rosrun rviz rviz -d `rospack find rbx1_nav`/nav.rviz
4.运行move_base_square.py脚本移动机器人。
rosrun rbx1_nav move_base_square.py
5.rviz视图如下：
6.rqt_graph节点框图如下：
7.节点move_base_square.py代码如下：
#!/usr/bin/env python import rospy import actionlib from actionlib_msgs.msg import * from geometry_msgs.msg import Pose, Point, Quaternion, Twist from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from tf.transformations import quaternion_from_euler from visualization_msgs.msg import Marker from math import radians, pi class MoveBaseSquare(): def __init__(self): rospy.init_node('nav_test', anonymous=False) rospy.on_shutdown(self.shutdown) # How big is the square we want the robot to navigate? square_size = rospy.get_param("~square_size", 1.0) # meters # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi/2, pi, 3*pi/2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses waypoints = list() # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0])) waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1])) waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2])) waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) # Initialize the visualization markers for RViz self.init_markers() # Set a visualization marker at each waypoint for waypoint in waypoints: p = Point() p = waypoint.position self.markers.points.append(p) # Publisher to manually control the robot (e.g. to stop it, queue_size=5) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") rospy.loginfo("Starting navigation test") # Initialize a counter to track waypoints i = 0 # Cycle through the four waypoints while i < 4 and not rospy.is_shutdown(): # Update the marker display self.marker_pub.publish(self.markers) # Intialize the waypoint goal goal = MoveBaseGoal() # Use the map frame to define goal poses goal.target_pose.header.frame_id = 'map' # Set the time stamp to "now" goal.target_pose.header.stamp = rospy.Time.now() # Set the goal pose to the i-th waypoint goal.target_pose.pose = waypoints[i] # Start the robot moving toward the goal self.move(goal) i += 1 def move(self, goal): # Send the goal pose to the MoveBaseAction server self.move_base.send_goal(goal) # Allow 1 minute to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) # If we don't get there in time, abort the goal if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: # We made it! state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") def init_markers(self): # Set up our waypoint markers marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} # Define a marker publisher. self.marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=5) # Initialize the marker points list. self.markers = Marker() self.markers.ns = marker_ns self.markers.id = marker_id self.markers.type = Marker.CUBE_LIST self.markers.action = Marker.ADD self.markers.lifetime = rospy.Duration(marker_lifetime) self.markers.scale.x = marker_scale self.markers.scale.y = marker_scale self.markers.color.r = marker_color['r'] self.markers.color.g = marker_color['g'] self.markers.color.b = marker_color['b'] self.markers.color.a = marker_color['a'] self.markers.header.frame_id = 'odom' self.markers.header.stamp = rospy.Time.now() self.markers.points = list() def shutdown(self): rospy.loginfo("Stopping the robot...") # Cancel any active goals self.move_base.cancel_goal() rospy.sleep(2) # Stop the robot self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) if __name__ == '__main__': try: MoveBaseSquare() except rospy.ROSInterruptException: rospy.loginfo("Navigation test finished.")