ROS机器人Diego 1#制作（五）base controller---角速度的标定

角速度标定，即控制机器人人转动固定的角度，看机器人是否按照控制指令完成，在这里我们让机器人转360度，代码如下：

#!/usr/bin/env pythonimport rospyfrom geometry_msgs.msg import Twist, Quaternionfrom nav_msgs.msg import Odometryimport tffrom math import radians, copysignfrom transform_utils import quat_to_angle, normalize_angleimport PyKDLfrom math import piclass CalibrateAngular():    def __init__(self):        # Give the node a name        rospy.init_node('calibrate_angular', 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?        self.rate = 10        r = rospy.Rate(self.rate)        # The test angle is 360 degrees        self.test_angle = 2*pi #这里注意，在ROS中使用的弧度，不能直接写360        self.speed = 0.1 # radians per second        self.tolerance = 1 # degrees converted to radians        self.odom_angular_scale_correction = 1        self.start_test = True        # Publisher to control the robot's speed        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)        # The base frame is usually base_link or base_footprint        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)        # Make sure we see the odom and base frames        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))        rospy.loginfo("Bring up rqt_reconfigure to control the test.")        reverse = 1        while not rospy.is_shutdown():            if self.start_test:                # Get the current rotation angle from tf                self.odom_angle = self.get_odom_angle()                rospy.loginfo("self.odom_angle: "+str(self.odom_angle))                last_angle = self.odom_angle                turn_angle = 0                self.test_angle *= reverse                error = self.test_angle - turn_angle                rospy.loginfo("errir: "+str(error))                # Alternate directions between tests                reverse = -reverse                while abs(error) > self.tolerance and self.start_test:                    if rospy.is_shutdown():                        return                    rospy.loginfo("*************************** ")                    # Rotate the robot to reduce the error                    move_cmd = Twist()                    move_cmd.angular.z = copysign(self.speed, error)                    rospy.loginfo("move_cmd.angular.z: "+str(move_cmd.angular.z))                    self.cmd_vel.publish(move_cmd)                    r.sleep()                    # Get the current rotation angle from tf                                       self.odom_angle = self.get_odom_angle()                    rospy.loginfo("current rotation angle: "+str(self.odom_angle))                    # Compute how far we have gone since the last measurement                    delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)                    rospy.loginfo("delta_angle: "+str(delta_angle))                    # Add to our total angle so far                    turn_angle += abs(delta_angle)                    rospy.loginfo("turn_angle: "+str(turn_angle))                    # Compute the new error                    error = self.test_angle - turn_angle                    rospy.loginfo("error: "+str(error))                    # Store the current angle for the next comparison                    last_angle = self.odom_angle                # Stop the robot                self.cmd_vel.publish(Twist())                # Update the status flag                self.start_test = False                params = {'start_test': False}                dyn_client.update_configuration(params)            rospy.sleep(0.5)        # Stop the robot        self.cmd_vel.publish(Twist())    def get_odom_angle(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        # Convert the rotation from a quaternion to an Euler angle        return 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:        CalibrateAngular()    except:        rospy.loginfo("Calibration terminated.")

接下来运行如下命令，控制小车旋转：

rosrun diego_nav calibrate_angular.py

影响角速度的主要参数是wheel_track，所以如果发现机器人不能按照要求旋转固定角度，可以调整此参数。

可以到优酷查看已经完成线速度和角速度标定的Diego 1#履带机器人