add support for Unitree 4D LiDAR L1

.gitignore

@@ -2,3 +2,4 @@
 /cmake-build-debug/
 *.pcd
 /Log/pos_log.csv
+/build

CMakeLists.txt

@@ -45,6 +45,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(pcl_ros REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
 find_package(livox_ros_driver2 REQUIRED)
 
 find_package(Eigen3 REQUIRED)
@@ -77,6 +78,7 @@ ament_target_dependencies(pointlio_mapping
         pcl_ros
         pcl_conversions
         tf2_ros
+        visualization_msgs
         livox_ros_driver2
 )
 target_link_libraries(pointlio_mapping ${PYTHON_LIBRARIES})

README.md

@@ -88,7 +88,7 @@ Clone the repository and colcon build:
 
 ```
     cd ~/$Point_LIO_ROS_DIR$/src
-    git clone https://github.com/hku-mars/Point-LIO.git
+    git clone -b ros2 https://github.com/whyscience/Point-LIO.git
     cd Point-LIO
     git submodule update --init
     cd ../..
@@ -142,6 +142,21 @@ Edit ``` config/velodyne.yaml ``` to set the below parameters:
 7. Saturation value of IMU's accelerator and gyroscope: ```satu_acc```, ```satu_gyro```
 8. The norm of IMU's acceleration according to unit of acceleration messages: ``` acc_norm ```
 
+### 5.4 For Unitree 4D LiDAR L1
+Step A: Setup before run
+
+Edit ``` config/unilidar_l1.yaml ``` to set the below parameters:
+
+1. LiDAR point cloud topic name: ``` lid_topic ```
+2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)
+3. Set the parameter ```timestamp_unit``` 
+4. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ```
+5. Translational extrinsic: ``` extrinsic_T ```
+6. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
+- The extrinsic parameters in Point-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame).
+7. Saturation value of IMU's accelerator and gyroscope: ```satu_acc```, ```satu_gyro```
+8. The norm of IMU's acceleration according to unit of acceleration messages: ``` acc_norm ```
+
 Step B: Run below
 ```
     cd ~/$Point_LIO_ROS_DIR$
@@ -151,7 +166,7 @@ Step B: Run below
 
 Step C: Run LiDAR's ros driver or play rosbag.
 
-### 5.4 PCD file save
+### 5.5 PCD file save
 
 Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` Point-LIO/PCD/scans.pcd ``` after the Point-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds.
 

config/unilidar.yaml

@@ -0,0 +1,61 @@
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/unilidar/cloud" 
+            imu_topic: "/unilidar/imu" 
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency   
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+                                        # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 5
+            scan_line: 18
+            timestamp_unit: 0           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 0.5
+
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.004 # = 1 / frequency of IMU
+            satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.01 # 0.001
+            acc_cov_output: 500
+            gyr_cov_output: 1000 
+            b_acc_cov: 0.0001 
+            b_gyr_cov: 0.0001 
+            imu_meas_acc_cov: 0.1 #0.1 # 2
+            imu_meas_omg_cov: 0.1 #0.1 # 2 
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81
+            fov_degree: 180 
+            det_range: 100.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+
+            # transform from imu to lidar
+            extrinsic_T: [ 0.007698, 0.014655, -0.00667] # ulhk # [-0.5, 1.4, 1.5] # utbm
+            extrinsic_R: [ 1, 0, 0,
+                        0, 1, 0,
+                        0, 0, 1 ] # ulhk 4 utbm 3
+
+        odometry: 
+            publish_odometry_without_downsample: enable  
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: true       # save map to pcd file
+            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.

config/unilidar_l1.yaml

@@ -0,0 +1,60 @@
+# parameters from https://github.com/unitreerobotics/point_lio_unilidar/blob/main/config/unilidar_l1.yaml
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/unilidar/cloud"
+            imu_topic: "/unilidar/imu"
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+            # the timestamp of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 5
+            scan_line: 18
+            timestamp_unit: 0         # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 0.5
+
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.004 # = 1 / frequency of IMU
+            satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.01 # 0.001; 0.01
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001
+            b_gyr_cov: 0.0001
+            imu_meas_acc_cov: 0.1 #0.1 # 0.1
+            imu_meas_omg_cov: 0.1 #0.01 # 0.1
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81.0
+            fov_degree: 180.0
+            det_range: 100.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            extrinsic_T: [ 0.007698, 0.014655, -0.00667]
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                           0.0, 1.0, 0.0,
+                           0.0, 0.0, 1.0 ]
+
+        odometry:
+            publish_odometry_without_downsample: true
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: false
+            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.

launch/mapping_unilidar.py

@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'unilidar.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 3,  # Options: 1, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.5,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.5,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000
+            'runtime_pos_log_enable': False,  # Option: True
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld

launch/mapping_unilidar_l1.launch.py

@@ -0,0 +1,69 @@
+# parameters from https://github.com/unitreerobotics/point_lio_unilidar/blob/main/config/unilidar_l1.yaml
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'unilidar_l1.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 1,  # options: 4, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.1,  # options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.1,  # options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # option: 1000
+            'runtime_pos_log_enable': False,  # option: True
+        },
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld

src/preprocess.cpp

@@ -75,6 +75,10 @@ void Preprocess::process(const sensor_msgs::msg::PointCloud2::SharedPtr &msg, Po
             hesai_handler(msg);
             break;
 
+        case UNILIDAR:
+            unilidar_handler(msg);
+            break;
+
         default:
             printf("Error LiDAR Type");
             break;
@@ -333,6 +337,51 @@ void Preprocess::hesai_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &m
 
 }
 
+void Preprocess::unilidar_handler(const sensor_msgs::msg::PointCloud2::ConstPtr &msg)
+{
+    pl_surf.clear();
+    pl_corn.clear();
+    pl_full.clear();
+
+    pcl::PointCloud<unilidar_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.points.size();
+    if (plsize == 0) return;
+
+    pl_surf.reserve(plsize);
+
+    // std::cout << "plsize = " << plsize << ", given_offset_time = " << given_offset_time << std::endl;
+    int countElimnated = 0;
+    for (int i = 0; i < plsize; i++)
+    {
+      PointType added_pt;
+
+      added_pt.normal_x = 0;
+      added_pt.normal_y = 0;
+      added_pt.normal_z = 0;
+
+      added_pt.x = pl_orig.points[i].x;
+      added_pt.y = pl_orig.points[i].y;
+      added_pt.z = pl_orig.points[i].z;
+
+      added_pt.intensity = pl_orig.points[i].intensity;
+
+      added_pt.curvature = pl_orig.points[i].time * time_unit_scale; 
+
+      if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind))
+      {
+        pl_surf.points.push_back(added_pt);
+      }
+      else
+      {
+        countElimnated++;
+      }
+    }
+
+    // std::cout << "pl_surf.size() = " << pl_surf.size() << ", countElimnated = " << countElimnated << std::endl;
+
+}
+
 void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types) {
     int plsize = pl.size();
     int plsize2;