Turbot与python编程-实现角速度标定

Turbot与python教程-实现角速度标定

说明：

• 介绍如何实现通过python控制turbot实现角速度标定
• 针对turtlebot1或非turtlebot2机器人参考

代码：

• 参考代码：github
• 实现代码：

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, Quaternion
from nav_msgs.msg import Odometry
from dynamic_reconfigure.server import Server
import dynamic_reconfigure.client
from turbot_code.cfg import CalibrateAngularConfig
import tf
from math import radians, copysign
from turbot_code.transform_utils import quat_to_angle, normalize_angle

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 = rospy.get_param('~rate', 20)
        r = rospy.Rate(self.rate)
        
        # The test angle is 360 degrees
        self.test_angle = radians(rospy.get_param('~test_angle', 360.0))

        self.speed = rospy.get_param('~speed', 0.5) # radians per second
        self.tolerance = radians(rospy.get_param('tolerance', 1)) # degrees converted to radians
        self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)
        
        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        
        # Fire up the dynamic_reconfigure server
        dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback)
        
        # Connect to the dynamic_reconfigure server
        dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60)
        
        # 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()
                
                last_angle = self.odom_angle
                turn_angle = 0
                self.test_angle *= reverse
                error = self.test_angle - turn_angle
                                
                # Alternate directions between tests
                reverse = -reverse
                
                while abs(error) > self.tolerance and self.start_test:
                    if rospy.is_shutdown():
                        return
                    
                    # Rotate the robot to reduce the error
                    move_cmd = Twist()
                    move_cmd.angular.z = copysign(self.speed, error)
                    self.cmd_vel.publish(move_cmd)
                    r.sleep()
                 
                    # Get the current rotation angle from tf                   
                    self.odom_angle = self.get_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)
                    
                    # Add to our total angle so far
                    turn_angle += delta_angle

                    # Compute the new error
                    error = self.test_angle - turn_angle

                    # 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 dynamic_reconfigure_callback(self, config, level):
        self.test_angle =  radians(config['test_angle'])
        self.speed = config['speed']
        self.tolerance = radians(config['tolerance'])
        self.odom_angular_scale_correction = config['odom_angular_scale_correction']
        self.start_test = config['start_test']
        
        return config
        
    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.")

整合指南：

• 请参考[rbx1的indigo版本][2]的校准部分及[rbx1_nav包][3]
• 增加cfg的配置文件
• 修改CMakelists.txt 增加dynamic_reconfigure部分
• 修改package.xml 增加dynamic_reconfigure部分

演示：

• 主机，新终端，启动底盘

$ roslaunch turbot_bringup minimal.launch

• 从机，新终端，启动脚本

$ rosrun turbot_code calibrateAngular.py

• 从机，新终端，启动动态参数窗口

$ rosrun rqt_reconfigure rqt_reconfigure

• 勾选start_test，机器人开始旋转360度

• 确定偏移值的步骤：

  • 机器人正面和地下分别划定一定线，测量和记录每次旋转是否会重合
  • 记录每次两线的偏移角度
  • 进入rqt_reconfigure界面，不断修改odom_angular_scale_correction的值X
  • 重新测试，勾上start_test
  • 继续重复测试，直到符合理想状态，接近重合

• 应用偏移值步骤：

  • 修改turtlebot.launch

<param name="turtlebot_node/odom_angular_scale_correction" value="X" />

• 修改X为获取的值

• 如果是非Turtlebot机器人

  • 修改base_width(轴距), 新值=base_width旧值/odom_angular_scale_correction

参考：

• http://wiki.ros.org/turtlebot_calibration/Tutorials/Calibrate%20Odometry%20and%20Gyro