原创博客：转载请表明出处：http://www.cnblogs.com/zxouxuewei/
1.到目前为止，我们已经从命令行移动机器人，但大多数时间你将依靠一个ros节点发布适当的Twist消息。作为一个简单的例子，假设你想让你的机器人向前移动一个1米大约180度，然后回到起点。我们将尝试完成这项任务，这将很好地说明不同层次的ros运动控制。
启动tulterbot机器人：
roslaunch rbx1_bringup fake_turtlebot.launch
2.在rviz视图窗口查看机器人：
rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
3.运行timed_out_and_back.py节点：
rosrun rbx1_nav timed_out_and_back.py
4.通过rqt_graph查看消息订阅的框图：
rosrun rqt_graph rqt_graph
5.分析timed_out_and_back.py节点代码：
#!/usr/bin/env python import rospy from geometry_msgs.msg import Twist from math import pi class OutAndBack(): def __init__(self): # Give the node a name rospy.init_node('out_and_back', anonymous=False) # Set rospy to execute a shutdown function when exiting rospy.on_shutdown(self.shutdown) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1) # How fast will we update the robot's movement? rate = 50 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the forward linear speed to 0.2 meters per second linear_speed = 0.2 # Set the travel distance to 1.0 meters goal_distance = 1.0 # How long should it take us to get there? linear_duration = goal_distance / linear_speed # Set the rotation speed to 1.0 radians per second angular_speed = 1.0 # Set the rotation angle to Pi radians (180 degrees) goal_angle = pi # How long should it take to rotate? angular_duration = goal_angle / angular_speed # Loop through the two legs of the trip for i in range(2): # Initialize the movement command move_cmd = Twist() # Set the forward speed move_cmd.linear.x = linear_speed # Move forward for a time to go the desired distance ticks = int(linear_duration * rate) for t in range(ticks): self.cmd_vel.publish(move_cmd) r.sleep() # Stop the robot before the rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Now rotate left roughly 180 degrees # Set the angular speed move_cmd.angular.z = angular_speed # Rotate for a time to go 180 degrees ticks = int(goal_angle * rate) for t in range(ticks): self.cmd_vel.publish(move_cmd) r.sleep() # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Stop the robot self.cmd_vel.publish(Twist()) def shutdown(self): # Always stop the robot when shutting down the node. rospy.loginfo("Stopping the robot...") self.cmd_vel.publish(Twist()) rospy.sleep(1) if __name__ == '__main__': try: OutAndBack() except: rospy.loginfo("Out-and-Back node terminated.")
6.等以上节点运行完成后。可以运行下一个节点;
rosrun rbx1_nav nav_square.py
查看节点订阅框图：
7.分析nav_square.py节点的源码：
#!/usr/bin/env python import rospy from geometry_msgs.msg import Twist, Point, Quaternion import tf from rbx1_nav.transform_utils import quat_to_angle, normalize_angle from math import radians, copysign, sqrt, pow, pi class NavSquare(): def __init__(self): # Give the node a name rospy.init_node('nav_square', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the parameters for the target square goal_distance = rospy.get_param("~goal_distance", 1.0) # meters goal_angle = rospy.get_param("~goal_angle", radians(90)) # degrees converted to radians linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second angular_tolerance = rospy.get_param("~angular_tolerance", radians(2)) # degrees to radians # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() # Cycle through the four sides of the square for i in range(4): # Initialize the movement command move_cmd = Twist() # Set the movement command to forward motion move_cmd.linear.x = linear_speed # Get the starting position values (position, rotation) = self.get_odom() x_start = position.x y_start = position.y # Keep track of the distance traveled distance = 0 # Enter the loop to move along a side while distance < goal_distance and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current position (position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2)) # Stop the robot before rotating move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # Set the movement command to a rotation move_cmd.angular.z = angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 # Begin the rotation while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation (position, rotation) = self.get_odom() # Compute the amount of rotation since the last lopp delta_angle = normalize_angle(rotation - last_angle) turn_angle += delta_angle last_angle = rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # Stop the robot when we are done self.cmd_vel.publish(Twist()) def get_odom(self): # Get the current transform between the odom and base frames try: (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("TF Exception") return return (Point(*trans), quat_to_angle(Quaternion(*rot))) def shutdown(self): # Always stop the robot when shutting down the node rospy.loginfo("Stopping the robot...") self.cmd_vel.publish(Twist()) rospy.sleep(1) if __name__ == '__main__': try: NavSquare() except rospy.ROSInterruptException: rospy.loginfo("Navigation terminated.")