摘要: 原创博客：转载请表明出处：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/, pi, *pi/, ) # Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(, , 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[]))
waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[]))
waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[]))
waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[])) # 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=)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=) # Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration()) rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test") # Initialize a counter to track waypoints
i = # Cycle through the four waypoints
while i < 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 += def move(self, goal):
# Send the goal pose to the MoveBaseAction server
self.move_base.send_goal(goal) # Allow minute to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration()) # 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 = # is forever
marker_ns = 'waypoints'
marker_id =
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=) # 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()
# Stop the robot
self.cmd_vel_pub.publish(Twist())
rospy.sleep() if __name__ == '__main__':
try:
MoveBaseSquare()
except rospy.ROSInterruptException:
rospy.loginfo("Navigation test finished.")