ros 机器人角速度校准时一直转个不停,原因怎么查找
我们对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/
通过降低速度后发现还是不行，怎么办？只有加入打印跟踪。
#!/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度，所以是参数设置不对问题。
欢迎加群探讨
