用数据集跑激光slam,gmapping/karto

需要用到激光slam做底层，手头没有激光，只能鼓捣点其他办法了，跑数据集就是一个！

1，第一种办法
下载编译安装turtlebot_simulator。然后

roslaunch turtlebot_stage turtlebot_in_stage.launch

会报错：

Invalid <arg> tag: environment variable 'TURTLEBOT_STAGE_MAP_FILE' is not set. 

Google：

export TURTLEBOT_STAGE_WORLD_FILE="/opt/ros/indigo/share/turtlebot_stage/maps/stage/maze.world"
export TURTLEBOT_STAGE_MAP_FILE="/opt/ros/indigo/share/turtlebot_stage/maps/maze.yaml"

OK，然后启动键盘控制：
安装：turtlebot_teleop

启动：

roslaunch turtlebot_teleop keyboard_teleop.launch

就可以用键盘控制机器人跑了，会有Topic出来，用rosbag记录下来即可。

然后：

rosbag play turtle.bag

rosparam set use_sim_time true

rosrun gmapping slam_gmapping

运行完毕后：

rosrun map_server map_saver

2,第二种办法，首先去http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets.php下载数据集，我用的是Intel Research Lab 的数据集，保存为intel.clf。

编写把clf文件转化为rosbag文件的python脚本：

#!/usr/bin/env python


'''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

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

with open('/home/kylefan/catkin_ws/data/intel/intel.clf') as dataset:
    with rosbag.Bag('/home/kylefan/intel.bag', 'w') as bag:
        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_link'
                t = rospy.Time(float(tokens[(num_scans + 8)]))
                msg.header.stamp = t
                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,'base_link','base_laser_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(odom_x, odom_y, odom_theta, t,'odom','base_link')
                bag.write('tf', tf_msg, t)

保存为bag.py，放到ros包beginner_tutorials的scripts文件夹下，然后：

chmod +x bag.py

rosrun beginner_tutorials bag.py

就把激光和odom的数据按照真实的时间点写入到了intel.bag里了。

最后

rosbag play intel.bag

激光的数据就发布到/laser了，这样就实现模拟的作用了。

这时候想再rviz里看看，记得要在左上角Global Options里面设置/odom为fixed的：

参考大神：http://answers.ros.org/question/233042/in-ros-gmapping-how-to-use-intel-research-lab-dataset/

大神写的py文件tf只有base_link -> base_laser_link的变换，我自己加了odom->base_link的变换，tf变换树如下图
rosrun tf view_frames: