1.安装g2o
git clone https://github.com/RainerKuemmerle/g2o
cd g2o-master
mkdir build
cd build
cmake ..
make
sudo make install
如果编译没有出错,就会在/usr/local/include看到g2o的包含文件,在usr/local/lib看到g2o的库文件,在usr/local/bin看到g2o的可执行文件。
2.安装Karto
cd catkin_ws/src
git clone https://github.com/ros-perception/open_karto.git
git clone https://github.com/sauravag/slam_karto_g2o
cd ..
catkin_make
catkin_make
没有报错的话,基本上就安装成功了.你可以尝试运行,但在运行之前别忘记执行下source devel/setup.bash
也可以一劳永逸,在home
下的.bashrc
末尾追加
source /home/yourusername/catkin_ws/devel/setup.bash
3.下载数据集
之前大家的教程也是,光写文字或者运行,就是不给数据集,搞得我光找数据集就找了半天,功夫不负有心人还真让我给找到了,还不少呢.
戳这里:
SLAM Benchmarking Datasets
下载说明:
进去网站后点你想下载的对应的"download log file"
然后你可能会看到变成页面一堆数据
全部复制,这就是你要的数据集
新建一个文档如data.clf,存之.注意是.clf.
4.处理数据集
这个.clf不是我们用的,我们需要把它转化为特定格式的.bag文件,然后rosbag play来使用数据集,
下面给出转化用的python代码.
先说使用方法:
在你slam_karto下创建一个script文件夹,与launch文件夹同级目录.
把下面代码创建成一个.py文件如convert.py,然后放到script中.
因为他要用到ros库所以必须保存到某个parkage的script中.
cd这个script下,然后python convert.py path/data.clf path/data.bag 转化成功
convert.py
#!/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)]]
msg.ranges.append(float(0)) #我修改了这
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)
5.测试
roscore
rosrun slam_karto slam_karto
rosbag play data.bag
rosrun rviz rviz
效果: