使用数据集跑Hector_slam

  • Post author:
  • Post category:其他


流程和跑gmapping是类似的,只不过hector_slam不需要里程计数据,所以,只要数据集中有 /scan 和 /tf 就可以。

1 数据集预处理

这里我用slam benchmark 数据集,网址:

http://ais.informatik.uni-freiburg.de/slamevaluation/datasets.php

这个数据集非常小,里面真的是只有 /scan 和 /tf ,但是这个数据集的格式为.clf,需要把它转换成.bag文件才能在ros中使用。网上已经有人做了这件事情,那我就直接用了,python文件如下:

#!/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)

把它放到一个ros包中,这样rosrun就能找到它了。使用前还得把它的属性改成可执行文件:

chmod +x clf_to_rosbag.py

这个脚本接收两个参数,第一个为输入文件路径,第二个为输出文件路径。例如:

$ rosrun gmapping clf_to_rosbag.py /home/nlsde/qintianhao/benchmark/laser_benchmark/aces.clf /home/nlsde/qintianhao/benchmark/laser_benchmark/rosbag_laser_benchmark/aces.bag 

脚本执行后,就会在相应路径生成.bag文件了。


参考:

https://blog.csdn.net/u013859301/article/details/52986476

2 下载源码

这里可以省点事儿了,直接安装就行了:

$sudo apt-get install ros-indigo-hector-slam

3 写launch文件

这里我们需要写两个文件,一个是.launch的启动文件,一个是.xml的参数文件。

hector_mapping_demo.launch文件如下:

<launch>
  <!-- this launch used to run hector mapping by rosbag-->

  <param name="use_sim_time" value="true"/>
  
  <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0 0 0.0 /odom /base_footprint 100" /> 
  <node pkg="tf" type="static_transform_publisher" name="base_frame_laser" args="0 0 0 0 0 0 /base_footprint /scan 100" /> 

  <!-- hector mapping -->
  <include file="$(find turtlebot_navigation)/launch/includes/hector_mapping.launch.xml"/>
</launch>

这里需要给出两个静态的tf变换,单位为m,周期通常设为100ms。第一个是里程计到基座的转换,第二个是基座到雷达的转换。讲道理hector_slam是能够提供第一个转换的,但是在我跑的时候去掉第一个就是不行,不知道为什么。

xml文件中是hector_slam的参数,hector_slam已经给出了默认参数,在/opt/ros/indigo/share/hector_mapping/launch/mapping_default.launch中,这里我们参考默认参数,给出适合自己的数据集的参数文件hector_mapping.launch.xml:

<launch>

<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

<!-- Frame names -->
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="scan_topic"  value="scan" />
<param name="base_frame"  value="base_footprint"/>
<param name="odom_frame"  value="odom"/>

<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="false"/>

<!-- Map size / start point -->
<param name="map_resolution" value="0.05"/>
<param name="map_size" value="2048"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="laser_z_min_value" value = "-1.0" />
<param name="laser_z_max_value" value = "1.0" />
<param name="map_multi_res_levels" value="2" />

<!--
this is not include in mapping_default.launch
<param name="map_pub_period" value="2" />
<param name="laser_min_dist" value="0.4" />
<param name="laser_max_dist" value="5.5" />
<param name="output_timing" value="false" />
<param name="pub_map_scanmatch_transform" value="true" />
<param name="tf_map_scanmatch_transform_frame_name" value="scanmatcher_frame" />
-->

<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.9" />    
<param name="map_update_distance_thresh" value="0.4"/>
<param name="map_update_angle_thresh" value="0.06" />

<!-- Advertising config --> 
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="5"/>
</node>
</launch>

其实需要调整的就三个参数:scan_topic , base_frame,odom_frame, 把他们分别和launch文件和bag发布的话题对应起来就可以了。

4 运行

roscore
roslaunch turtlebot_navigation hector_mapping_demo.launch
rosbag play --clock aces.bag 
rosrun rviz rviz
rqt


参考

https://www.cnblogs.com/nowornever-L/p/5731729.html

https://www.ncnynl.com/archives/201702/1365.html

https://blog.csdn.net/dawn_jin/article/details/64127605

https://www.ncnynl.com/archives/201702/1367.html



版权声明:本文为qintianhaohao原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。