Documentation/P600 UAV WikiEnglish · Allspark2
Browse documentation
Additional documentation

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.

3 min read · English documentation
  • 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.
Note:
  1. This demo can create maps indoors and outdoors.
  2. When using MID360 outdoors, select a location with plenty of reference objects.
  3. The following tutorial assumes that the communication link, RTK positioning data, and Prometheus ground station are ready.
  4. Place the UAV facing due east to keep the initial SLAM map coordinates approximately aligned with the RTK coordinates.

Check Whether the UAV Status Is Normal

RTK Positioning

  • Connect to the UAV in Connection Settings (the positioning source must be switched to RTK positioning).
  1. The status bar displays the current UAV status (whether the UAV flight controller is connected, the current flight mode, control status, RTK positioning status, whether the UAV is armed, etc.).
  2. The x, y, and z positions should be around 0, indicating that the positioning data is normal.
  3. A red Prometheus status indicator means that the status is abnormal. The figure below shows a normal status.

image.png

Check the Data

  1. It is normal for the initial position data to be close to 0.
  2. An excessive error in the initial position data, greater than 0.5, is abnormal (usually caused by an initial data-fusion error or accumulated error after multiple flights).

RTK positioning is normal

image.png

Note:
1. The following two scripts, ekf2 stop and ekf2 start, reinitialize EKF2 fusion for the flight controller's position data when the initial data is abnormal or the error is excessive under RTK positioning.
2. Run these scripts while checking the data before takeoff. Do not click or use them during flight, as doing so may cause loss of positioning and a crash. (This will be updated later to prevent accidental activation; refer to the official release notes for the applicable behavior.)

Initial Data

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

image.png

Start the Fast_Lio Mapping Script Function Demo

Start the Fast_Lio Mapping Script

  1. Under Function Scripts, click MID360 LiDAR Driver (the LiDAR driver must be started).
  • The onboard command is: /home/amov/p600_experiment/src/p600_experiment/scripts/mid360_location.sh
  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
  1. A message confirming that the corresponding function demo has started appears.
  2. Point-cloud data appears in rviz on the onboard computer.

image.png

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

image.png

  1. 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).

1728618684790.png

Take Off

  1. Move SW1 to the bottom position to arm the UAV. SW2, SW3, and SW4 should be in the top position at this time.
  2. The current Prometheus control status is INIT, which corresponds to the flight controller's POSITION mode.
  3. By default, the UAV begins mapping after takeoff. You can connect rviz on the onboard computer and ground station to view the point-cloud mapping data.

image.png

  1. After mapping is complete, the map is automatically saved in PCD format.
  2. You can view the map.

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

image.png

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. 1726818634319.png

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