2D laser Slam (using laser scan matcher)

One 、Laser Scan Matcher Installation configuration

ROS Self contained laser_scan_matcher library , It uses CSM（Canonical Scan Matcher）, It's pure. C The realization of language PLICP Algorithm . Besides ,laser_scan_matcher It's integrated gmapping, So that we don't need a odometer , You can use it gmapping To build a map .
If you use Kinetic Version of ROS, Can directly apt download
First , You need to determine if... Is installed laser_scan_matcher

apt search laser-scan-matcher

If installed , Can be used directly ; If not in accordance with , Then use apt install install

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

To configure launch Document and rviz file , Pay attention to your own ros edition

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/

If you use Melodic Version of ROS, Need to compile from source code , Please refer to

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

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

And then put ~/XTDrone/sensing/slam/laser_slam/laser_scan_matcher/ Medium launch Document and rviz The file should be copied to catkin_ws/src/scan_tools/laser_scan_matcher/demo/ in .

in addition , It also needs to be installed map_server Function pack

sudo apt install ros-kinetic-map-server #melodic Corresponding modification is enough 

Two 、 Two dimensional laser positioning

indoor3.world Composed of walls , Suitable for two-dimensional laser SLAM. First you need to modify launch file , One is to add the transformation relationship between the lidar coordinate system and the body coordinate system , Instead, choose models with lidar . Here is a scheme （ Pay attention to the 3、4、18 That's ok ）

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"/>

start-up PX4 Simulation ,

cd ~/PX4_Firmware
roslaunch px4 indoor3.launch 

Start at another terminal PLICP

roslaunch laser_scan_matcher matcher.launch 

The laser and gitee The animation on the home page is different , Because the angle and distance of laser scanning have changed , These can be modified according to your requirements , See 2d Lidar model .

  Then the laser SLAM Two dimensional positioning topic will be published

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
---

Then establish communication

cd ~/XTDrone/communication

python multirotor_communication.py iris 0

Due to the two-dimensional laser SLAM Only the horizontal position information of UAV can be obtained , therefore , UAV positioning also needs altitude information , The height information used here is Gazebo The height of the hair is true . Because this coordinate system is different from the coordinate system of UAV , Therefore, the corresponding coordinate transformation is required and transformed into the corresponding mavros topic of conversation .

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

python laser_transfer_old.py iris 0 2d

So the location information passes through mavros Sent to PX4, Can be in PX4 The simulation terminal sees



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

 Control the UAV to take off 

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

Here we are , You have realized the use of PLICP Achieve positioning , The next step is to build a two-dimensional grid map （Occupancy Grid Map）, Prepare for two-dimensional motion planning .
Two dimensional laser mapping
Put it on first gmapping.

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

The above process is similar , But at the same time PLICP+gmapping, And use the keyboard to control the movement of the aircraft , Walk through the whole map , Realize the establishment of a complete map . Be careful not to fly too fast , Otherwise, the consistency of the map will be bad .
 

roslaunch laser_scan_matcher matcher_gmapping.launch

rviz By default, all frame, It looks messy , We just need to observe base_link and map Just fine , You can go to rviz Put something else in the frame close .rviz The coordinate system in ,x The axis corresponds to red （red）,y The axis corresponds to green （green）,z The axis corresponds to blue （blue）,xyz It's exactly the same as rgb, It's easier to remember .
 

When the maps are built , adopt map_server You can save the map , obtain indoor3.pgm and indoor3.yaml, For the next example Two dimensional motion planning Provide maps .

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

In this example , It involves some coordinate system transformation and topic naming problems ,XTDrone These problems have been solved in advance , But if developers want to add lidar to other models , Carry out laser SLAM, Then it is necessary to study . The following shows the... Of this example TF Tree and Node Graph, For developers to reference .

Here we are , Two dimensional laser SLAM The simulation is complete ！ Congratulations on completing another simulation ！