#!/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.")
第六章 SLAM
1 栅格地图的概念
占据栅格地图是一种在 SLAM 地图中相较而言创建和维护较简单的地图表达形式，使用栅格地图更容易实现导航功能。
占据栅格地图在目前二维 SLAM 中应用较为广泛，其基本原理是将机器人所在的空间划分为若干个大小相同且相互独立的栅格块，如图所示。
而对于每一个栅格块，我们给予（0，1）之间的数来表示在这个栅格内有多大的概率存在障碍。
对栅格的划分越细，其地图的精度自然也就越高。
栅格地图也是对 SLAM 建图支持度相当高的一种地图模式。
2 建图原理
2D 激光 SLAM 的建图的输入一般是里程计数据、IMU 数据、2D 激光雷达数据，得到的输出为覆盖栅格地图以及机器人的运动轨迹。目前的算法主要由两个部分执行：一部分通过数据融合、特征处理等方式将提取得的传感器数据转化为可用的数据模型；另一部分则通过概率算法等手段来对这些模型进行处理，生成环境地图。但一般来说这种直接根据传感器数据得出的地图会在某些情况下不可取。因为但凡传感器都是有误差的，而在建图的时候上一帧数据对下一帧的处理影响非常大，这就导致一旦出现误差就会在建图过程中一直累积下去。如果环境非常大，到最后就会导致所获得的的地图完全不可用。因此，回环检测是 SLAM 建图算法中非常重要的一部分。回环检测的基本原理就是将获得的一帧数据以及自己的位置信息与之前某一帧数据或是已计算出的地图进行匹配，找到能够成功匹配的帧，建立位姿约束关系，并以此约束关系为基准再去调整其他的数据，从而起到减小累积误差的效果。
要使用地图，首先需要绘制地图。关于地图建模技术不断涌现，这其中有一门称之为 SLAM 的理论脱颖而出:
SLAM(simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。SLAM问题可以描述为: 机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图。
在 ROS 中，较为常用的 SLAM 实现也比较多，比如: gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM ....
当然如果要完成 SLAM ，机器人必须要具备感知外界环境的能力，尤其是要具备获取周围环境深度信息的能力。感知的实现需要依赖于传感器，比如: 激光雷达、摄像头、RGB-D摄像头...
SLAM 可以用于地图生成，而生成的地图还需要被保存以待后续使用，在 ROS 中保存地图的功能包是 map_server
另外注意: SLAM 虽然是机器人导航的重要技术之一，但是 二者并不等价，确切的讲，SLAM 只是实现地图构建和即时定位。
关于SLAM的详细内容已在第九章中详细解释过，此处不再赘述。
1.2 自身定位
导航开始和导航过程中，机器人都需要确定当前自身的位置，如果在室外，可以使用GPS 。
但在室内、隧道、地下或一些特殊的区域，无法收到 GPS 的信号， GPS 信号被弱化甚至完全不可用，就要使用其他方法来定位，如 SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包: amcl
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<param name="base_frame_id" value="base_footprint"/>
<!-- Change this if you want to change your base frame id. -->
<param name="gui_publish_rate" value="10.0"/>
<!-- Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable. -->
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="max_particles" value="2000"/>
<param name="min_particles" value="500"/>
<param name="odom_alpha1" value="0.25"/>
<!-- Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion. -->
<param name="odom_alpha2" value="0.25"/>
<!-- Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion. -->
<param name="odom_alpha3" value="0.25"/>
<!-- Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion. -->
<param name="odom_alpha4" value="0.25"/>
<!-- Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion. -->
<param name="odom_alpha5" value="0.1"/>
<!-- Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion. -->
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff"/>
<param name="recovery_alpha_slow" value="0.001"/>
<!-- Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. -->
<param name="recovery_alpha_fast" value="0.1"/>
<!-- Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. -->
<param name="resample_interval" value="1"/>
<!-- Number of filter updates required before resampling. -->
<param name="transform_tolerance" value="1.25"/>
<!-- Default 0.1; time with which to post-date the transform that is published, to indicate that this transform is valid into the future. -->
<param name="update_min_a" value="0.2"/>
<!-- Rotational movement required before performing a filter update. 0.1 represents 5.7 degrees -->
<param name="update_min_d" value="0.2"/>
<!-- Translational movement required before performing a filter update. -->
</node>
</launch>
