ROS机器人Diego 1#制作（五）base controller—角速度的标定详解架构师

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

#!/usr/bin/env python 
import rospy 
from geometry_msgs.msg import Twist, Quaternion 
from nav_msgs.msg import Odometry 
import tf 
from math import radians, copysign 
from transform_utils import quat_to_angle, normalize_angle 
import PyKDL 
from math import pi 
class 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#履带机器人