前言
最近新进入一家公司开始从事AGV产品的研发，本次主要介绍下线速度和角度的标定实现
新建功能包calibration
mkdir calibration
设置CMakeLists.txt,package.xml和setup.py
CMakeLists.txt如下所示：
cmake_minimum_required(VERSION 2.8.3)
project(calibration)
find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure)
catkin_python_setup()
generate_dynamic_reconfigure_options(cfg/CalibrateAngular.cfg cfg/CalibrateLinear.cfg)
catkin_package(CATKIN_DEPENDS dynamic_reconfigure)
需要用到dynamic_reconfigure ros功能包进行动态参数设置，cfg目录下为初始时刻标定角速度和线速度的配置文件，稍后进行介绍。
package.xml文件如下所示：
<?xml version="1.0"?>
<package>
<name>calibration</name>
<version>0.1.0</version>
<description>The calibration package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="1599784024@qq.com">zhoujun</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/base_server</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>actionlib</build_depend>
<build_depend>orocos_kdl</build_depend>
<run_depend>dynamic_reconfigure</run_depend>
<export>
</export>
</package>
setup.py文件如下所示：
## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
packages=['calibration'],
package_dir={'': 'src'})
setup(**setup_args)
设置cfg文件
在cfg有两个文件，分别为：CalibrateAngular.cfg和CalibrateLinear.cfg，其中CalibrateAngular.cfg如下所示：
#!/usr/bin/env python
PACKAGE = "calibration"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("test_angle", double_t, 0, "Test Angle", 360.0, 0.0, 360.0)
gen.add("speed", double_t, 0, "Angular speed in radians per second", 0.5, -1.5, 1.5)
gen.add("tolerance", double_t, 0, "Error tolerance to goal angle in degrees", 1.0, 1.0, 10.0)
gen.add("odom_angular_scale_correction", double_t, 0, "Angular correction factor", 1.0, 0.0, 3.0)
gen.add("start_test", bool_t, 0, "Check to start the test", False)
exit(gen.generate(PACKAGE, "calibrate_angular", "CalibrateAngular"))
其中"Test Angle"为要测试的角度，范围在0-360，默认为360度。"speed"为测试时的旋转速度。"tolerance"为可以接受的角度误差。"odom_angular_scale_correction"为要进行纠正的比例系数。
CalibrateLinear.cfg文件如下所示
#! /usr/bin/env python
PACKAGE = "calibration"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("test_distance", double_t, 0, "Test distance in meters", 1.0, 0.0, 2.0)
gen.add("speed", double_t, 0, "Robot speed in meters per second", 0.15, 0.0, 0.3)
gen.add("tolerance", double_t, 0, "Error tolerance to goal distance in meters", 0.01, 0.0, 0.1)
gen.add("odom_linear_scale_correction", double_t, 0, "Linear correction factor", 1.0, 0.0, 3.0)
gen.add("start_test", bool_t, 0, "Check to start the test", False)
exit(gen.generate(PACKAGE, "calibrate_linear", "CalibrateLinear"))
标定文件介绍
calibrate_angular.py为标定角速度的文件，calibrate_linear.py为标定线速度的文件
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 calibration.cfg import CalibrateAngularConfig
import tf
from math import radians, copysign
import PyKDL
from math import pi
def quat_to_angle(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return rot.GetRPY()[2]
def normalize_angle(angle):
res = angle
while res > pi:
res -= 2.0 * pi
while res < -pi:
res += 2.0 * pi
return res
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()
# 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.")
calibrate_linear.py文件如下所示：
#!/usr/bin/env python
""" calibrate_linear.py - Version 1.1 2013-12-20
Move the robot 1.0 meter to check on the PID 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, Point
from math import copysign, sqrt, pow
from dynamic_reconfigure.server import Server
import dynamic_reconfigure.client
from calibration.cfg import CalibrateLinearConfig
import tf
class CalibrateLinear():
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_linear', 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)
# Set the distance to travel
self.test_distance = rospy.get_param('~test_distance', 1.0) # meters
self.speed = rospy.get_param('~speed', 0.15) # meters per second
self.tolerance = rospy.get_param('~tolerance', 0.01) # meters
self.odom_linear_scale_correction = rospy.get_param('~odom_linear_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(CalibrateLinearConfig, self.dynamic_reconfigure_callback)
# Connect to the dynamic_reconfigure server
dyn_client = dynamic_reconfigure.client.Client("calibrate_linear", timeout=60)
# The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
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.")
self.position = Point()
# Get the starting position from the tf transform between the odom and base frames
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
move_cmd = Twist()
while not rospy.is_shutdown():
# Stop the robot by default
move_cmd = Twist()
if self.start_test:
# Get the current position from the tf transform between the odom and base frames
self.position = self.get_position()
# Compute the Euclidean distance from the target point
distance = sqrt(pow((self.position.x - x_start), 2) +
pow((self.position.y - y_start), 2))
# Correct the estimated distance by the correction factor
distance *= self.odom_linear_scale_correction
# How close are we?
error = distance - self.test_distance
# Are we close enough?
if not self.start_test or abs(error) < self.tolerance:
self.start_test = False
params = {'start_test': False}
rospy.loginfo(params)
dyn_client.update_configuration(params)
else:
# If not, move in the appropriate direction
move_cmd.linear.x = copysign(self.speed, -1 * error)
else:
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
self.cmd_vel.publish(move_cmd)
r.sleep()
# Stop the robot
self.cmd_vel.publish(Twist())
def dynamic_reconfigure_callback(self, config, level):
self.test_distance = config['test_distance']
self.speed = config['speed']
self.tolerance = config['tolerance']
self.odom_linear_scale_correction = config['odom_linear_scale_correction']
self.start_test = config['start_test']
return config
def get_position(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
return Point(*trans)
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:
CalibrateLinear()
rospy.spin()
except:
rospy.loginfo("Calibration terminated.")
152,1 底端
实际操作
编译
catkin_make
运行
第一个终端：
启动机器人底盘
第二个终端：
rosrun calibration calibrate_angular.py //标定线速度
第三个终端：
rosrun rqt_reconfigure rqt_reconfigure //启动可视化界面进行动态参数配置
打开的动态配置窗口如下所示：
勾选start_test选项就会开始进行测试
8864
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
