本文所用的源码地址:https://github.com/ros-perception/pointcloud_to_laserscan.git
ROS官网的说明: http://wiki.ros.org/pointcloud_to_laserscan
激光SLAM算法种类比较多:
其中就不乏人们所常用的:Hector,Karto,gmapping,cartographer;其中Hector算法只需激光雷达就能建图,给没有里程计的小伙伴带去福利(我也是迫于此),建图效果也是依次递增的;我此次使用的激光雷达是北科天绘的R—Fans的16线激光雷达,官方自带的驱动是两个计算node,如下图,
最终得到使用的是/rfans_driver/rfans_points话题,此话题类型sensor_msgs/PointCloud2,为三维的点云数据,而上面的算法的输入都是二维的sensor_msgs/LaserScan这就需要我们增加一个新的ROS节点来订阅sensor_msgs/PointCloud2类型的/rfans_driver/rfans_points话题,然后发布一个sensor_msgs/LaserScan类型的/scan话题
pointcloud_to_laserscan包
包里有两个cpp的node对应的两个launch文件,我只用到了sample_node.launch对应的节点为pointcloud_to_laserscan_node
launch文件需要修改:
<?xml version="1.0"?>
<launch>
<!-- run pointcloud_to_laserscan node -->
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="/rfans_driver/rfans_points"/>
<rosparam>
transform_tolerance: 0.01
min_height: 0.0
max_height: 1.0
angle_min: -3.14159 # -M_PI/2
angle_max: 3.14159 # M_PI/2
angle_increment: 0.0087 # M_PI/360.0
scan_time: 10
range_min: 0.05
range_max: 30.0
use_inf: true
inf_epsilon: 1.0
concurrency_level: 1
</rosparam>
</node>
</launch>
上面的参数修改之处可以对照github源码主要的一处改动就是:
<remap from="cloud_in" to="/rfans_driver/rfans_points"/>
改为你自己的三维点云数据topic
今后在你启动激光雷达之后启动此launch文件或将此launch文件include到激光雷达启动文件里面就能完成数据的转换,中间的rosparam参数可以自己根据要求自由改动以更加适合自己的需要。
附上加入转换节点后的计算图:
以及rviz的显示图:
我的下一文将介绍利用hector建立2D地图,场地为实验室。
note:
在使用pointcloud_to_laserscan将多线激光降为1线时,程序里面发布的topic(/scan)是没有时间戳的(header.stamp)所以在跑gmapping时出现 slam_gmapping stop in Registering First Scan problem也就是注册第一帧后程序不动了,然后rviz里面的地图也不更新,然后我找到、scan消息发现无时间戳,遂在pointcloud_to_laserscan_nodelet.cpp程序里面加上一句
output.header.stamp = ros::Time::now()
即可顺利建图。
你问我为什么要用16线激光建2维图?实验室太穷只有多线而无1线,只能勉强降低身份。
你问我建图效果咋样?其实除了容易出错,我目前发现还是差不多,缺点就是扫描频率相对40Hz的要求还是达不到,所以hector_slam旋转时容易跑偏,其他的都还不错。