diff --git a/.gitignore b/.gitignore index f239d34..ca87db2 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ /cmake-build-debug/ *.pcd /Log/pos_log.csv +/build diff --git a/CMakeLists.txt b/CMakeLists.txt index 15b4c5f..2c8089d 100755 --- a/CMakeLists.txt +++ b/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}) diff --git a/README.md b/README.md index 6e5edca..941680d 100644 --- a/README.md +++ b/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. diff --git a/config/unilidar.yaml b/config/unilidar.yaml new file mode 100644 index 0000000..c7e9cb5 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/config/unilidar_l1.yaml b/config/unilidar_l1.yaml new file mode 100755 index 0000000..1647bb1 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/launch/mapping_unilidar.py b/launch/mapping_unilidar.py new file mode 100644 index 0000000..c56a52f --- /dev/null +++ b/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 diff --git a/launch/mapping_unilidar_l1.launch.py b/launch/mapping_unilidar_l1.launch.py new file mode 100755 index 0000000..27154b3 --- /dev/null +++ b/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 diff --git a/src/preprocess.cpp b/src/preprocess.cpp index b8a2e90..2f8641d 100755 --- a/src/preprocess.cpp +++ b/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 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 &pl, vector &types) { int plsize = pl.size(); int plsize2; diff --git a/src/preprocess.h b/src/preprocess.h index 98ff3a5..a366c72 100755 --- a/src/preprocess.h +++ b/src/preprocess.h @@ -13,8 +13,8 @@ typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud PointCloudXYZI; enum LID_TYPE { - AVIA = 1, VELO16, OUST64, HESAIxt32 -}; //{1, 2, 3, 4} + AVIA = 1, VELO16, OUST64, HESAIxt32, UNILIDAR +}; //{1, 2, 3, 4, 5} enum TIME_UNIT { SEC = 0, MS = 1, US = 2, NS = 3 }; @@ -83,6 +83,24 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point, (std::uint16_t, ring, ring) ) +namespace unilidar_ros { + struct Point { + PCL_ADD_POINT4D + PCL_ADD_INTENSITY + std::uint16_t ring; + float time; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + } EIGEN_ALIGN16; + } +POINT_CLOUD_REGISTER_POINT_STRUCT(unilidar_ros::Point, + (float, x, x)(float, y, y)(float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) + (float, time, time) +) + namespace ouster_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D;