使用机器人构建地图
使用机器人构建地图
rosbag
rosbag可以录制一段时间的message，这是一个调试新算法时候非常有用的工具。可以帮助我们快速的分离bug。（相同的数据很容易对比出算法改进后的效果）
录制消息时候可以用record函数后面跟topic名字，例如要录制所有scan和tf topic，
rosbag record scan tf
将生成一个格式为YYYY_MM_DD_HH_mm_ss.bag，时间是开始录制的时间，如果想不想生成这种格式名字有两种方法
rosbag record -O foo.bag scan tf
rosbag record -o foo scan tf
第一个名字是foo.bag第二个格式是foo_YY_MM_DD_HH_mm_ss.bag
我们通常录制所有的topic，用-a参数
rosbag record -a
输入Ctrl+C停止录制。它有一些参数可以控制回放速度的快慢等。
使用方法
rosbag play --clock foo.bag
这将回放foo.bag记录的topic messages，如果指定多个bag会按顺序播放。--clock参数是让topic发布录制时候的clock。这在构建地图时候非常重要。
注意：clock 参数使rosbag回放时候发布当时的clock，如果同时有其它topic发布消息，可能会出一些问题，例如在Gazebo模拟器，有两个不同的clock，在构建地图时候，时间会跳跃出现，这会导致构建地图算发混乱。
所以使用rosbag的–clock参数时确保没有其它结点发布topic，一个简单的方法就是把所有模拟器关了。。。
可以查看bag文件的信息:
rosbag info foo.bag
会展示一些录制时间和录制topic的信息。
构建地图
构建地图用到gmapping包中的slam_gmapping结点，算法是Rao-Blackwellized(粒子概率假设密度算法)，保持机器人跟踪位置。
首先我们要生成构建地图的数据，虽然可以在世界里移动机器人得到数据，这里我们采用另一种方法，我们用rosbag保存驾驶机器人存储的数据，然后回放让slam_gmapping为我们构建地图。这是一个好的主意，可以让我们的slam_gmapping结点每次都得到一个好的地图，不用每次都操作小车走。
首先，启动模拟器:
roslaunch turtlebot_stage turtlebot_in_stage.launch
启动了Stage机器人在rviz中
使用键盘控制它，启动写好的launch文件
roslaunch turtlebot_teleop keyboard_teleop.launch
然后我们需要收集一些传感器信息，slam_gmapping构建地图需要laser和odometry系统，odometry用tf报告，我已我们订阅/scan和/tf
rosbag record -O data.bag /scan /tf
现在可以驾驶机器人走了，但是嫌麻烦，我这里用的是导航功能走的。。。录制完成后用Ctrl+C停止录制，用rosbag info data.bag可以看到录制信息
在构建地图之前要把模拟器关掉，因为模拟器发布的激光数据和要回放的冲突。我们现在来构建地图，启动roscore，在另一个termianl中告诉ROS我们要使用录制的bag文件，并启动slam_gmapping结点
rosparam set use_sim_time true
rosrun gmapping slam_gmapping
如果你的激光topic不叫scan，需要重映射。我们开始回放消息
rosbag play --clock data.bag
这是slam_gmapping应该输出一堆信息，当回放完成后slam_gmapping也停止输出。地图也构建完了，我们需要保存地图,不停止slam_gmapping运行：
rosrun map_server map_saver
会在硬盘上存储map.pgm和map.yaml,打开图片
出现转弯墙面错位的情况，地图很差劲，一个原因是传感器不能很好地创建地图，slam_gmapping期待/scan消息，turtlebot没有激光测距仪，他用的数据是由Microsoft Kinect传感器合成的。问题是假的激光测距仪相对于一般激光测距仪范围小而窄，slam_gmapping使用激光数据来估计robot如何移动，并且没有里程计来纠正误差，所以效果很差。（现实中也可以用3dcamera模拟激光数据，例子 点击跳转 )
我们可以设置一些gmapping参数改善mapping的质量。
rosparam set /slam_gmapping/angularUpdate 0.1
rosparam set /slam_gmapping/linearUpdate 0.1
rosparam set /slam_gmapping/lskip 10
rosparam set /slam_gmapping/xmax 10
rosparam set /slam_gmapping/xmin -10
rosparam set /slam_gmapping/ymax 10
rosparam set /slam_gmapping/ymin -10
gmapping参数介绍:
//启动slam的节点
//地图更新的一个间隔，两次scanmatch的间隔，地图更新也受scanmach的影响，如果scanmatch没有成功的话，是不会更新地图的 //set maxUrange < maximum range of the real sensor <= maxRange
optimize机器人移动的初始值（距离） //optimize机器人移动的初始值（角度） //icp的迭代次数
//为0,表示所有的激光都处理，尽可能为零，如果计算压力过大，可以改成1 //很重要，判断scanmatch是否成功的阈值，过高的话会使scanmatch失败，从而影响地图更新速率 //以下四个参数是运动模型的噪声参数
//机器人移动linearUpdate距离，进行scanmatch
机器人选择angularUpdate角度，进行scanmatch
//很重要，粒子个数
//map初始化的大小
重要参数说明: 1. particles (int, default: 30) gmapping算法中的粒子数，因为gmapping使用的是粒子滤波算法，粒子在不断地迭代更新，所以选取一个合适的粒子数可以让算法在保证比较准确的同时有较高的速度。
- minimumScore (float, default: 0.0) 最小匹配得分，这个参数很重要，它决定了对激光的一个置信度，越高说明对激光匹配算法的要求越高，激光的匹配也越容易失败而转去使用里程计数据，而设的太低又会使地图中出现大量噪声，所以需要权衡调整。
我们要改善质量需要慢一点驾驶机器人，尤其是在机器人转弯的时候。我们在启动gmapping前将上面的参数设置，会得到一个比较不错的地图。注意参数是如何影响slam_gmapping的你可以两次使用同一个bag文件，看看构建地图的效果对比。你可以改变不同的参数，然后用同一个bag做对比，找到合适的效果。
启动Map Server并查看map
rosrun map_server map_server map.yaml
map.yaml就是我们刚才保存的map文件，运行map server会发布两个topic，map和 map_metadata，map发布的messages类型是 nav_msgs/OccupancyGrid对应map本身。map_metadata发布messages类型是nav_msgs/MapMetaData，对应YAML文件
$ rostopic echo /map_metadata
map_load_time:
secs: 0
nsecs: 0
resolution: 0.0500000007451
width: 4000
height: 4000
origin:
position:
x: -100.0
y: -100.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
有4000*4000个格子，每个格子是5cm，下面是坐标系圆点的位置和角度，我们可以用rviz看到，可以用如下格式命令
roscd mapping/maps
rosrun map_server mapserver willow.yaml
也就是切换到有地图的yaml文件目录中运行mapserver，然后启动rviz就可以看到地图
rviz rviz
注意要监听map消息。这样就可以看到地图在rviz上显示了