一、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
启动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仿真完成！祝贺您又完成了一个仿真！
