十一、二维激光SLAM建图（运行）

 

这里使用的是ROS自带的laser_scan_matcher库，使用的是CSM（Canonical Scan Matcher），是一个纯C语言实现的PLICP算法。此外，laser_scan_matcher还集成好了gmapping，使得我们可以不需要里程计，就可以用gmapping进行建图。

所述 laser_scan_matcher 功能包是增量激光扫描配准工具。该软件包允许扫描连续的 sensor_msgs / LaserScan 消息之间的匹配，并将估计的激光位置发布为 geometry_msgs / Pose2D 或 tf 变换。该包可以在没有其他传感器提供的任何测距估计的情况下使用。因此，它可以作为独立的里程计算器估算器。

可以通过雷达扫描匹配功能包laser_scan_matcher提供里程计信息（odom→base_footprint），这里会把gazebo运动仿真插件提供的 （odom→base_footprint） 转换屏蔽，直接由功能包 laser_scan_matcher 提供。

可以直接运行以下脚本文件：

#!/bin/bash

source ~/PX4_Firmware/Tools/setup_gazebo.bash ~/PX4_Firmware/ ~/PX4_Firmware/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware/Tools/sitl_gazebo
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/uav_test_ws/src/ORB_SLAM2/Examples/ROS
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/uav_test_ws/src/scan_tools/laser_scan_matcher

gnome-terminal --window -e 'bash -c "roscore; exec bash"' \
--tab -e  'bash -c " sleep 3 ;cd ~/PX4_Firmware && roslaunch px4 indoor3.launch ; exec bash"' \
--tab -e  'bash -c " sleep 6 ;roslaunch laser_scan_matcher matcher_gmapping.launch; exec bash"' \
--tab -e  'bash -c " sleep 9 ;cd ~/uav_test_ws/scripts && python laser_transfer.py iris 0 2d; exec bash"' \
--tab -e  'bash -c " sleep 12 ;cd ~/XTDrone/communication && python multirotor_communication.py iris 0; exec bash"' \
--tab -e  'bash -c " sleep 14 ;cd ~/XTDrone/control && python multirotor_keyboard_control.py iris 1 vel; exec bash"' \

或者按照下面的操作一步一步来：

启动px4仿真：（注意这里的launch文件是已经加入激光雷达坐标系和机体坐标系的变换关系，以及标明要选用带有激光雷达的机型的）

cd ~/PX4_Firmware
roslaunch px4 indoor3.launch 

然后启动PLICP：

roslaunch laser_scan_matcher matcher_gmapping.launch

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

rostopic echo /iris_0/pose2D

会输出：

theta是角度的意思（希腊字母）应该指的是无人机的方向

由于二维激光SLAM只能获取无人机水平的位置信息，因此，无人机实现定位还需要高度信息，在这里高度信息使用的是Gazebo发的高度真值。（我猜在真实的飞机当中，读取的是气压计返回的高度值，这个我们待会儿看一下这个转换的代码）

由于该坐标系与无人机的坐标系不同，因此需要对应的坐标变换并转为对应的mavros话题。

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

INFO  [ecl/EKF] 3324000: reset position to ev position
INFO  [ecl/EKF] 3324000: starting vision pos fusion
INFO  [ecl/EKF] 3324000: starting vision yaw fusion

 

>>>未完待续...