FAST-LIO 3D SLAM Mapping

• Familiarity with indoor and outdoor position-hold flight is required first. This demo is suitable for both indoor and outdoor flight.
• The following demonstration uses Windows.

Check Whether the UAV Status Is Normal

Start MID360 Positioning

• Connect to the UAV in Connection Settings (the positioning source must be switched to MID360 positioning).

1. Under Function Scripts, click P600_MID360 Positioning.

• The onboard command is: /home/amov/p600_experiment/src/p600_experiment/scripts/mid360_location.sh

2. The status bar displays the current UAV status (whether the UAV flight controller is connected, the current flight mode, control status, MID360 positioning status, whether the UAV is armed, etc.).
3. A red Prometheus status indicator means that the status is abnormal. The figure below shows a normal status.
4. The x, y, and z positions should be around 0, indicating that the positioning data is normal.

Check the Data

Handling Abnormal Data

• It is normal for the initial position data to be close to 0.
• If the initial position data has an excessive error greater than 0.5, it is abnormal. Perform the EKF2 reset below (this is usually caused by an initial data-fusion error or accumulated error after multiple flights).

MID360 positioning is normal

Initial Data

1. Click Function Scripts in the ground station.
2. Click ekf2 stop.
3. Click ekf2 start.

Start the Fast_Lio Mapping Script Function Demo

Start the Fast_Lio Mapping Script

1. Under Function Scripts, click P600_FIST_LIO Mapping.

• The onboard command is: /home/amov/p600_experiment/src/p600_experiment/scripts/Fast_lio_mapping.sh

2. A message confirming that the corresponding function demo has started appears.
3. Point-cloud data appears in rviz on the onboard computer.

1. Click System Settings in the ground station.
2. Click Basic Settings.
3. Click Other.
4. Enable rviz visualization.

5. The point-cloud data image is displayed in the ground station. If it is not displayed, disable the ground station's rviz display and then enable it again (Step 4).

1. Use the remote controller to fly the UAV and create a map by sensing the environment.
2. After mapping is complete, the map is automatically saved in PCD format.
3. You can view the map.

PCD map save path: ~/p600_experiment/src/Prometheus/Modules/FAST_LIO/PCD

View the Map

Open a terminal and enter pcl_viewer /home/amov/p600_experiment/src/Prometheus/Modules/FAST_LIO/PCD/scans.pcd . With the map selected, press 1, 2, 3, or 4 to display it in different colors.

Function Parameters

fast_lio Mapping

• The script used by this function is:

`/home/amov/p600_experiment/src/p600_experiment/scripts/Fast_lio_mapping.sh`

• The script contents are as follows:

gnome-terminal --window -e 'bash -c "sleep 1; roslaunch p600_experiment Fast_lio_mapping.rviz; exec bash"' \

• The contents of mapping_mid360.launch are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/launch_mid360/mapping_mid360.launch

<launch>


	<arg name="rviz" default="true" />

	<rosparam command="load" file="$(find p600_experiment)/config/mid360_config/mid360.yaml" />

	<param name="feature_extract_enable" type="bool" value="0"/>
	<param name="point_filter_num" type="int" value="3"/>
	<param name="max_iteration" type="int" value="3" />
	<param name="filter_size_surf" type="double" value="0.5" />
	<param name="filter_size_map" type="double" value="0.5" />
	<param name="cube_side_length" type="double" value="1000" />
	<param name="runtime_pos_log_enable" type="bool" value="0" />

	<param name="filter_range_x" type="double" value="25" />
	<param name="filter_range_y" type="double" value="25" />
	<param name="filter_range_z" type="double" value="0.5" />

	<remap from = "/cloud_registered" to = "/uav1/mid_point_cloud_centers"/>
	<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
	<node pkg="tf" type="static_transform_publisher" name="tf_MID360" args="0.0 0.0 0.0 0.0 0.0 0.0 world camera_init 100"/>
	<node pkg = "fast_lio" type="fliter_points" name="fliter_points" output="screen" />
	
</launch>

• The contents of mid360.yaml are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/config/mid360_config/mid360.yaml

common:
    lid_topic:  "/livox/lidar"   # LiDAR topic
    imu_topic:  "/livox/imu"     # IMU topic
    time_sync_en: false         # Whether to synchronize time
    time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
                                  # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0

preprocess:
    lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,  # LiDAR type
    scan_line: 4
    blind: 0.6                   # Point-cloud exclusion range

mapping:
    acc_cov: 0.1
    gyr_cov: 0.1
    b_acc_cov: 0.0001
    b_gyr_cov: 0.0001
    fov_degree:    360
    det_range:     100.0
    extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
    extrinsic_T: [ -0.011, -0.02329, 0.04412 ]    # Translation matrix between the IMU and the UAV center position
    extrinsic_R: [ 1, 0, 0,                       # Rotation matrix
                   0, 1, 0,
                   0, 0, 1]

publish:
    path_en:  false              # Whether to publish the LiDAR path
    scan_publish_en:  true       # Whether to publish point-cloud data
    dense_publish_en: true       # Whether to publish point-cloud data
    scan_bodyframe_pub_en: true  # Whether to output the point cloud to the LiDAR IMU coordinate frame

pcd_save:
    pcd_save_en: false           # Whether to save the PCD point-cloud map
    interval: -1                 # Point-cloud save method:
    				  # -1: Save all point clouds to one PCD file; 1: Save each frame's point cloud as a separate PDCD file