二维激光SLAM( 使用Laser Scan Matcher )

一、Laser Scan Matcher安装配置

ROS自带的laser_scan_matcher库，使用的是CSM（Canonical Scan Matcher），是一个纯C语言实现的PLICP算法。此外，laser_scan_matcher还集成好了gmapping，使得我们可以不需要里程计，就可以用gmapping进行建图。
如果您使用Kinetic版的ROS，可以直接apt下载
首先，您需要确定是否已安装laser_scan_matcher

apt search laser-scan-matcher

如果显示已安装，则可以直接使用；若未按照，则用apt install安装

sudo apt install ros-noetic-laser-scan-matcher

配置launch文件和rviz文件，注意对应好自己的ros版本

sudo cp ~/XTDrone/sensing/slam/laser_slam/laser_scan_matcher/launch/* /opt/ros/kinetic/share/laser_scan_matcher/demo/
sudo cp ~/XTDrone/sensing/slam/laser_slam/laser_scan_matcher/rviz/* /opt/ros/kinetic/share/laser_scan_matcher/demo/

如果您使用的Melodic版的ROS，需要从源码编译，请参考

Can't build laser_scan_matcher for melodic · Issue #63 · CCNYRoboticsLab/scan_tools · GitHub

或https://medium.com/@k3083518729/robot-localization-using-laser-scanner-and-pose-graph-optimization-fc40605bf5bc

然后把~/XTDrone/sensing/slam/laser_slam/laser_scan_matcher/中的launch文件和rviz文件对应复制到catkin_ws/src/scan_tools/laser_scan_matcher/demo/中。

另外，还需要安装map_server功能包

sudo apt install ros-kinetic-map-server #melodic对应修改即可

二、二维激光定位

indoor3.world由墙体组成，适合二维激光SLAM。首先需要修改launch文件，一是加入激光雷达坐标系和机体坐标系的变换关系，而是要选用带有激光雷达的机型。下面给了一种方案（注意第3、4、18行）

gedit ~/PX4_Firmware/launch/indoor3.launch

<?xml version="1.0"?>
<launch>
    <node pkg="tf" type="static_transform_publisher" name="iris_0_base_link_to_laser"
    args="0.0 0.0 0.06 0.0 0.0 0.0 /base_link /iris_0/laser_2d 40" />
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
    <!-- vehicle pose -->
    <arg name="x" default="0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>
    <arg name="R" default="0"/>
    <arg name="P" default="0"/>
    <arg name="Y" default="0"/>
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/indoor3.world"/>
    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/iris_2d_lidar/iris_2d_lidar.sdf"/>

启动PX4仿真，

cd ~/PX4_Firmware
roslaunch px4 indoor3.launch 

在另一个终端启动PLICP

roslaunch laser_scan_matcher matcher.launch 

下图的激光和gitee主页上的动画有差异，因为激光扫描的角度和距离变了，这些可以根据您的要求修改，详见2d激光雷达模型。

 这时激光SLAM将发布二维定位话题

rostopic echo /iris_0/pose2D

x: 0.0
y: 2.22044604925e-16
theta: 0.0
---
x: 0.0
y: 6.66133814775e-16
theta: 1.11022302463e-16
---
x: -1.66533453694e-16
y: -2.22044604925e-16
theta: 1.66533453694e-16
---

然后建立通信

cd ~/XTDrone/communication

python multirotor_communication.py iris 0

由于二维激光SLAM只能获取无人机水平的位置信息，因此，无人机实现定位还需要高度信息，在这里高度信息使用的是Gazebo发的高度真值。由于该坐标系与无人机的坐标系不同，因此需要对应的坐标变换并转为对应的mavros话题。

cd ~/XTDrone/sensing/slam/laser_slam/script/

python laser_transfer_old.py iris 0 2d

这样定位信息就通过mavros发到了PX4，可以在PX4仿真终端看到



INFO  [ecl/EKF] 19804000: reset position to ev position
INFO  [ecl/EKF] 19804000: commencing external vision position fusion
INFO  [ecl/EKF] 19804000: commencing external vision yaw fusion

控制无人机起飞

cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 1 vel

到了这一步，您已经实现了利用PLICP实现定位，下一步将建立二维占据栅格地图（Occupancy Grid Map），为二维运动规划做准备。
二维激光建图
先装一下gmapping。

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

类似上述流程，但是同时启动PLICP+gmapping，并用键盘控制飞机的运动，走过整个地图，实现完整地图的建立。注意飞机飞行速度不要太快，否则地图的一致性会不好。
 

roslaunch laser_scan_matcher matcher_gmapping.launch

rviz里默认显示了所有的frame，这样看着比较乱，咱们其实只需要观察base_link和map就好，您可以在rviz里把别的frame关闭。rviz中的坐标系，x轴对应红色（red），y轴对应绿色（green），z轴对应蓝色（blue），xyz正好对应rgb，比较容易记住。
 

当地图都建好了，通过map_server可以保存地图，得到indoor3.pgm和indoor3.yaml，为下一个示例二维运动规划提供地图。

rosrun map_server map_saver -f ~/XTDrone/motion_planning/2d/map/indoor3

在这个示例里，涉及了一些坐标系变换和话题命名问题，XTDrone已经事先把这些问题都解决了，但如果开发者想要给其他机型加上激光雷达，进行激光SLAM，则有必要深入研究一下。下面展示了这个示例的TF Tree 和 Node Graph，供开发者参考。

到此，二维激光SLAM仿真完成！祝贺您又完成了一个仿真！