002--Karto_slam运行笔记

（0）参考来源：

• 主：https://zhuanlan.zhihu.com/p/350852337
• 辅：https://blog.csdn.net/qq_24894159/article/details/81265991
• 文件目录：

（1）工程下载：https://github.com/smilefacehh/Karto-Note

上图中，.py/.bag/.clf文件，是后边新添加的。

如果没有现成下载好的文件，按如下操作，效果一样 

cd ~/catkin_ws/src
git clone https://github.com/ros-perception/slam_karto
git clone https://github.com/ros-perception/open_karto
git clone https://github.com/ros-perception/sparse_bundle_adjustment
cd ..
catkin_make

（2）Freiburg大学2d激光数据集下载：http://ais.informatik.uni-freiburg.de/slamevaluation/datasets.php

选择Intel Research Lab(Seattle) ，点击"download log file"，需要等几分钟（网页圈圈转完），然后创建一个IntelResearchLab.txt文件，将全部内容复制进去，修改文件后缀后为为IntelResearchLab.clf

（3）Freiburg大学2d激光数据集处理

上述数据集，需要运行一个Python脚本将IntelResearchLab.clf转换成ros bag文件，程序如下：

新建convert_clf_bag.py文件，将如下代码段拷贝进去。

# !/usr/bin/env python
# -*- coding:utf-8 -*-

"""
原始激光数据(odom、flaser、sonar)，制作bag
"""

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):
    """
    t时刻，base0系在base系中的坐标表示：(x, y, theta)
    """
    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
    # 2d的旋转只需要一个yaw角
    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("Usage: python convert_clf_bag.py example.clf example.bag") 
        exit()
    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])

                    # 只有Intel Research Lab满足
                    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
                    # [-90,90]范围
                    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)]]
                    # Karto运行时，需要181个scan
                    msg.ranges.append(float(0))

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

                    # 机器人坐标系base_link在odom系下的坐标，来自IMU里程计、轮式里程计等
                    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]))
                    # 激光坐标系base_laser_link在机器人坐标系base_link中的坐标，这里使二者相等
                    tf_msg = make_tf_msg(0, 0, 0, t,'base_link','base_laser_link')
                    bag.write('tf', tf_msg, t)

将数据集文件IntelResearchLab.clf和python文件convert_clf_bag.py放在同一文件夹下而后运行下边的命令（python默认的是python2），即可生成主程序可用的.bag包IntelResearchLab.bag

python convert_clf_bag.py IntelResearchLab.clf IntelResearchLab.bag

（4）运行过程：

• 进入文件夹catkin_ws/src/下
• 将工程文件Karto-Note-main文件夹拷贝到src中
• 返回到文件夹catkin_ws/下，进行如下操作：

1. 在文件夹catkin_ws/下，打开终端1：catkin_make
2. 进行source：source devel/setup.bash
3. 打开ros：roscore&    （或另外新建终端打开ros：roscore  【此时省略步骤4】）
4. 按Enter键将ros切到后台
5. 运行模型：rosrun slam_karto slam_karto  （此时由于没有运行点云数据集，所以没有输出）
6. 播放数据集：在数据集所在目录下打开终端2执行：rosbag play IntelResearchLab.bag
7. 打开RVIZ：新建终端3，运行命令：rosrun rviz rviz
8. 在RVIZ软件中的File->Open Config->打开catkin_ws/src/Karto-Note-main/slam_karto/launch/karto_slam.rviz导入配置文件（或者自己根据对应话题文件进行配置）
9. 显示界面如下图：