二维SLAM数据集的获取与处理

摘要

经常需要做一些SLAM算法的测试，由于直接放到机器人上太麻烦，而使用gazebo的仿真器又不能完全模拟现实世界中机器人的表现，而且算法运行效果也无法考量。选择数据集进行测试是一种比较好的解决方法。本文主要说明了如何通过wget自动化获取SLAM benchmark上的数据内容，通过python对数据集进行处理并打包成rosbag，最后再在roslaunch文件中配置nav2d_karto进行SLAM建图。

获取数据集

通过wget可以方便的下载网页上的slam数据集。执行下面的语句。

wget -r -np --accept=clf http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets.php

其中，-r表示递归的在网页中下载链接；–accept=clf表示只接受clf文件。-np表示不搜索父目录。这样下来，我们就能得到网站上所有的6个数据集。

Python数据集处理

数据集文件差不多长下面这样：

ODOM -0.000602 0.012727 -1.570796 0.000000 0.000000 0.000000 72.459800 nohost 2.280101
FLASER 180 46.61 47.26 12.51 14.71 12.16 12.26 12.21 11.71 10.46 9.46 8.66 7.91 7.31 6.81 6.36 5.96 5.61 5.36 5.06 4.81 4.56 4.41 4.21 4.06 3.86 3.76 3.66 3.46 3.41 3.26 3.16 3.11 3.01 2.91 2.86 2.76 2.71 2.64 2.59 2.54 2.54 2.44 2.39 2.34 2.34 2.29 2.24 2.14 2.14 2.14 2.09 2.09 2.09 1.99 1.99 1.94 1.94 1.89 1.97 1.90 1.92 1.94 2.09 2.14 2.21 2.26 2.36 2.46 2.51 2.66 2.86 2.96 3.16 3.36 3.56 3.81 4.06 4.36 4.76 5.26 5.76 6.21 7.21 8.36 9.91 12.11 18.76 21.76 24.61 30.36 30.31 30.31 16.41 12.71 10.11 8.96 7.31 6.41 5.66 5.16 4.66 4.36 4.41 4.71 4.71 4.76 4.81 2.86 2.66 2.56 2.46 2.31 2.21 2.11 2.06 2.01 1.94 1.84 1.84 1.84 1.89 1.94 1.94 1.94 1.99 1.99 1.99 2.04 2.09 2.14 2.14 2.19 2.24 2.29 2.29 2.34 2.39 2.39 2.51 2.46 2.36 2.31 2.36 2.36 2.41 2.41 2.51 2.56 3.16 3.31 6.91 4.56 50.00 50.00 6.21 6.16 6.06 6.06 5.96 5.96 6.01 5.86 5.81 5.76 5.81 5.76 5.71 5.71 5.61 3.31 5.51 5.56 5.56 5.51 5.51 3.01 6.51 6.36 4.41 6.31 -0.000602 0.012727 -1.570796 -0.000602 0.012727 -1.570796 72.521299 nohost 2.341600
ODOM -0.000602 0.012727 -1.570796 0.000000 0.000000 0.000000 72.569599 nohost 2.389900
FLASER 180 46.56 47.31 12.56 14.76 12.16 12.26 12.21 11.76 10.51 9.46 8.61 7.91 7.26 6.81 6.31 6.01 5.61 5.36 5.06 4.81 4.61 4.36 4.21 4.06 3.91 3.76 3.61 3.46...

我们只需要将文件中的雷达和odom数据读出即可。雷达中包含了ODOM，因此我们只解析雷达的数据便可满足要求。雷达数据格式如下：

# FLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta

这里参考了http://blog.csdn.net/fk1174/article/details/52673413，原博中的脚本tf是错的，将其改正后脚本内容如下：

parse_dataset

#!/usr/bin/env python
#coding=utf8


'''This is a converter for the Intel Research Lab SLAM dataset
   ( http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets/intel.clf )
   to rosbag'''

import rospy
import rosbag
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
import tf
import sys

def make_tf_msg(x, y, theta, t,base,base0):
    trans = TransformStamped()
    trans.header.stamp = t
    trans.header.frame_id = base
    trans.child_frame_id = base0
    trans.transform.translation.x = x
    trans.transform.translation.y = y
    q = tf.transformations.quaternion_from_euler(0, 0, theta)
    trans.transform.rotation.x = q[0]
    trans.transform.rotation.y = q[1]
    trans.transform.rotation.z = q[2]
    trans.transform.rotation.w = q[3]

    msg = TFMessage()
    msg.transforms.append(trans)
    return msg
if __name__ == "__main__":
    if len(sys.argv) < 3:
        print "请输入dataset文件名。" 
        exit()
    print "正在处理" + sys.argv[1] + "..."
    with open(sys.argv[1]) as dataset:
        with rosbag.Bag(sys.argv[2], 'w') as bag:
            i = 1
            for line in dataset.readlines():
                line = line.strip()
                tokens = line.split(' ')
                if len(tokens) <= 2:
                    continue
                if tokens[0] == 'FLASER':
                    msg = LaserScan()
                    num_scans = int(tokens[1])

                    if num_scans != 180 or len(tokens) < num_scans + 9:
                        rospy.logwarn("unsupported scan format")
                        continue

                    msg.header.frame_id = 'base_laser_link'
                    t = rospy.Time(float(tokens[(num_scans + 8)]))
                    msg.header.stamp = t
                    msg.header.seq = i
                    i += 1
                    msg.angle_min = -90.0 / 180.0 * pi
                    msg.angle_max = 90.0 / 180.0 * pi
                    msg.angle_increment = pi / num_scans
                    msg.time_increment = 0.2 / 360.0
                    msg.scan_time = 0.2
                    msg.range_min = 0.001
                    msg.range_max = 50.0
                    msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]

                    bag.write('scan', msg, t)

                    odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]]
                    tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t,'odom','base_link')
                    bag.write('tf', tf_msg, t)

                elif tokens[0] == 'ODOM':
                    odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
                    t = rospy.Time(float(tokens[7]))
                    tf_msg = make_tf_msg(0, 0, 0, t,'base_link','base_laser_link')
                    bag.write('tf', tf_msg, t)

脚本接受两个参数，输入文件路径和输出文件路径。为了实现自动化，我们写一个shell将所有的数据集转化成相应的bag文件。

generate_rosbag

#!/bin/bash
PACK_PATH=`rospack find hm_slam_benchmark`
BAG_PATH=${PACK_PATH}/bag
DATASET_PATH=${PACK_PATH}/dataset
for filename in `ls ${DATASET_PATH}`
do
    filename=${filename%.clf}
    rosrun hm_slam_benchmark parse_dataset ${DATASET_PATH}/${filename}.clf ${BAG_PATH}/${filename}.bag
done

这样一来，所有的clf都转化成了bag文件。

launch文件的编写

下一步是把launch文件启动起来。这里要注意的是，由于仿真时间与系统时间不同，因此为了使程序正常工作，必须将use_sim_time变量设为true，并为rosbag开启–clock选项发布其时间。具体内容如下。

<?xml version="1.0"?>
<launch>
        <param name="use_sim_time" value="true" />

    <node pkg="rosbag" type="play" name="rosbag" args="--clock $(find hm_slam_benchmark)/bag/intel.bag"/>


        <rosparam file="$(find hm_slam_benchmark)/param/ros.yaml"/>

        <node name="Mapper" pkg="nav2d_karto" type="mapper">
                <rosparam file="$(find hm_slam_benchmark)/param/mapper.yaml"/>
        </node>


    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hm_slam_benchmark)/param/navigation.rviz"/>

</launch>

最终效果图