Ericsii / FAST_LIO_ROS2

ROS2 version of FAST_LIO2. Welcome to the technical communication discord server discord: https://discord.gg/U3B65MGH8m
GNU General Public License v2.0
141 stars 34 forks source link

Issue when saving pointcloud (Input point cloud has no data!) #14

Closed TZECHIN6 closed 3 months ago

TZECHIN6 commented 3 months ago

Environment

OS: Ubuntu 22.04 ROS: Humble ROS2 Gazebo: Gazebo Classic, LiDAR data from velodyne_simulator, VLP-16, IMU data from GazeboRosImuSensor

Description

I have setup a gazebo simulation with a small vehicle equipped with simulated lidar and imu. A virtual scene was made and rosbag was recorded. I have tried to run LIO-SAM and it just work out fine. However, when I tried to run FAST-LIO with the same rosbag. A error was popped:

[fastlio_mapping-1] [INFO] [1719452142.327284402] [laser_mapping]: p_pre->lidar_type 2
[fastlio_mapping-1] Multi thread started 
[fastlio_mapping-1] ~~~~/home/thomas/Code/FAST-LIO_ws/src/FAST_LIO_ROS2/ file opened
[fastlio_mapping-1] [INFO] [1719452142.329168004] [laser_mapping]: Node init finished.
[rviz2-2] [INFO] [1719452142.481044649] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [INFO] [1719452142.481085598] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-2] [INFO] [1719452142.491244564] [rviz2]: Stereo is NOT SUPPORTED
[fastlio_mapping-1] IMU Initial Done
[fastlio_mapping-1] [WARN] [1719452145.779175811] [laser_mapping]: No point, skip this scan!
[fastlio_mapping-1] 
[fastlio_mapping-1] [INFO] [1719452145.889551555] [laser_mapping]: Initialize the map kdtree
[fastlio_mapping-1] [INFO] [1719452184.611217013] [laser_mapping]: Saving map to ./test.pcd...
[fastlio_mapping-1] terminate called after throwing an instance of 'pcl::IOException'
[fastlio_mapping-1]   what():  : [pcl::PCDWriter::writeBinary] Input point cloud has no data!
[ERROR] [fastlio_mapping-1]: process has died [pid 8895, exit code -6, cmd '/home/thomas/Code/FAST-LIO_ws/install/fast_lio/lib/fast_lio/fastlio_mapping --ros-args --params-file /home/thomas/Code/FAST-LIO_ws/install/fast_lio/share/fast_lio/config/velodyne.yaml --params-file /tmp/launch_params_05mrs0fq'].
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)

Based on the log, it said there is no pointcloud data... I am wondering do I set the config file wrongly or what? Attached is the frame transform of my robot and sensor.

frames_2024-06-27_09.35.10.pdf

velodyne.yaml

/**:
    ros__parameters:
        feature_extract_enable: false
        point_filter_num: 4
        max_iteration: 3
        filter_size_surf: 0.5
        filter_size_map: 0.5
        cube_side_length: 1000.0
        runtime_pos_log_enable: false
        map_file_path: "./test.pcd"

        common:
            lid_topic:  "/velodyne_points"
            imu_topic:  "/imu_raw"
            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
            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: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
            scan_line: 16
            scan_rate: 10                # only need to be set for velodyne, unit: Hz,
            timestamp_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
            blind: 2.0

        mapping:
            acc_cov: 0.1
            gyr_cov: 0.1
            b_acc_cov: 0.0001
            b_gyr_cov: 0.0001
            fov_degree:    360.0
            det_range:     100.0
            extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
            extrinsic_T: [ 0., 0., 0.28]
            extrinsic_R: [ 1., 0., 0., 
                        0., 1., 0., 
                        0., 0., 1.]

        publish:
            path_en:  false
            scan_publish_en:  true       # false: close all the point cloud output
            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame

        pcd_save:
            pcd_save_en: true
            interval: -1                 # how many LiDAR frames saved in each pcd file; 
                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

Screenshot from 2024-06-27 09-50-04 Remark: The pointcloud will disappear after several seconds...

Thanks for anyone could give me a helping hand about this.

Ericsii commented 3 months ago

Please add publish.map_en: true into the config file so that the global map will be accumulated in memory. You can change 'publish' part in your config file as below:

        publish:
            map_en: true
            path_en:  false
            scan_publish_en:  true       # false: close all the point cloud output
            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
TZECHIN6 commented 3 months ago

Thanks for the reply, and it fix the issue. Just notice this parameter existed in the mid360.yaml but not the velodyne. See if you would like to update this config accordingly to the repo.

Great thanks and case close.