机器人车技术开发分享网
ros 机器人角速度校准时一直转个不停,原因怎么查找
  • 首页 > 智能小车
  • 作者:小v
  • 2020年4月16日 14:03 星期四
  • 浏览:1036
  • 字号:
  • 评论:0
  • 我们对ros机器人进行校准时的指令一般是下面这样


    rosrun xxx  calibrate_angular.py
    rosrun rqt_reconfigure rqt_reconfigure
    然后出现一个窗口,通过单击 start_test 启动测试,等待小车自动旋转一个角度后,会停止。这个时候重新设定参数再来一遍。


    校准的时候发现旋转不停。这里提供一种解决的思路。

    首先打开  calibrate_angular.py

    分析校准的原理

    #!/usr/bin/env python
    
    """ calibrate_angular.py - Version 1.1 2013-12-20
    
        Rotate the robot 360 degrees to check the odometry parameters of the base controller.
    
        Created for the Pi Robot Project: http://www.pirobot.org
        Copyright (c) 2012 Patrick Goebel.  All rights reserved.
    
        This program is free software; you can redistribute it and/or modify
        it under the terms of the GNU General Public License as published by
        the Free Software Foundation; either version 2 of the License, or
        (at your option) any later version.5
        
        This program is distributed in the hope that it will be useful,
        but WITHOUT ANY WARRANTY; without even the implied warranty of
        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
        GNU General Public License for more details at:
        
        http://www.gnu.org/licenses/gpl.html
          
    """
    
    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 rikirobot_nav.cfg import CalibrateAngularConfig
    import tf
    from math import radians, copysign
    from rikirobot_nav.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
                    #只有当error的绝对值>1 初始时为360 
                    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   获取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  self.test_angle=360 再次求出差值,如果旋转速度过快,导致error直接越过1 则会导致一直转动
                        error = self.test_angle - turn_angle
    
                        # Store the current angle for the next comparison
                        last_angle = self.odom_angle
                                        
                    # Stop the robot  正常情况下 当error的绝对值小于1时 停止旋转。
                    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.")
    


    这个文件实现了 如何下发指令,并检监测数据是否收到。

    这里可以查看loginfor,位置位于

     /home/rikirobot/.ros/log/ 

    微信图片_20200416151510.jpg

    通过降低速度后发现还是不行,怎么办?只有加入打印跟踪。


    #!/usr/bin/env python
    
    """ calibrate_angular.py - Version 1.1 2013-12-20
    
        Rotate the robot 360 degrees to check the odometry parameters of the base controller.
    
        Created for the Pi Robot Project: http://www.pirobot.org
        Copyright (c) 2012 Patrick Goebel.  All rights reserved.
    
        This program is free software; you can redistribute it and/or modify
        it under the terms of the GNU General Public License as published by
        the Free Software Foundation; either version 2 of the License, or
        (at your option) any later version.5
        
        This program is distributed in the hope that it will be useful,
        but WITHOUT ANY WARRANTY; without even the implied warranty of
        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
        GNU General Public License for more details at:
        
        http://www.gnu.org/licenses/gpl.html
          
    """
    
    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 rikirobot_nav.cfg import CalibrateAngularConfig
    import tf
    from math import radians, copysign
    from rikirobot_nav.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
                    rospy.loginfo("tste angle %s is %s",self.test_angle,turn_angle)                
                    # Alternate directions between tests
                    reverse = -reverse
                    rospy.loginfo("error is %s",error)
                    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()
    
                        rospy.loginfo("self.odom_angle is %s",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 is %s",delta_angle)
                        # Add to our total angle so far
                        turn_angle += delta_angle
    		    rospy.loginfo("turn_angle is %s",turn_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.")
    



    rikirobot@robot:~/catkin_ws/src/rikirobot_project$ rosrun  rikirobot_nav calibrate_angular.py 
    [INFO] [1587270684.761507]: Bring up rqt_reconfigure to control the test.
    [INFO] [1587270690.270457]: tste angle 6.28318530718 is 0
    [INFO] [1587270690.273641]: error is 6.28318530718
    [INFO] [1587270690.277238]: self.odom_angle is 2.27587868599
    [INFO] [1587270690.279302]: delta_angle is 1.58107604968e-05
    [INFO] [1587270690.281146]: turn_angle is 1.58107604968e-05
    [INFO] [1587270690.327256]: self.odom_angle is 2.27593487196
    [INFO] [1587270690.329269]: delta_angle is 5.61859701995e-05
    [INFO] [1587270690.331225]: turn_angle is 7.19967306964e-05
    [INFO] [1587270690.377461]: self.odom_angle is 2.27594267309
    [INFO] [1587270690.379743]: delta_angle is 7.80113725352e-06
    [INFO] [1587270690.382216]: turn_angle is 7.97978679499e-05
    [INFO] [1587270690.427329]: self.odom_angle is 2.27599327817
    [INFO] [1587270690.429264]: delta_angle is 5.06050736719e-05
    [INFO] [1587270690.431127]: turn_angle is 0.000130402941622
    [INFO] [1587270690.477346]: self.odom_angle is 2.27608393176
    [INFO] [1587270690.479301]: delta_angle is 9.06535880314e-05
    [INFO] [1587270690.481722]: turn_angle is 0.000221056529653
    [INFO] [1587270690.527329]: self.odom_angle is 2.27609447613
    [INFO] [1587270690.529322]: delta_angle is 1.05443740677e-05
    [INFO] [1587270690.531191]: turn_angle is 0.000231600903721
    [INFO] [1587270690.577436]: self.odom_angle is 2.27612682199
    [INFO] [1587270690.579777]: delta_angle is 3.23458596454e-05
    [INFO] [1587270690.581642]: turn_angle is 0.000263946763366
    [INFO] [1587270690.627324]: self.odom_angle is 2.27569826433
    [INFO] [1587270690.629390]: delta_angle is -0.000428557656077
    [INFO] [1587270690.631166]: turn_angle is -0.00016461089271
    [INFO] [1587270690.677529]: self.odom_angle is 2.27540951025
    [INFO] [1587270690.679394]: delta_angle is -0.000288754082085
    [INFO] [1587270690.681085]: turn_angle is -0.000453364974796
    [INFO] [1587270690.727276]: self.odom_angle is 2.27419613828
    [INFO] [1587270690.729337]: delta_angle is -0.00121337197386
    [INFO] [1587270690.731225]: turn_angle is -0.00166673694866
    [INFO] [1587270690.777925]: self.odom_angle is 2.27048576152
    [INFO] [1587270690.779856]: delta_angle is -0.00371037675479
    [INFO] [1587270690.781846]: turn_angle is -0.00537711370345
    [INFO] [1587270690.827456]: self.odom_angle is 2.2653054
    [INFO] [1587270690.830471]: delta_angle is -0.00518036152142
    [INFO] [1587270690.832685]: turn_angle is -0.0105574752249
    [INFO] [1587270690.877331]: self.odom_angle is 2.2617987231
    [INFO] [1587270690.879067]: delta_angle is -0.00350667689925
    [INFO] [1587270690.880935]: turn_angle is -0.0140641521241
    [INFO] [1587270690.927225]: self.odom_angle is 2.24932958911
    [INFO] [1587270690.928904]: delta_angle is -0.012469133993
    [INFO] [1587270690.930480]: turn_angle is -0.0265332861171
    [INFO] [1587270690.977487]: self.odom_angle is 2.24383826094
    [INFO] [1587270690.979468]: delta_angle is -0.00549132817027
    [INFO] [1587270690.981143]: turn_angle is -0.0320246142873
    [INFO] [1587270691.027322]: self.odom_angle is 2.22514174272
    [INFO] [1587270691.029078]: delta_angle is -0.0186965182223
    [INFO] [1587270691.031013]: turn_angle is -0.0507211325096
    [INFO] [1587270691.077301]: self.odom_angle is 2.21103480165
    [INFO] [1587270691.079068]: delta_angle is -0.0141069410667
    [INFO] [1587270691.080701]: turn_angle is -0.0648280735763
    [INFO] [1587270691.127345]: self.odom_angle is 2.19344286341
    [INFO] [1587270691.129115]: delta_angle is -0.0175919382418
    [INFO] [1587270691.131010]: turn_angle is -0.0824200118181
    [INFO] [1587270691.177918]: self.odom_angle is 2.1820921159
    [INFO] [1587270691.179652]: delta_angle is -0.0113507475118
    [INFO] [1587270691.181476]: turn_angle is -0.0937707593299
    [INFO] [1587270691.227337]: self.odom_angle is 2.16327446023
    [INFO] [1587270691.229317]: delta_angle is -0.0188176556693
    [INFO] [1587270691.230929]: turn_angle is -0.112588414999
    [INFO] [1587270691.277709]: self.odom_angle is 2.12958469544
    [INFO] [1587270691.281415]: delta_angle is -0.033689764782
    [INFO] [1587270691.283838]: turn_angle is -0.146278179781
    

    可见转了好几圈才发现这个值增到了1度,所以是参数设置不对问题。


    欢迎加群探讨


    微信图片_20191202151440.png

      您阅读这篇文章共花了:  
     本文无需标签!
    二维码加载中...
    本文作者:小v      文章标题: ros 机器人角速度校准时一直转个不停,原因怎么查找
    本文地址:http://blog.cvosrobot.com/?post=481
    版权声明:若无注明,本文皆为“机器人车技术开发分享网”原创,转载请保留文章出处。

    返回顶部| 首页| 手气不错| 捐赠支持| 自定义链接| 自定义链接| 自定义链接| 手机版本|后花园

    Copyright © 2014-2017 机器人车技术开发分享网   京ICP备14059411 Copyright 2014-2019 小v工作室 版权所有 All Rights Reserved

    sitemap