欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

思岚A1激光雷达hector_mapping建图与定位

程序员文章站 2022-07-14 21:15:17
...

ROS中最常见的定位包还是gmapping,gmapping是需要里程计的数据也就是IMU或者视觉里程计的数据,但是目前还是仅仅在电脑中测试,并未实现树莓派与飞控通信,因此使用不用里程计数据的hector_mapping首先进行测试并熟悉ROS中建图所需要的基础技能。

首先下载好思岚A1激光雷达对应的ROS包,编译一下,然后启动rviz查看激光雷达获得的点云数据。

roslaunch rplidar_ros view_rplidar.launch

思岚A1激光雷达hector_mapping建图与定位
说明激光雷达正常工作。接着安装hector_mapping对应的包:

sudo apt-get install ros-kinetic-hector-slam

接着,写一个launch文件进行hector_mapping的设置与启动。

<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="base_frame" value="base_link" />
<param name="odom_frame" value="base_link" />


<!-- 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="512"/>
<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" />


<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.7" />    
<param name="map_update_distance_thresh" value="0.2"/>
<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"/>
<param name="scan_topic" value="scan"/>
</node>


<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /base_link /laser 100"/>


  <node pkg="rviz" type="rviz" name="rviz"
    args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>


</launch>

解释一下代码。

<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="base_frame" value="base_link" />
<param name="odom_frame" value="base_link" />

第一句使得我们能获得map与odom的坐标系变换,这样才能根据地图获得激光雷达目前在地图中的位姿;然后由于没有里程计,因此odom_frame也设置为base_link就可以了。

<param name="map_resolution" value="0.05"/>
<param name="map_size" value="512"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />

第一句设置图片分辨率,0.05m/pix;第二句为图片大小;三四句为初始点在图片中的位置,这里为中间。

<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="5"/>
<param name="scan_topic" value="scan"/>

第一句设置发布地图的服务,第二句设置读取激光雷达数据的队列长度,第三句设置从哪个topic读取激光雷达是数据。

<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /base_link /laser 100"/>

这里直接启动了一个tf中的static_transform_publisher,用来发布base_link与激光雷达的位姿关系,前三个0代表相对位移,后三个0代表转动的欧拉角,然后写出两个相对的坐标系名称,最后一个100是100ms发布一次。

static_transform_publisher具体写法格式如下,第一种为欧拉角形式:
static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
或者四元数形式:
static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms

最后就是启动rviz了。我们来看一下绕房子一圈的效果:
思岚A1激光雷达hector_mapping建图与定位
效果还可以,因为是举着的很难保证高度一致,因此同一个地方走回来的时候可能会有偏差,也就是散出去的那些地方。
我们把这个地图保存下来:
先安装map_server包:

sudo apt-get install ros-kinetic-map-server

然后保存:

rosrun map_server map_saver -f ~/my_map

然后我们用下列tf命令行看一下现在坐标系的关系:

rosrun tf view_frames
evince frames.pdf

思岚A1激光雷达hector_mapping建图与定位
我们来看一下hector_mapping运行时的节点和话题关系:

rqt-graph

思岚A1激光雷达hector_mapping建图与定位
我们可以看到hector_mapping订阅了激光雷达节点发布的/scan话题和base_link与laser的tf,发布了base_link与map的tf,还发布了/map话题,这是一个珊格地图类型的msg,还有一个姿态,这里的姿态并不是base_link相对于map的姿态,因此我们想要获取base_link的姿态,需要手动获取对应的tf:

rosrun tf tf_echo /map /base_link

思岚A1激光雷达hector_mapping建图与定位
Translation是平移量,后面分别为四元数与欧拉角形式的转动量。致此,我们完成了用hector_mapping完成建图并且获取当前激光雷达在图坐标系中的位姿信息。