二維激光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仿真完成！祝賀您又完成了一個仿真！