原创博客：转载请表明出处：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 pythonimport rospyfrom geometry_msgs.msg import Twistfrom math import piclass 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 pythonimport rospyfrom geometry_msgs.msg import Twist, Point, Quaternionimport tffrom rbx1_nav.transform_utils import quat_to_angle, normalize_anglefrom math import radians, copysign, sqrt, pow, piclass 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.")