diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..f239d34 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +/.idea/ +/cmake-build-debug/ +*.pcd +/Log/pos_log.csv diff --git a/CMakeLists.txt b/CMakeLists.txt index 27cc6cf..15b4c5f 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,34 +1,33 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(point_lio) -SET(CMAKE_BUILD_TYPE "Debug") - -ADD_COMPILE_OPTIONS(-std=c++14 ) -ADD_COMPILE_OPTIONS(-std=c++14 ) -set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) - -add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") - -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) -set(CMAKE_CXX_STANDARD 14) +# Use C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) + +# Compiler settings for performance and threading +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") +add_compile_options(-O3 -pthread -fexceptions) -message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") +# Processor count and definitions +message(STATUS "Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) include(ProcessorCount) ProcessorCount(N) - message("Processer number: ${N}") + message(STATUS "Processor number: ${N}") if(N GREATER 5) add_definitions(-DMP_EN) add_definitions(-DMP_PROC_NUM=4) - message("core for MP: 3") + message(STATUS "core for MP: 3") elseif(N GREATER 3) math(EXPR PROC_NUM "${N} - 2") add_definitions(-DMP_EN) add_definitions(-DMP_PROC_NUM="${PROC_NUM}") - message("core for MP: ${PROC_NUM}") + message(STATUS "core for MP: ${PROC_NUM}") else() add_definitions(-DMP_PROC_NUM=1) endif() @@ -36,48 +35,65 @@ else() add_definitions(-DMP_PROC_NUM=1) endif() -find_package(OpenMP QUIET) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +# Find packages +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(livox_ros_driver2 REQUIRED) -find_package(PythonLibs REQUIRED) -find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") +find_package(Eigen3 REQUIRED) -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - nav_msgs - sensor_msgs - roscpp - rospy - std_msgs - pcl_ros - tf - livox_ros_driver - message_generation - eigen_conversions -) -find_package(Eigen3 REQUIRED) -find_package(PCL 1.8 REQUIRED) +# OpenMP +find_package(OpenMP QUIET) +if(OPENMP_FOUND) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +endif() -message(Eigen: ${EIGEN3_INCLUDE_DIR}) +find_package(PythonLibs REQUIRED) # Consider using Pybind11 for ROS2 +find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") include_directories( - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - ${PCL_INCLUDE_DIRS} - ${PYTHON_INCLUDE_DIRS} - include) - -catkin_package( - CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime - DEPENDS EIGEN3 PCL - INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIR} + ${PYTHON_INCLUDE_DIRS} ) +# Declare a ROS2 executable add_executable(pointlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp) -target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) +ament_target_dependencies(pointlio_mapping + rclcpp + rclpy + geometry_msgs + nav_msgs + sensor_msgs + pcl_ros + pcl_conversions + tf2_ros + livox_ros_driver2 +) +target_link_libraries(pointlio_mapping ${PYTHON_LIBRARIES}) target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) +# Install the executable +install(TARGETS + pointlio_mapping + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY config launch rviz_cfg + DESTINATION share/${PROJECT_NAME} +) +# Export dependencies +ament_export_dependencies(rclcpp rclpy geometry_msgs nav_msgs sensor_msgs pcl_ros pcl_conversions tf2_ros livox_ros_driver2 Eigen3) +ament_package() diff --git a/README.md b/README.md index 26058f8..6e5edca 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # Point-LIO (If you have problems with the formal commits, please try the newest commit first, several modifications have be made in newest commit for wider adaptivity) ## Point-LIO: Robust High-Bandwidth Lidar-Inertial Odometry (Pay attention to modifying the parameters for IMU in .yaml file, according to the IMU you use.) +ROS2 version of Point-LIO + ## 1. Introduction
@@ -63,7 +65,7 @@ Our accompany video is available on **YouTube**. # **3. Prerequisites** ## **3.1 Ubuntu and [ROS](https://www.ros.org/)** -We tested our code on Ubuntu20.04 with noetic. Ubuntu18.04 and lower versions have problems of environments to support the Point-LIO, try to avoid using Point-LIO in those systems. Additional ROS package is required: +We tested our code on Ubuntu 20.04 with Galactic. Ubuntu18.04 and lower versions have problems of environments to support the Point-LIO, try to avoid using Point-LIO in those systems. Additional ROS package is required: ``` sudo apt-get install ros-xxx-pcl-conversions ``` @@ -74,26 +76,26 @@ Following the official [Eigen installation](eigen.tuxfamily.org/index.php?title= sudo apt-get install libeigen3-dev ``` -## **3.3 livox_ros_driver** -Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). +## **3.3 livox_ros_driver2** +Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2). *Remarks:* -- Since the Point-LIO supports Livox serials LiDAR, so the **livox_ros_driver** must be installed and **sourced** before run any Point-LIO luanch file. -- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document). +- Since the Point-LIO supports Livox serials LiDAR, so the **livox_ros_driver2** must be installed and **sourced** before run any Point-LIO launch file. +- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/install/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document). ## 4. Build -Clone the repository and catkin_make: +Clone the repository and colcon build: ``` - cd ~/$A_ROS_DIR$/src + cd ~/$Point_LIO_ROS_DIR$/src git clone https://github.com/hku-mars/Point-LIO.git cd Point-LIO git submodule update --init cd ../.. - catkin_make - source devel/setup.bash + colcon build --symlink-install + source install/setup.bash ``` -- Remember to source the livox_ros_driver before build (follow 3.3 **livox_ros_driver**) +- Remember to source the livox_ros_driver before build (follow 3.3 **livox_ros_driver2**) - If you want to use a custom build of PCL, add the following line to ~/.bashrc ```export PCL_ROOT={CUSTOM_PCL_PATH}``` @@ -103,12 +105,12 @@ Clone the repository and catkin_make: Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then ``` cd ~/$Point_LIO_ROS_DIR$ - source devel/setup.bash - roslaunch point_lio mapping_avia.launch - roslaunch livox_ros_driver livox_lidar_msg.launch + source install/setup.bash + ros2 launch point_lio mapping_avia.launch.py + ros2 launch livox_ros_driver msg_HAP_launch.py ``` -- For livox serials, Point-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for Point-LIO. ``` livox_lidar.launch ``` can not produce it right now. -- If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage. +- For livox serials, Point-LIO only support the data collected by the ``` msg_HAP_launch.py ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for Point-LIO. ``` livox_lidar.launch.py ``` can not produce it right now. +- If you want to change the frame rate, please modify the **publish_freq** parameter in the [msg_HAP_launch.py](https://github.com/Livox-SDK/livox_ros_driver2/blob/master/launch_ROS2/msg_HAP_launch.py) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage. ### 5.2 For Livox serials with external IMU @@ -143,8 +145,8 @@ Edit ``` config/velodyne.yaml ``` to set the below parameters: Step B: Run below ``` cd ~/$Point_LIO_ROS_DIR$ - source devel/setup.bash - roslaunch point_lio mapping_velody16.launch + source install/setup.bash + ros2 launch point_lio mapping_velody16.launch.py ``` Step C: Run LiDAR's ros driver or play rosbag. diff --git a/config/avia.yaml b/config/avia.yaml index 546e44d..935797e 100755 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -1,57 +1,59 @@ -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/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 +/**: + ros__parameters: + common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/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: 1 - scan_line: 6 - timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 1.0 + preprocess: + lidar_type: 1 + scan_line: 6 + timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 1.0 -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.005 # = 1 / frequency of IMU - satu_acc: 3.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: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration - lidar_meas_cov: 0.001 # 0.001; 0.01 - 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 # 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 - fov_degree: 90 - det_range: 450.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.04165, 0.02326, -0.0284 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1 ] + 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.005 # = 1 / frequency of IMU + satu_acc: 3.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: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration + lidar_meas_cov: 0.001 # 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: 90.0 + det_range: 450.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.04165, 0.02326, -0.0284 ] + 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: false + odometry: + publish_odometry_without_downsample: false -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 + 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 + 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/config/horizon.yaml b/config/horizon.yaml index 070bf90..b5320f6 100755 --- a/config/horizon.yaml +++ b/config/horizon.yaml @@ -1,57 +1,59 @@ -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/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 +/**: + ros__parameters: + common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/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: 1 - scan_line: 6 - timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 4.0 + preprocess: + lidar_type: 1 + scan_line: 6 + timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 4.0 -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.005 # = 1 / frequency of IMU - satu_acc: 3.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: 1.0 # 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.01 #0.1 # 2 - imu_meas_omg_cov: 0.01 #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: 100 - det_range: 260.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.05512, 0.02226, -0.0297 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1 ] + 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.005 # = 1 / frequency of IMU + satu_acc: 3.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: 1.0 # 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.0 + gyr_cov_output: 1000.0 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.01 #0.1 # 2 + imu_meas_omg_cov: 0.01 #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.0 + fov_degree: 100.0 + det_range: 260.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.05512, 0.02226, -0.0297 ] + 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: false + odometry: + publish_odometry_without_downsample: false -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 + 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 + 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/config/mid360.yaml b/config/mid360.yaml new file mode 100755 index 0000000..0e38af0 --- /dev/null +++ b/config/mid360.yaml @@ -0,0 +1,59 @@ +/**: + ros__parameters: + common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/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: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR + scan_line: 4 + timestamp_unit: 3 # 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.005 # = 1 / frequency of IMU + satu_acc: 3.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: 1.0 # 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.0 + gyr_cov_output: 1000.0 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.01 #0.1 # 2 + imu_meas_omg_cov: 0.01 #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.0 + fov_degree: 360.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.011, -0.02329, 0.04412 ] + 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: false + + 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/config/ouster64.yaml b/config/ouster64.yaml index 2ba04b4..612d8aa 100755 --- a/config/ouster64.yaml +++ b/config/ouster64.yaml @@ -1,57 +1,59 @@ -common: - lid_topic: "/os_cloud_node/points" - imu_topic: "/os_cloud_node/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 +/**: + ros__parameters: + common: + lid_topic: "/os_cloud_node/points" + imu_topic: "/os_cloud_node/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: 3 # 2 #velodyne # 1 Livox Avia LiDAR - scan_line: 32 # 32 #velodyne 6 avia - timestamp_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 0.20 + preprocess: + lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR + scan_line: 32 # 32 #velodyne 6 avia + timestamp_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 0.20 -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.01 # = 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.1 # 0.01 - 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: 150.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.0, 0.0, 0.0] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1 ] + 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.01 # = 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.1 # 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 # 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.0 + fov_degree: 180.0 + det_range: 150.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.0, 0.0, 0.0 ] + 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: false + odometry: + publish_odometry_without_downsample: false -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 + 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 + 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/config/velody16.yaml b/config/velody16.yaml index ed59f00..50ed7f3 100755 --- a/config/velody16.yaml +++ b/config/velody16.yaml @@ -1,63 +1,65 @@ -common: - lid_topic: "/velodyne_points" - imu_topic: "/imu/data" - 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 +/**: + ros__parameters: + common: + lid_topic: "/velodyne_points" + imu_topic: "/imu/data" + 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: 2 - scan_line: 32 - timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 2.0 + preprocess: + lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR + scan_line: 32 + 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: - 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.01 # = 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 - extrinsic_T: [ 0, 0, 0.28] # ulhk # [-0.5, 1.4, 1.5] # utbm - # extrinsic_R: [ 0, 1, 0, - # -1, 0, 0, - # 0, 0, 1 ] # ulhk 5 6 - # extrinsic_R: [ 0, -1, 0, - # 1, 0, 0, - # 0, 0, 1 ] # utbm 1, 2 - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1 ] # ulhk 4 utbm 3 + 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.01 # = 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 + 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 # 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.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.0, 0.0, 0.28 ] # ulhk # [-0.5, 1.4, 1.5] # utbm + # extrinsic_R: [ 0, 1, 0, + # -1, 0, 0, + # 0, 0, 1 ] # ulhk 5 6 + # extrinsic_R: [ 0, -1, 0, + # 1, 0, 0, + # 0, 0, 1 ] # utbm 1, 2 + extrinsic_R: [ 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 ] # ulhk 4 utbm 3 -odometry: - publish_odometry_without_downsample: false + odometry: + publish_odometry_without_downsample: false -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 + 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 + 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/include/.vscode/c_cpp_properties.json b/include/.vscode/c_cpp_properties.json deleted file mode 100755 index d7fe77d..0000000 --- a/include/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,18 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "", - "limitSymbolsToIncludedHeaders": true - }, - "includePath": [ - "/home/ecstasy/catkin_ws/devel/include/**", - "/opt/ros/melodic/include/**", - "/home/ecstasy/catkin_ws/src/FAST_LIO/include/**", - "/usr/include/**" - ], - "name": "ROS" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/include/.vscode/settings.json b/include/.vscode/settings.json deleted file mode 100755 index 4a9ea37..0000000 --- a/include/.vscode/settings.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "python.autoComplete.extraPaths": [ - "/home/ecstasy/catkin_ws/devel/lib/python2.7/dist-packages", - "/opt/ros/melodic/lib/python2.7/dist-packages" - ] -} \ No newline at end of file diff --git a/include/common_lib.h b/include/common_lib.h index 5de7546..a3ef8e5 100755 --- a/include/common_lib.h +++ b/include/common_lib.h @@ -5,17 +5,17 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include using namespace std; using namespace Eigen; #define PI_M (3.14159265358) -#define G_m_s2 (9.81) // Gravaty const in GuangDong/China -#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) -#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) +#define G_m_s2 (9.81) // Gravity const in GuangDong/China +#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) +#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) #define CUBE_LEN (6.0) #define LIDAR_SP_LEN (2) #define INIT_COV (0.0001) @@ -23,7 +23,7 @@ using namespace Eigen; #define MAX_MEAS_DIM (10000) #define VEC_FROM_ARRAY(v) v[0],v[1],v[2] -#define VEC_FROM_ARRAY_SIX(v) v[0],v[1],v[2],v[3],v[4],v[5] +#define VEC_FROM_ARRAY_SIX(v) v[0],v[1],v[2],v[3],v[4],v[5] #define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] #define CONSTRAIN(v,min,max) ((v>min)?((v imu; + deque imu{}; }; template @@ -173,4 +173,17 @@ bool esti_plane(Matrix &pca_result, const PointVector &point, const T & return true; } +inline double get_time_sec(const builtin_interfaces::msg::Time &time) +{ + return rclcpp::Time(time).seconds(); +} + +inline rclcpp::Time get_ros_time(double timestamp) +{ + int32_t sec = std::floor(timestamp); + auto nanosec_d = (timestamp - std::floor(timestamp)) * 1e9; + uint32_t nanosec = nanosec_d; + return rclcpp::Time(sec, nanosec); +} + #endif \ No newline at end of file diff --git a/launch/gdb_debug_example.launch b/launch/gdb_debug_example.launch deleted file mode 100755 index 4495e76..0000000 --- a/launch/gdb_debug_example.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - launch-prefix="gdb -ex run --args" - - \ No newline at end of file diff --git a/launch/gdb_debug_example.launch.py b/launch/gdb_debug_example.launch.py new file mode 100755 index 0000000..a452b76 --- /dev/null +++ b/launch/gdb_debug_example.launch.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 = [ + { + '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: 1, 3 + 'space_down_sample': True, + 'filter_size_surf': 0.3, # Options: 0.5, 0.3, 0.2, 0.15, 0.1 + 'filter_size_map': 0.2, # Options: 0.5, 0.3, 0.15, 0.1 + 'cube_side_length': 1000.0, # Option: 1000, 2000 + 'runtime_pos_log_enable': False # Option: True + }, + # PathJoinSubstitution([ + # FindPackageShare('point_lio'), + # 'config', 'horizon.yaml' + # ]) + ] + + # 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_avia.launch b/launch/mapping_avia.launch deleted file mode 100755 index 2b1fc0e..0000000 --- a/launch/mapping_avia.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - launch-prefix="gdb -ex run --args" - - \ No newline at end of file diff --git a/launch/mapping_avia.launch.py b/launch/mapping_avia.launch.py new file mode 100755 index 0000000..dc067f7 --- /dev/null +++ b/launch/mapping_avia.launch.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', 'avia.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.3, # options: 0.5, 0.3, 0.2, 0.15, 0.1 + 'filter_size_map': 0.2, # options: 0.5, 0.3, 0.15, 0.1 + 'cube_side_length': 2000.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_horizon.launch b/launch/mapping_horizon.launch deleted file mode 100755 index 43c6968..0000000 --- a/launch/mapping_horizon.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - launch-prefix="gdb -ex run --args" - - \ No newline at end of file diff --git a/launch/mapping_horizon.launch.py b/launch/mapping_horizon.launch.py new file mode 100755 index 0000000..8b3374b --- /dev/null +++ b/launch/mapping_horizon.launch.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', 'horizon.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_mid360.launch.py b/launch/mapping_mid360.launch.py new file mode 100755 index 0000000..dbf6ee4 --- /dev/null +++ b/launch/mapping_mid360.launch.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', 'mid360.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_ouster64.launch b/launch/mapping_ouster64.launch deleted file mode 100755 index 5aa5fc7..0000000 --- a/launch/mapping_ouster64.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - launch-prefix="gdb -ex run --args" - - diff --git a/launch/mapping_ouster64.launch.py b/launch/mapping_ouster64.launch.py new file mode 100755 index 0000000..5577693 --- /dev/null +++ b/launch/mapping_ouster64.launch.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', 'ouster64.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': 4, # Options: 4, 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 (changed from 2000) + '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_velody16.launch b/launch/mapping_velody16.launch deleted file mode 100755 index 42e443c..0000000 --- a/launch/mapping_velody16.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - launch-prefix="gdb -ex run --args" - - \ No newline at end of file diff --git a/launch/mapping_velody16.launch.py b/launch/mapping_velody16.launch.py new file mode 100755 index 0000000..21cd90b --- /dev/null +++ b/launch/mapping_velody16.launch.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', 'velody16.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': 4, # Options: 4, 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 (changed from 2000) + '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/package.xml b/package.xml index 15ba3fd..ddfc701 100755 --- a/package.xml +++ b/package.xml @@ -1,13 +1,12 @@ - + point_lio 0.0.0 - This is a modified version of LOAM which is original algorithm is described in the following paper: J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. - Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. claydergc @@ -15,33 +14,26 @@ BSD Dongjiao He - - catkin - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs - sensor_msgs - tf - pcl_ros - livox_ros_driver - message_generation - geometry_msgs - nav_msgs - sensor_msgs - roscpp - rospy - std_msgs - tf - pcl_ros - livox_ros_driver - message_runtime + + ament_cmake + + + rclcpp + rclpy + sensor_msgs + geometry_msgs + nav_msgs + tf2_ros + pcl_ros + pcl_conversions + livox_ros_driver2 - rostest - rosbag + + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/rviz_cfg/.gitignore b/rviz_cfg/.gitignore deleted file mode 100755 index e69de29..0000000 diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz old mode 100755 new mode 100644 index c99ca7e..b6f0986 --- a/rviz_cfg/loam_livox.rviz +++ b/rviz_cfg/loam_livox.rviz @@ -1,357 +1,318 @@ Panels: - - Class: rviz/Displays - Help Height: 0 + - Class: rviz_common/Displays + Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /Axes1 - - /mapping1 - - /mapping1/surround1 - - /mapping1/currPoints1 - - /mapping1/currPoints1/Autocompute Value Bounds1 - - /Odometry1/Odometry1 - - /Odometry1/Odometry1/Shape1 - - /Odometry1/Odometry1/Covariance1 - - /Odometry1/Odometry1/Covariance1/Position1 - - /Odometry1/Odometry1/Covariance1/Orientation1 - - /MarkerArray1/Namespaces1 - Splitter Ratio: 0.6432291865348816 - Tree Height: 777 - - Class: rviz/Selection + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: surround -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 + SyncSource: CloudRegistered Visualization Manager: Class: "" Displays: - - Alpha: 1 - Cell Size: 1000 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 40 - Reference Frame: - Value: false - - Alpha: 1 - Class: rviz/Axes + - Class: rviz_default_plugins/TF Enabled: true - Length: 0.699999988079071 - Name: Axes - Radius: 0.05999999865889549 - Reference Frame: aft_mapped - Show Trail: false - Value: true - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 238; 238; 236 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 238; 238; 236 - Name: surround - Position Transformer: XYZ - Queue Size: 1 - Selectable: false - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Points - Topic: /cloud_registered - Unreliable: false - Use Fixed Frame: true - Use rainbow: true + Frame Timeout: 15 + Frames: + All Enabled: true + aft_mapped: Value: true - - Alpha: 0.10000000149011612 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -5 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 1000 - Enabled: true - Invert Rainbow: true - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: currPoints - Position Transformer: XYZ - Queue Size: 100000 - Selectable: true - Size (Pixels): 1 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud_registered - Unreliable: false - Use Fixed Frame: true - Use rainbow: true + base_footprint: Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /Laser_map - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Name: mapping - - Class: rviz/Group - Displays: - - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 1 - Name: Odometry - Position Tolerance: 0.0010000000474974513 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.20000000298023224 - Color: 255; 85; 0 - Head Length: 0 - Head Radius: 0 - Shaft Length: 0.05000000074505806 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /Odometry - Unreliable: false + base_link: + Value: true + camera_init: + Value: true + camera_link: + Value: true + gyro_link: + Value: true + laser: + Value: true + left_front_link: + Value: true + left_wheel_link: Value: true + map: + Value: true + odom_combined: + Value: true + right_front_link: + Value: true + right_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + camera_init: + aft_mapped: + {} + Update Interval: 0 + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true Enabled: true + Keep: 100 Name: Odometry - - Alpha: 1 - Class: rviz/Axes - Enabled: true - Length: 0.699999988079071 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Show Trail: false + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /Odometry Value: true - - Alpha: 0 - Buffer Length: 2 - Class: rviz/Path - Color: 25; 255; 255 + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 Enabled: true - Head Diameter: 0 - Head Length: 0 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.20000000298023224 + Line Style: Lines + Line Width: 0.029999999329447746 Name: Path Offset: X: 0 Y: 0 Z: 0 - Pose Color: 25; 255; 255 + Pose Color: 255; 85; 255 Pose Style: None - Queue Size: 10 Radius: 0.029999999329447746 - Shaft Diameter: 0.4000000059604645 - Shaft Length: 0.4000000059604645 - Topic: /path - Unreliable: false + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /path + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.8584054708480835 + Min Value: -0.10289539396762848 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 30 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 186 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: CloudRegistered + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_registered + Use Fixed Frame: true + Use rainbow: true Value: true - Alpha: 1 - Autocompute Intensity Bounds: false + Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false - Max Color: 239; 41; 41 - Max Intensity: 0 - Min Color: 239; 41; 41 + Max Color: 255; 255; 255 + Max Intensity: 184 + Min Color: 0; 0; 0 Min Intensity: 0 - Name: PointCloud2 + Name: CloudEffected Position Transformer: XYZ - Queue Size: 10 Selectable: true - Size (Pixels): 4 - Size (m): 0.30000001192092896 - Style: Spheres - Topic: /cloud_effected - Unreliable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_effected Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 13.139549255371094 - Min Value: -32.08251953125 + Max Value: 2.036320447921753 + Min Value: -0.09378375858068466 Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 138; 226; 52 - Color Transformer: FlatColor + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false - Max Color: 138; 226; 52 - Min Color: 138; 226; 52 - Name: PointCloud2 + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: CloudMap Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.10000000149011612 + Size (m): 0.019999999552965164 Style: Flat Squares - Topic: /Laser_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /Laser_map Use Fixed Frame: true Use rainbow: true - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /MarkerArray - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: false + Value: true Enabled: true Global Options: Background Color: 0; 0; 0 - Default Light: true Fixed Frame: camera_init - Frame Rate: 10 + Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 100.72154235839844 + Class: rviz_default_plugins/Orbit + Distance: 28.403661727905273 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 34.07209014892578 - Y: -2.53304386138916 - Z: -8.43538761138916 + X: -2.2604103088378906 + Y: -0.3224470913410187 + Z: 0.9725424647331238 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.06979652494192123 - Target Frame: global - Yaw: 4.245370388031006 + Pitch: 0.7497965693473816 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 1.7503817081451416 Saved: ~ Window Geometry: Displays: - collapsed: true - Height: 1016 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001c800000346fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000346000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000052fc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007380000034600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ad0000003efc0100000002fb0000000800540069006d00650100000000000005ad000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000451000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -359,7 +320,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true - Width: 1848 - X: 72 - Y: 27 + collapsed: false + Width: 1453 + X: 2404 + Y: 218 diff --git a/src/Estimator.cpp b/src/Estimator.cpp index 8823729..912e98b 100755 --- a/src/Estimator.cpp +++ b/src/Estimator.cpp @@ -6,10 +6,10 @@ std::vector time_seq; PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); std::vector pbody_list; -std::vector Nearest_Points; +std::vector Nearest_Points; KD_TREE ikdtree; std::vector pointSearchSqDis(NUM_MATCH_POINTS); -bool point_selected_surf[100000] = {0}; +bool point_selected_surf[100000] = {0}; std::vector crossmat_list; int effct_feat_num = 0; int k; @@ -26,7 +26,7 @@ M3D Lidar_R_wrt_IMU(Eye3d); typedef MTK::vect<3, double> vect3; typedef MTK::SO3 SO3; -typedef MTK::S2 S2; +typedef MTK::S2 S2; typedef MTK::vect<1, double> vect1; typedef MTK::vect<2, double> vect2; diff --git a/src/Estimator.h b/src/Estimator.h index 5f368df..6e0c552 100755 --- a/src/Estimator.h +++ b/src/Estimator.h @@ -16,7 +16,7 @@ extern std::vector time_seq; extern PointCloudXYZI::Ptr feats_down_body; //(new PointCloudXYZI()); extern PointCloudXYZI::Ptr feats_down_world; //(new PointCloudXYZI()); extern std::vector pbody_list; -extern std::vector Nearest_Points; +extern std::vector Nearest_Points; extern KD_TREE ikdtree; extern std::vector pointSearchSqDis; extern bool point_selected_surf[100000]; // = {0}; @@ -31,7 +31,7 @@ extern M3D Lidar_R_wrt_IMU; //(Eye3d); typedef MTK::vect<3, double> vect3; typedef MTK::SO3 SO3; -typedef MTK::S2 S2; +typedef MTK::S2 S2; typedef MTK::vect<1, double> vect1; typedef MTK::vect<2, double> vect2; @@ -111,7 +111,7 @@ void h_model_output(state_output &s, esekfom::dyn_share_modified &ekfom_ void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified &ekfom_data); -void pointBodyToWorld(PointType const * const pi, PointType * const po); +void pointBodyToWorld(PointType const *const pi, PointType *const po); const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);}; diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp index aabdd08..898a451 100755 --- a/src/IMU_Processing.hpp +++ b/src/IMU_Processing.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include #include @@ -13,165 +13,152 @@ #include #include #include -#include +#include #include #include -#include -#include +#include #include -#include -#include -#include +#include +#include +#include /// *************Preconfiguration #define MAX_INI_COUNT (100) /// *************IMU Process and undistortion -class ImuProcess -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - ImuProcess(); - ~ImuProcess(); - - void Reset(); - void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); - void Process(const MeasureGroup &meas, PointCloudXYZI::Ptr pcl_un_); - void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot); - - ofstream fout_imu; - // double first_lidar_time; - int lidar_type; - bool imu_en; - V3D mean_acc, gravity_; - bool imu_need_init_ = true; - bool b_first_frame_ = true; - bool gravity_align_ = false; - - private: - void IMU_init(const MeasureGroup &meas, int &N); - V3D mean_gyr; - int init_iter_num = 1; +class ImuProcess { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ImuProcess(); + + ~ImuProcess(); + + void Reset(); + + //void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu); + + void Process(const MeasureGroup &meas, const PointCloudXYZI::Ptr &pcl_un_); + + void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot); + + ofstream fout_imu; + // double first_lidar_time; + int lidar_type; + bool imu_en; + V3D mean_acc, gravity_; + bool imu_need_init_ = true; + bool b_first_frame_ = true; + bool gravity_align_ = false; + +private: + void IMU_init(const MeasureGroup &meas, int &N); + + V3D mean_gyr; + int init_iter_num = 1; + rclcpp::Logger logger; }; ImuProcess::ImuProcess() - : b_first_frame_(true), imu_need_init_(true), gravity_align_(false) -{ - imu_en = true; - init_iter_num = 1; - mean_acc = V3D(0, 0, -1.0); - mean_gyr = V3D(0, 0, 0); + : b_first_frame_(true), imu_need_init_(true), gravity_align_(false), + logger(rclcpp::get_logger("laserMapping")) { + imu_en = true; + init_iter_num = 1; + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); } ImuProcess::~ImuProcess() {} -void ImuProcess::Reset() -{ - ROS_WARN("Reset ImuProcess"); - mean_acc = V3D(0, 0, -1.0); - mean_gyr = V3D(0, 0, 0); - imu_need_init_ = true; - init_iter_num = 1; +void ImuProcess::Reset() { + RCLCPP_WARN(logger, "Reset ImuProcess"); + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + imu_need_init_ = true; + init_iter_num = 1; } -void ImuProcess::IMU_init(const MeasureGroup &meas, int &N) -{ - /** 1. initializing the gravity, gyro bias, acc and gyro covariance - ** 2. normalize the acceleration measurenments to unit gravity **/ - ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); - V3D cur_acc, cur_gyr; - - if (b_first_frame_) - { - Reset(); - N = 1; - b_first_frame_ = false; - const auto &imu_acc = meas.imu.front()->linear_acceleration; - const auto &gyr_acc = meas.imu.front()->angular_velocity; - mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; - mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; - } - - for (const auto &imu : meas.imu) - { - const auto &imu_acc = imu->linear_acceleration; - const auto &gyr_acc = imu->angular_velocity; - cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; - cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; - - mean_acc += (cur_acc - mean_acc) / N; - mean_gyr += (cur_gyr - mean_gyr) / N; - - N ++; - } -} +void ImuProcess::IMU_init(const MeasureGroup &meas, int &N) { + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + RCLCPP_INFO(logger, "IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); + V3D cur_acc, cur_gyr; + + if (b_first_frame_) { + Reset(); + N = 1; + b_first_frame_ = false; + const auto &imu_acc = meas.imu.front()->linear_acceleration; + const auto &gyr_acc = meas.imu.front()->angular_velocity; + mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; + mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + } -void ImuProcess::Process(const MeasureGroup &meas, PointCloudXYZI::Ptr cur_pcl_un_) -{ - if (imu_en) - { - if(meas.imu.empty()) return; - ROS_ASSERT(meas.lidar != nullptr); + for (const auto &imu: meas.imu) { + const auto &imu_acc = imu->linear_acceleration; + const auto &gyr_acc = imu->angular_velocity; + cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; + cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; - if (imu_need_init_) - { - /// The very first lidar frame - IMU_init(meas, init_iter_num); + mean_acc += (cur_acc - mean_acc) / N; + mean_gyr += (cur_gyr - mean_gyr) / N; - imu_need_init_ = true; + N++; + } +} - if (init_iter_num > MAX_INI_COUNT) - { - ROS_INFO("IMU Initializing: %.1f %%", 100.0); - imu_need_init_ = false; +void ImuProcess::Process(const MeasureGroup &meas, const PointCloudXYZI::Ptr &cur_pcl_un_) { + if (imu_en) { + if (meas.imu.empty()) return; + assert(meas.lidar != nullptr); + + if (imu_need_init_) { + /// The very first lidar frame + IMU_init(meas, init_iter_num); + + imu_need_init_ = true; + + if (init_iter_num > MAX_INI_COUNT) { + RCLCPP_INFO(logger, "IMU Initializing: %.1f %%", 100.0); + imu_need_init_ = false; + *cur_pcl_un_ = *(meas.lidar); + } + return; + } + if (!gravity_align_) gravity_align_ = true; + *cur_pcl_un_ = *(meas.lidar); + return; + } else { + if (!b_first_frame_) { if (!gravity_align_) gravity_align_ = true; } + else { + b_first_frame_ = false; + return; + } *cur_pcl_un_ = *(meas.lidar); - } - return; + return; } - if (!gravity_align_) gravity_align_ = true; - *cur_pcl_un_ = *(meas.lidar); - return; - } - else - { - if (!b_first_frame_) - {if (!gravity_align_) gravity_align_ = true;} - else - {b_first_frame_ = false; - return;} - *cur_pcl_un_ = *(meas.lidar); - return; - } } -void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot) -{ - /** 1. initializing the gravity, gyro bias, acc and gyro covariance - ** 2. normalize the acceleration measurenments to unit gravity **/ - // V3D tmp_gravity = - mean_acc / mean_acc.norm() * G_m_s2; // state_gravity; - M3D hat_grav; - hat_grav << 0.0, gravity_(2), -gravity_(1), - -gravity_(2), 0.0, gravity_(0), - gravity_(1), -gravity_(0), 0.0; - double align_norm = (hat_grav * tmp_gravity).norm() / tmp_gravity.norm() / gravity_.norm(); - double align_cos = gravity_.transpose() * tmp_gravity; - align_cos = align_cos / gravity_.norm() / tmp_gravity.norm(); - if (align_norm < 1e-6) - { - if (align_cos > 1e-6) - { - rot = Eye3d; - } - else - { - rot = -Eye3d; +void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot) { + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + // V3D tmp_gravity = - mean_acc / mean_acc.norm() * G_m_s2; // state_gravity; + M3D hat_grav; + hat_grav << 0.0, gravity_(2), -gravity_(1), + -gravity_(2), 0.0, gravity_(0), + gravity_(1), -gravity_(0), 0.0; + double align_norm = (hat_grav * tmp_gravity).norm() / tmp_gravity.norm() / gravity_.norm(); + double align_cos = gravity_.transpose() * tmp_gravity; + align_cos = align_cos / gravity_.norm() / tmp_gravity.norm(); + if (align_norm < 1e-6) { + if (align_cos > 1e-6) { + rot = Eye3d; + } else { + rot = -Eye3d; + } + } else { + V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos); + rot = Exp(align_angle(0), align_angle(1), align_angle(2)); } - } - else - { - V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos); - rot = Exp(align_angle(0), align_angle(1), align_angle(2)); - } } \ No newline at end of file diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 7caddfc..a9d8289 100755 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -1,31 +1,30 @@ #include #include -#include +#include #include #include #include -#include #include #include -#include +#include #include #include "IMU_Processing.hpp" -#include -#include -#include + +#include +#include +#include #include #include #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include + #include "parameters.h" #include "Estimator.h" -// #include #define MAXN (720000) @@ -45,19 +44,19 @@ double time_update_last = 0.0, time_current = 0.0, time_predict_last_const = 0.0 shared_ptr p_imu(new ImuProcess()); bool init_map = false, flg_first_scan = true; -PointCloudXYZI::Ptr ptr_con(new PointCloudXYZI()); +PointCloudXYZI::Ptr ptr_con(new PointCloudXYZI()); // Time Log Variables double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot11[MAXN]; double match_time = 0, solve_time = 0, propag_time = 0, update_time = 0; -bool lidar_pushed = false, flg_reset = false, flg_exit = false; +bool lidar_pushed = false, flg_reset = false, flg_exit = false; vector cub_needrm; -deque lidar_buffer; -deque time_buffer; -deque imu_deque; +deque lidar_buffer; +deque time_buffer; +deque imu_deque; //surf feature in map PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); @@ -71,35 +70,31 @@ V3D euler_cur; MeasureGroup Measures; -sensor_msgs::Imu imu_last, imu_next; -sensor_msgs::Imu::ConstPtr imu_last_ptr; -nav_msgs::Path path; -nav_msgs::Odometry odomAftMapped; -geometry_msgs::PoseStamped msg_body_pose; +sensor_msgs::msg::Imu imu_last, imu_next; +sensor_msgs::msg::Imu::ConstSharedPtr imu_last_ptr; +nav_msgs::msg::Path path; +nav_msgs::msg::Odometry odomAftMapped; +geometry_msgs::msg::PoseStamped msg_body_pose; -void SigHandle(int sig) -{ +auto logger = rclcpp::get_logger("laserMapping"); + +void SigHandle(int sig) { flg_exit = true; - ROS_WARN("catch sig %d", sig); + RCLCPP_WARN(logger, "catch sig %d", sig); sig_buffer.notify_all(); } -inline void dump_lio_state_to_log(FILE *fp) -{ +inline void dump_lio_state_to_log(FILE *fp) { V3D rot_ang; - if (!use_imu_as_input) - { + if (!use_imu_as_input) { rot_ang = SO3ToEuler(kf_output.x_.rot); - } - else - { + } else { rot_ang = SO3ToEuler(kf_input.x_.rot); } - + fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle - if (use_imu_as_input) - { + if (use_imu_as_input) { fprintf(fp, "%lf %lf %lf ", kf_input.x_.pos(0), kf_input.x_.pos(1), kf_input.x_.pos(2)); // Pos fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega fprintf(fp, "%lf %lf %lf ", kf_input.x_.vel(0), kf_input.x_.vel(1), kf_input.x_.vel(2)); // Vel @@ -107,38 +102,30 @@ inline void dump_lio_state_to_log(FILE *fp) fprintf(fp, "%lf %lf %lf ", kf_input.x_.bg(0), kf_input.x_.bg(1), kf_input.x_.bg(2)); // Bias_g fprintf(fp, "%lf %lf %lf ", kf_input.x_.ba(0), kf_input.x_.ba(1), kf_input.x_.ba(2)); // Bias_a fprintf(fp, "%lf %lf %lf ", kf_input.x_.gravity(0), kf_input.x_.gravity(1), kf_input.x_.gravity(2)); // Bias_a - } - else - { + } else { fprintf(fp, "%lf %lf %lf ", kf_output.x_.pos(0), kf_output.x_.pos(1), kf_output.x_.pos(2)); // Pos fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega fprintf(fp, "%lf %lf %lf ", kf_output.x_.vel(0), kf_output.x_.vel(1), kf_output.x_.vel(2)); // Vel fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc fprintf(fp, "%lf %lf %lf ", kf_output.x_.bg(0), kf_output.x_.bg(1), kf_output.x_.bg(2)); // Bias_g fprintf(fp, "%lf %lf %lf ", kf_output.x_.ba(0), kf_output.x_.ba(1), kf_output.x_.ba(2)); // Bias_a - fprintf(fp, "%lf %lf %lf ", kf_output.x_.gravity(0), kf_output.x_.gravity(1), kf_output.x_.gravity(2)); // Bias_a + fprintf(fp, "%lf %lf %lf ", kf_output.x_.gravity(0), kf_output.x_.gravity(1), + kf_output.x_.gravity(2)); // Bias_a } - fprintf(fp, "\r\n"); + fprintf(fp, "\r\n"); fflush(fp); } -void pointBodyLidarToIMU(PointType const * const pi, PointType * const po) -{ +void pointBodyLidarToIMU(PointType const *const pi, PointType *const po) { V3D p_body_lidar(pi->x, pi->y, pi->z); V3D p_body_imu; - if (extrinsic_est_en) - { - if (!use_imu_as_input) - { + if (extrinsic_est_en) { + if (!use_imu_as_input) { p_body_imu = kf_output.x_.offset_R_L_I.normalized() * p_body_lidar + kf_output.x_.offset_T_L_I; - } - else - { + } else { p_body_imu = kf_input.x_.offset_R_L_I.normalized() * p_body_lidar + kf_input.x_.offset_T_L_I; } - } - else - { + } else { p_body_imu = Lidar_R_wrt_IMU * p_body_lidar + Lidar_T_wrt_IMU; } po->x = p_body_imu(0); @@ -158,21 +145,18 @@ void points_cache_collect() // seems for debug BoxPointType LocalMap_Points; bool Localmap_Initialized = false; -void lasermap_fov_segment() -{ + +void lasermap_fov_segment() { cub_needrm.shrink_to_fit(); V3D pos_LiD; - if (use_imu_as_input) - { + if (use_imu_as_input) { pos_LiD = kf_input.x_.pos + kf_input.x_.rot.normalized() * Lidar_T_wrt_IMU; - } - else - { + } else { pos_LiD = kf_output.x_.pos + kf_output.x_.rot.normalized() * Lidar_T_wrt_IMU; } - if (!Localmap_Initialized){ - for (int i = 0; i < 3; i++){ + if (!Localmap_Initialized) { + for (int i = 0; i < 3; i++) { LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; } @@ -181,23 +165,26 @@ void lasermap_fov_segment() } float dist_to_map_edge[3][2]; bool need_move = false; - for (int i = 0; i < 3; i++){ + for (int i = 0; i < 3; i++) { dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); - if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true; + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || + dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) + need_move = true; } if (!need_move) return; BoxPointType New_LocalMap_Points, tmp_boxpoints; New_LocalMap_Points = LocalMap_Points; - float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD -1))); - for (int i = 0; i < 3; i++){ + float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, + double(DET_RANGE * (MOV_THRESHOLD - 1))); + for (int i = 0; i < 3; i++) { tmp_boxpoints = LocalMap_Points; - if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE){ + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) { New_LocalMap_Points.vertex_max[i] -= mov_dist; New_LocalMap_Points.vertex_min[i] -= mov_dist; tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist; cub_needrm.emplace_back(tmp_boxpoints); - } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE){ + } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) { New_LocalMap_Points.vertex_max[i] += mov_dist; New_LocalMap_Points.vertex_min[i] += mov_dist; tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist; @@ -207,17 +194,15 @@ void lasermap_fov_segment() LocalMap_Points = New_LocalMap_Points; points_cache_collect(); - if(cub_needrm.size() > 0) int kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); + if (cub_needrm.size() > 0) int kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); } -void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ +void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { mtx_buffer.lock(); - scan_count ++; + scan_count++; double preprocess_start_time = omp_get_wtime(); - if (msg->header.stamp.toSec() < last_timestamp_lidar) - { - ROS_ERROR("lidar loop back, clear buffer"); + if (get_time_sec(msg->header.stamp) < last_timestamp_lidar) { + RCLCPP_ERROR(logger, "lidar loop back, clear buffer"); // lidar_buffer.shrink_to_fit(); mtx_buffer.unlock(); @@ -225,24 +210,22 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) return; } - last_timestamp_lidar = msg->header.stamp.toSec(); + last_timestamp_lidar = msg->header.stamp.sec; - PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); - PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); - double time_div = msg->header.stamp.toSec(); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); + double time_div = get_time_sec(msg->header.stamp); p_pre->process(msg, ptr); - if (cut_frame) - { + if (cut_frame) { sort(ptr->points.begin(), ptr->points.end(), time_list); - for (int i = 0; i < ptr->size(); i++) - { + for (int i = 0; i < ptr->size(); i++) { ptr_div->push_back(ptr->points[i]); // cout << "check time:" << ptr->points[i].curvature << endl; - if (ptr->points[i].curvature / double(1000) + msg->header.stamp.toSec() - time_div > cut_frame_time_interval) - { - if(ptr_div->size() < 1) continue; - PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); + if (ptr->points[i].curvature / double(1000) + get_time_sec(msg->header.stamp) - time_div > + cut_frame_time_interval) { + if (ptr_div->size() < 1) continue; + PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); *ptr_div_i = *ptr_div; lidar_buffer.push_back(ptr_div_i); time_buffer.push_back(time_div); @@ -250,31 +233,23 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) ptr_div->clear(); } } - if (!ptr_div->empty()) - { + if (!ptr_div->empty()) { lidar_buffer.push_back(ptr_div); // ptr_div->clear(); time_buffer.push_back(time_div); } - } - else if (con_frame) - { - if (frame_ct == 0) - { - time_con = last_timestamp_lidar; //msg->header.stamp.toSec(); + } else if (con_frame) { + if (frame_ct == 0) { + time_con = last_timestamp_lidar; //get_time_sec(msg->header.stamp); } - if (frame_ct < con_frame_num) - { - for (int i = 0; i < ptr->size(); i++) - { + if (frame_ct < con_frame_num) { + for (int i = 0; i < ptr->size(); i++) { ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000; ptr_con->push_back(ptr->points[i]); } - frame_ct ++; - } - else - { - PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI()); + frame_ct++; + } else { + PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI()); *ptr_con_i = *ptr_con; lidar_buffer.push_back(ptr_con_i); double time_con_i = time_con; @@ -282,48 +257,42 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) ptr_con->clear(); frame_ct = 0; } - } - else - { + } else { lidar_buffer.emplace_back(ptr); - time_buffer.emplace_back(msg->header.stamp.toSec()); + time_buffer.emplace_back(get_time_sec(msg->header.stamp)); } s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; mtx_buffer.unlock(); sig_buffer.notify_all(); } -void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) -{ +void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg) { mtx_buffer.lock(); double preprocess_start_time = omp_get_wtime(); - scan_count ++; - if (msg->header.stamp.toSec() < last_timestamp_lidar) - { - ROS_ERROR("lidar loop back, clear buffer"); + scan_count++; + if (get_time_sec(msg->header.stamp) < last_timestamp_lidar) { + RCLCPP_ERROR(logger, "lidar loop back, clear buffer"); mtx_buffer.unlock(); sig_buffer.notify_all(); return; } - last_timestamp_lidar = msg->header.stamp.toSec(); - - PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); - PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); + last_timestamp_lidar = get_time_sec(msg->header.stamp); + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); p_pre->process(msg, ptr); - double time_div = msg->header.stamp.toSec(); - if (cut_frame) - { + double time_div = get_time_sec(msg->header.stamp); + if (cut_frame) { sort(ptr->points.begin(), ptr->points.end(), time_list); - for (int i = 0; i < ptr->size(); i++) - { + for (int i = 0; i < ptr->size(); i++) { ptr_div->push_back(ptr->points[i]); - if (ptr->points[i].curvature / double(1000) + msg->header.stamp.toSec() - time_div > cut_frame_time_interval) - { - if(ptr_div->size() < 1) continue; - PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); + if (ptr->points[i].curvature / double(1000) + get_time_sec(msg->header.stamp) - time_div > + cut_frame_time_interval) { + if (ptr_div->size() < 1) continue; + PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); // cout << "ptr div num:" << ptr_div->size() << endl; *ptr_div_i = *ptr_div; // cout << "ptr div i num:" << ptr_div_i->size() << endl; @@ -333,31 +302,23 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) ptr_div->clear(); } } - if (!ptr_div->empty()) - { + if (!ptr_div->empty()) { lidar_buffer.push_back(ptr_div); // ptr_div->clear(); time_buffer.push_back(time_div); } - } - else if (con_frame) - { - if (frame_ct == 0) - { - time_con = last_timestamp_lidar; //msg->header.stamp.toSec(); + } else if (con_frame) { + if (frame_ct == 0) { + time_con = last_timestamp_lidar; //get_time_sec(msg->header.stamp); } - if (frame_ct < con_frame_num) - { - for (int i = 0; i < ptr->size(); i++) - { + if (frame_ct < con_frame_num) { + for (int i = 0; i < ptr->size(); i++) { ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000; ptr_con->push_back(ptr->points[i]); } - frame_ct ++; - } - else - { - PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI()); + frame_ct++; + } else { + PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI()); *ptr_con_i = *ptr_con; double time_con_i = time_con; lidar_buffer.push_back(ptr_con_i); @@ -365,62 +326,52 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) ptr_con->clear(); frame_ct = 0; } - } - else - { + } else { lidar_buffer.emplace_back(ptr); - time_buffer.emplace_back(msg->header.stamp.toSec()); + time_buffer.emplace_back(get_time_sec(msg->header.stamp)); } s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; mtx_buffer.unlock(); sig_buffer.notify_all(); } -void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) -{ - publish_count ++; - sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); +void imu_cbk(const sensor_msgs::msg::Imu::SharedPtr msg_in) { + publish_count++; + sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in)); - msg->header.stamp = ros::Time().fromSec(msg_in->header.stamp.toSec() - time_lag_imu_to_lidar); - double timestamp = msg->header.stamp.toSec(); + msg->header.stamp = get_ros_time(get_time_sec(msg_in->header.stamp) - time_lag_imu_to_lidar); + double timestamp = get_time_sec(msg->header.stamp); mtx_buffer.lock(); - if (timestamp < last_timestamp_imu) - { - ROS_ERROR("imu loop back, clear deque"); + if (timestamp < last_timestamp_imu) { + RCLCPP_ERROR(logger, "imu loop back, clear deque"); // imu_deque.shrink_to_fit(); mtx_buffer.unlock(); sig_buffer.notify_all(); return; } - + imu_deque.emplace_back(msg); last_timestamp_imu = timestamp; mtx_buffer.unlock(); sig_buffer.notify_all(); } -bool sync_packages(MeasureGroup &meas) -{ - if (!imu_en) - { - if (!lidar_buffer.empty()) - { +bool sync_packages(MeasureGroup &meas) { + if (!imu_en) { + if (!lidar_buffer.empty()) { meas.lidar = lidar_buffer.front(); meas.lidar_beg_time = time_buffer.front(); time_buffer.pop_front(); lidar_buffer.pop_front(); - if(meas.lidar->points.size() < 1) - { + if (meas.lidar->points.size() < 1) { cout << "lose lidar" << std::endl; return false; } double end_time = meas.lidar->points.back().curvature; - for (auto pt: meas.lidar->points) - { - if (pt.curvature > end_time) - { + for (auto pt: meas.lidar->points) { + if (pt.curvature > end_time) { end_time = pt.curvature; } } @@ -431,17 +382,14 @@ bool sync_packages(MeasureGroup &meas) return false; } - if (lidar_buffer.empty() || imu_deque.empty()) - { + if (lidar_buffer.empty() || imu_deque.empty()) { return false; } /*** push a lidar scan ***/ - if(!lidar_pushed) - { + if (!lidar_pushed) { meas.lidar = lidar_buffer.front(); - if(meas.lidar->points.size() < 1) - { + if (meas.lidar->points.size() < 1) { cout << "lose lidar" << endl; lidar_buffer.pop_front(); time_buffer.pop_front(); @@ -449,49 +397,41 @@ bool sync_packages(MeasureGroup &meas) } meas.lidar_beg_time = time_buffer.front(); double end_time = meas.lidar->points.back().curvature; - for (auto pt: meas.lidar->points) - { - if (pt.curvature > end_time) - { + for (auto pt: meas.lidar->points) { + if (pt.curvature > end_time) { end_time = pt.curvature; } } lidar_end_time = meas.lidar_beg_time + end_time / double(1000); - + meas.lidar_last_time = lidar_end_time; lidar_pushed = true; } - if (last_timestamp_imu < lidar_end_time) - { + if (last_timestamp_imu < lidar_end_time) { return false; } /*** push imu data, and pop from imu buffer ***/ - if (p_imu->imu_need_init_) - { - double imu_time = imu_deque.front()->header.stamp.toSec(); + if (p_imu->imu_need_init_) { + double imu_time = get_time_sec(imu_deque.front()->header.stamp); meas.imu.shrink_to_fit(); - while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) - { - imu_time = imu_deque.front()->header.stamp.toSec(); - if(imu_time > lidar_end_time) break; + while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) { + imu_time = get_time_sec(imu_deque.front()->header.stamp); + if (imu_time > lidar_end_time) break; meas.imu.emplace_back(imu_deque.front()); imu_last = imu_next; imu_last_ptr = imu_deque.front(); imu_next = *(imu_deque.front()); imu_deque.pop_front(); } - } - else if(!init_map) - { - double imu_time = imu_deque.front()->header.stamp.toSec(); + } else if (!init_map) { + double imu_time = get_time_sec(imu_deque.front()->header.stamp); meas.imu.shrink_to_fit(); meas.imu.emplace_back(imu_last_ptr); - while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) - { - imu_time = imu_deque.front()->header.stamp.toSec(); - if(imu_time > lidar_end_time) break; + while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) { + imu_time = get_time_sec(imu_deque.front()->header.stamp); + if (imu_time > lidar_end_time) break; meas.imu.emplace_back(imu_deque.front()); imu_last = imu_next; imu_last_ptr = imu_deque.front(); @@ -507,81 +447,79 @@ bool sync_packages(MeasureGroup &meas) } int process_increments = 0; -void map_incremental() -{ + +void map_incremental() { PointVector PointToAdd; PointVector PointNoNeedDownsample; PointToAdd.reserve(feats_down_size); PointNoNeedDownsample.reserve(feats_down_size); - - for(int i = 0; i < feats_down_size; i++) - { - if (!Nearest_Points[i].empty()) - { - const PointVector &points_near = Nearest_Points[i]; - bool need_add = true; - PointType downsample_result, mid_point; - mid_point.x = floor(feats_down_world->points[i].x/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; - mid_point.y = floor(feats_down_world->points[i].y/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; - mid_point.z = floor(feats_down_world->points[i].z/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; - /* If the nearest points is definitely outside the downsample box */ - if (fabs(points_near[0].x - mid_point.x) > 1.732 * filter_size_map_min || fabs(points_near[0].y - mid_point.y) > 1.732 * filter_size_map_min || fabs(points_near[0].z - mid_point.z) > 1.732 * filter_size_map_min){ - PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); - continue; - } - /* Check if there is a point already in the downsample box */ - float dist = calc_dist(feats_down_world->points[i],mid_point); - for (int readd_i = 0; readd_i < points_near.size(); readd_i ++) - { - /* Those points which are outside the downsample box should not be considered. */ - if (fabs(points_near[readd_i].x - mid_point.x) < 0.5 * filter_size_map_min && fabs(points_near[readd_i].y - mid_point.y) < 0.5 * filter_size_map_min && fabs(points_near[readd_i].z - mid_point.z) < 0.5 * filter_size_map_min) { - need_add = false; - break; - } - } - if (need_add) PointToAdd.emplace_back(feats_down_world->points[i]); - } - else - { - // PointToAdd.emplace_back(feats_down_world->points[i]); + + for (int i = 0; i < feats_down_size; i++) { + if (!Nearest_Points[i].empty()) { + const PointVector &points_near = Nearest_Points[i]; + bool need_add = true; + PointType downsample_result, mid_point; + mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min + + 0.5 * filter_size_map_min; + mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min + + 0.5 * filter_size_map_min; + mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min + + 0.5 * filter_size_map_min; + /* If the nearest points is definitely outside the downsample box */ + if (fabs(points_near[0].x - mid_point.x) > 1.732 * filter_size_map_min || + fabs(points_near[0].y - mid_point.y) > 1.732 * filter_size_map_min || + fabs(points_near[0].z - mid_point.z) > 1.732 * filter_size_map_min) { PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); + continue; } + /* Check if there is a point already in the downsample box */ + float dist = calc_dist(feats_down_world->points[i], mid_point); + for (int readd_i = 0; readd_i < points_near.size(); readd_i++) { + /* Those points which are outside the downsample box should not be considered. */ + if (fabs(points_near[readd_i].x - mid_point.x) < 0.5 * filter_size_map_min && + fabs(points_near[readd_i].y - mid_point.y) < 0.5 * filter_size_map_min && + fabs(points_near[readd_i].z - mid_point.z) < 0.5 * filter_size_map_min) { + need_add = false; + break; + } + } + if (need_add) PointToAdd.emplace_back(feats_down_world->points[i]); + } else { + // PointToAdd.emplace_back(feats_down_world->points[i]); + PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); } + } int add_point_size = ikdtree.Add_Points(PointToAdd, true); ikdtree.Add_Points(PointNoNeedDownsample, false); } -void publish_init_kdtree(const ros::Publisher & pubLaserCloudFullRes) -{ +void publish_init_kdtree(const rclcpp::Publisher::SharedPtr &pubLaserCloudFullRes) { int size_init_ikdtree = ikdtree.size(); - PointCloudXYZI::Ptr laserCloudInit(new PointCloudXYZI(size_init_ikdtree, 1)); + PointCloudXYZI::Ptr laserCloudInit(new PointCloudXYZI(size_init_ikdtree, 1)); - sensor_msgs::PointCloud2 laserCloudmsg; - PointVector ().swap(ikdtree.PCL_Storage); + sensor_msgs::msg::PointCloud2 laserCloudmsg; + PointVector().swap(ikdtree.PCL_Storage); ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); - + laserCloudInit->points = ikdtree.PCL_Storage; pcl::toROSMsg(*laserCloudInit, laserCloudmsg); - - laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); - laserCloudmsg.header.frame_id = "camera_init"; - pubLaserCloudFullRes.publish(laserCloudmsg); + laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes->publish(laserCloudmsg); } PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); -void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes) -{ - if (scan_pub_en) - { + +void publish_frame_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudFullRes) { + if (scan_pub_en) { PointCloudXYZI::Ptr laserCloudFullRes(feats_down_body); int size = laserCloudFullRes->points.size(); - PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); - - for (int i = 0; i < size; i++) - { + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) { // if (i % 3 == 0) // { laserCloudWorld->points[i].x = feats_down_world->points[i].x; @@ -590,38 +528,35 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes) laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; // feats_down_world->points[i].y; // // } } - sensor_msgs::PointCloud2 laserCloudmsg; + sensor_msgs::msg::PointCloud2 laserCloudmsg; pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); - - laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + + laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); laserCloudmsg.header.frame_id = "camera_init"; - pubLaserCloudFullRes.publish(laserCloudmsg); + pubLaserCloudFullRes->publish(laserCloudmsg); publish_count -= PUBFRAME_PERIOD; } - + /**************** save map ****************/ /* 1. make sure you have enough memories /* 2. noted that pcd save will influence the real-time performences **/ - if (pcd_save_en) - { + if (pcd_save_en) { int size = feats_down_world->points.size(); - PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); - for (int i = 0; i < size; i++) - { + for (int i = 0; i < size; i++) { laserCloudWorld->points[i].x = feats_down_world->points[i].x; laserCloudWorld->points[i].y = feats_down_world->points[i].y; laserCloudWorld->points[i].z = feats_down_world->points[i].z; laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; } - + *pcl_wait_save += *laserCloudWorld; static int scan_wait_num = 0; - scan_wait_num ++; - if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) - { - pcd_index ++; + scan_wait_num++; + if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) { + pcd_index++; string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd")); pcl::PCDWriter pcd_writer; cout << "current scan saved to /PCD/" << all_points_dir << endl; @@ -632,30 +567,26 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes) } } -void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body) -{ +void publish_frame_body(const rclcpp::Publisher::SharedPtr &pubLaserCloudFull_body) { int size = feats_undistort->points.size(); PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); - for (int i = 0; i < size; i++) - { + for (int i = 0; i < size; i++) { pointBodyLidarToIMU(&feats_undistort->points[i], \ &laserCloudIMUBody->points[i]); } - sensor_msgs::PointCloud2 laserCloudmsg; + sensor_msgs::msg::PointCloud2 laserCloudmsg; pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); - laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); laserCloudmsg.header.frame_id = "body"; - pubLaserCloudFull_body.publish(laserCloudmsg); + pubLaserCloudFull_body->publish(laserCloudmsg); publish_count -= PUBFRAME_PERIOD; } template -void set_posestamp(T & out) -{ - if (!use_imu_as_input) - { +void set_posestamp(T &out) { + if (!use_imu_as_input) { out.position.x = kf_output.x_.pos(0); out.position.y = kf_output.x_.pos(1); out.position.z = kf_output.x_.pos(2); @@ -663,9 +594,7 @@ void set_posestamp(T & out) out.orientation.y = kf_output.x_.rot.coeffs()[1]; out.orientation.z = kf_output.x_.rot.coeffs()[2]; out.orientation.w = kf_output.x_.rot.coeffs()[3]; - } - else - { + } else { out.position.x = kf_input.x_.pos(0); out.position.y = kf_input.x_.pos(1); out.position.z = kf_input.x_.pos(2); @@ -676,66 +605,62 @@ void set_posestamp(T & out) } } -void publish_odometry(const ros::Publisher & pubOdomAftMapped) -{ +void publish_odometry(const rclcpp::Publisher::SharedPtr &pubOdomAftMapped, + std::shared_ptr &tf_br) { odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.child_frame_id = "aft_mapped"; - if (publish_odometry_without_downsample) - { - odomAftMapped.header.stamp = ros::Time().fromSec(time_current); - } - else - { - odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); + if (publish_odometry_without_downsample) { + odomAftMapped.header.stamp = get_ros_time(time_current); + } else { + odomAftMapped.header.stamp = get_ros_time(lidar_end_time); } set_posestamp(odomAftMapped.pose.pose); - - pubOdomAftMapped.publish(odomAftMapped); - - static tf::TransformBroadcaster br; - tf::Transform transform; - tf::Quaternion q; - transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, \ - odomAftMapped.pose.pose.position.y, \ - odomAftMapped.pose.pose.position.z)); - q.setW(odomAftMapped.pose.pose.orientation.w); - q.setX(odomAftMapped.pose.pose.orientation.x); - q.setY(odomAftMapped.pose.pose.orientation.y); - q.setZ(odomAftMapped.pose.pose.orientation.z); - transform.setRotation( q ); - br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped" ) ); + + pubOdomAftMapped->publish(odomAftMapped); + + //static tf2_ros::TransformBroadcaster br = std::make_shared(*this); + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "camera_init"; + transform.child_frame_id = "aft_mapped"; + transform.transform.translation.x = odomAftMapped.pose.pose.position.x; + transform.transform.translation.y = odomAftMapped.pose.pose.position.y; + transform.transform.translation.z = odomAftMapped.pose.pose.position.z; + transform.transform.rotation.w = odomAftMapped.pose.pose.orientation.w; + transform.transform.rotation.x = odomAftMapped.pose.pose.orientation.x; + transform.transform.rotation.y = odomAftMapped.pose.pose.orientation.y; + transform.transform.rotation.z = odomAftMapped.pose.pose.orientation.z; + transform.header.stamp = odomAftMapped.header.stamp; + tf_br->sendTransform(transform); } -void publish_path(const ros::Publisher pubPath) -{ +void publish_path(const rclcpp::Publisher::SharedPtr &pubPath) { set_posestamp(msg_body_pose.pose); // msg_body_pose.header.stamp = ros::Time::now(); - msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time); + msg_body_pose.header.stamp = get_ros_time(lidar_end_time); msg_body_pose.header.frame_id = "camera_init"; static int jjj = 0; jjj++; // if (jjj % 2 == 0) // if path is too large, the rvis will crash { path.poses.emplace_back(msg_body_pose); - pubPath.publish(path); + pubPath->publish(path); } -} +} -int main(int argc, char** argv) -{ - ros::init(argc, argv, "laserMapping"); - ros::NodeHandle nh("~"); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto nh = std::make_shared("laserMapping"); readParameters(nh); - cout<<"lidar_type: "< 179.9 ? 179.9 : (fov_deg + 10.0); double HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); @@ -743,17 +668,13 @@ int main(int argc, char** argv) memset(point_selected_surf, true, sizeof(point_selected_surf)); downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); - Lidar_T_wrt_IMU< P_init = MD(24,24)::Identity() * 0.01; - P_init.block<3, 3>(21, 21) = MD(3,3)::Identity() * 0.0001; - P_init.block<6, 6>(15, 15) = MD(6,6)::Identity() * 0.001; - P_init.block<6, 6>(6, 6) = MD(6,6)::Identity() * 0.0001; + Eigen::Matrix P_init = MD(24, 24)::Identity() * 0.01; + P_init.block<3, 3>(21, 21) = MD(3, 3)::Identity() * 0.0001; + P_init.block<6, 6>(15, 15) = MD(6, 6)::Identity() * 0.001; + P_init.block<6, 6>(6, 6) = MD(6, 6)::Identity() * 0.0001; kf_input.change_P(P_init); - Eigen::Matrix P_init_output = MD(30,30)::Identity() * 0.01; - P_init_output.block<3, 3>(21, 21) = MD(3,3)::Identity() * 0.0001; - P_init_output.block<6, 6>(6, 6) = MD(6,6)::Identity() * 0.0001; - P_init_output.block<6, 6>(24, 24) = MD(6,6)::Identity() * 0.001; + Eigen::Matrix P_init_output = MD(30, 30)::Identity() * 0.01; + P_init_output.block<3, 3>(21, 21) = MD(3, 3)::Identity() * 0.0001; + P_init_output.block<6, 6>(6, 6) = MD(6, 6)::Identity() * 0.0001; + P_init_output.block<6, 6>(24, 24) = MD(6, 6)::Identity() * 0.001; kf_input.change_P(P_init); kf_output.change_P(P_init_output); Eigen::Matrix Q_input = process_noise_cov_input(); @@ -779,98 +700,95 @@ int main(int argc, char** argv) /*** debug record ***/ FILE *fp; string pos_log_dir = root_dir + "/Log/pos_log.txt"; - fp = fopen(pos_log_dir.c_str(),"w"); + fp = fopen(pos_log_dir.c_str(), "w"); ofstream fout_out, fout_imu_pbp; - fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out); - fout_imu_pbp.open(DEBUG_FILE_DIR("imu_pbp.txt"),ios::out); + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), ios::out); + fout_imu_pbp.open(DEBUG_FILE_DIR("imu_pbp.txt"), ios::out); if (fout_out && fout_imu_pbp) - cout << "~~~~"<lidar_type == AVIA ? \ - nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ - nh.subscribe(lid_topic, 200000, standard_pcl_cbk); - ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); - ros::Publisher pubLaserCloudFullRes = nh.advertise + rclcpp::Subscription::SharedPtr sub_pcl_pc_; + rclcpp::Subscription::SharedPtr sub_pcl_livox_; + if (p_pre->lidar_type == AVIA) { + sub_pcl_livox_ = nh->create_subscription(lid_topic, 20, livox_pcl_cbk); + } else { + sub_pcl_pc_ = nh->create_subscription(lid_topic, rclcpp::SensorDataQoS(), + standard_pcl_cbk); + } + auto sub_imu = nh->create_subscription(imu_topic, 200000, imu_cbk); + auto pubLaserCloudFullRes = nh->create_publisher ("/cloud_registered", 100000); - ros::Publisher pubLaserCloudFullRes_body = nh.advertise + auto pubLaserCloudFullRes_body = nh->create_publisher ("/cloud_registered_body", 100000); - ros::Publisher pubLaserCloudEffect = nh.advertise + auto pubLaserCloudEffect = nh->create_publisher ("/cloud_effected", 100000); - ros::Publisher pubLaserCloudMap = nh.advertise + auto pubLaserCloudMap = nh->create_publisher ("/Laser_map", 100000); - ros::Publisher pubOdomAftMapped = nh.advertise + auto pubOdomAftMapped = nh->create_publisher ("/aft_mapped_to_init", 100000); - ros::Publisher pubPath = nh.advertise + auto pubPath = nh->create_publisher ("/path", 100000); - ros::Publisher plane_pub = nh.advertise - ("/planner_normal", 1000); + //auto plane_pub = nh->create_publisher + // ("/planner_normal", 1000); + auto tf_broadcaster = std::make_shared(nh); //------------------------------------------------------------------------------------------------------ signal(SIGINT, SigHandle); - ros::Rate rate(5000); - bool status = ros::ok(); - while (status) - { + rclcpp::Rate rate(5000); + while (rclcpp::ok()) { if (flg_exit) break; - ros::spinOnce(); - if(sync_packages(Measures)) - { - if (flg_first_scan) - { + //ros::spinOnce(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(nh); + executor.spin_some(); // 处理当前可用的回调 + + if (sync_packages(Measures)) { + if (flg_first_scan) { first_lidar_time = Measures.lidar_beg_time; flg_first_scan = false; cout << "first lidar time" << first_lidar_time << endl; } - if (flg_reset) - { - ROS_WARN("reset when rosbag play back"); + if (flg_reset) { + RCLCPP_WARN(logger, "reset when rosbag play back"); p_imu->Reset(); flg_reset = false; continue; } - double t0,t1,t2,t3,t4,t5,match_start, solve_start; + double t0, t1, t2, t3, t4, t5, match_start, solve_start; match_time = 0; solve_time = 0; propag_time = 0; update_time = 0; t0 = omp_get_wtime(); - + p_imu->Process(Measures, feats_undistort); - if (feats_undistort->empty() || feats_undistort == NULL) - { + if (feats_undistort->empty() || feats_undistort == nullptr) { continue; } - if(imu_en) - { - if (!p_imu->gravity_align_) - { - while (Measures.lidar_beg_time > imu_next.header.stamp.toSec()) - { + if (imu_en) { + if (!p_imu->gravity_align_) { + while (Measures.lidar_beg_time > get_time_sec(imu_next.header.stamp)) { imu_last = imu_next; imu_next = *(imu_deque.front()); imu_deque.pop_front(); // imu_deque.pop(); } - if (non_station_start) - { + if (non_station_start) { state_in.gravity << VEC_FROM_ARRAY(gravity_init); state_out.gravity << VEC_FROM_ARRAY(gravity_init); state_out.acc << VEC_FROM_ARRAY(gravity_init); state_out.acc *= -1; - } - else - { - state_in.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; - state_out.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; + } else { + state_in.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; + state_out.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; state_out.acc = p_imu->mean_acc * G_m_s2 / acc_norm; } - if (gravity_align) - { + if (gravity_align) { Eigen::Matrix3d rot_init; p_imu->gravity_ << VEC_FROM_ARRAY(gravity); p_imu->Set_init(state_in.gravity, rot_init); @@ -883,11 +801,8 @@ int main(int argc, char** argv) kf_input.change_x(state_in); kf_output.change_x(state_out); } - } - else - { - if (!p_imu->gravity_align_) - { + } else { + if (!p_imu->gravity_align_) { state_in.gravity << VEC_FROM_ARRAY(gravity_init); state_out.gravity << VEC_FROM_ARRAY(gravity_init); state_out.acc << VEC_FROM_ARRAY(gravity_init); @@ -898,42 +813,38 @@ int main(int argc, char** argv) lasermap_fov_segment(); /*** downsample the feature points in a scan ***/ t1 = omp_get_wtime(); - if(space_down_sample) - { + if (space_down_sample) { downSizeFilterSurf.setInputCloud(feats_undistort); downSizeFilterSurf.filter(*feats_down_body); - sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); - } - else - { + sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); + } else { feats_down_body = Measures.lidar; - sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); + sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); } time_seq = time_compressing(feats_down_body); feats_down_size = feats_down_body->points.size(); - + /*** initialize the map kdtree ***/ - if(!init_map) - { - if(ikdtree.Root_Node == nullptr) // - // if(feats_down_size > 5) + if (!init_map) { + if (ikdtree.Root_Node == nullptr) // + // if(feats_down_size > 5) { ikdtree.set_downsample_param(filter_size_map_min); } - + feats_down_world->resize(feats_down_size); - for(int i = 0; i < feats_down_size; i++) - { + for (int i = 0; i < feats_down_size; i++) { pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); } for (size_t i = 0; i < feats_down_world->size(); i++) { - init_feats_world->points.emplace_back(feats_down_world->points[i]);} - if(init_feats_world->size() < init_map_size) continue; - ikdtree.Build(init_feats_world->points); + init_feats_world->points.emplace_back(feats_down_world->points[i]); + } + if (init_feats_world->size() < init_map_size) continue; + ikdtree.Build(init_feats_world->points); init_map = true; publish_init_kdtree(pubLaserCloudMap); //(pubLaserCloudFullRes); continue; - } + } /*** ICP and Kalman filter update ***/ normvec->resize(feats_down_size); feats_down_world->resize(feats_down_size); @@ -941,96 +852,85 @@ int main(int argc, char** argv) Nearest_Points.resize(feats_down_size); t2 = omp_get_wtime(); - + /*** iterated state estimation ***/ crossmat_list.reserve(feats_down_size); pbody_list.reserve(feats_down_size); // pbody_ext_list.reserve(feats_down_size); - - for (size_t i = 0; i < feats_down_body->size(); i++) - { + + for (size_t i = 0; i < feats_down_body->size(); i++) { V3D point_this(feats_down_body->points[i].x, - feats_down_body->points[i].y, - feats_down_body->points[i].z); - pbody_list[i]=point_this; - if (extrinsic_est_en) - { - if (!use_imu_as_input) - { + feats_down_body->points[i].y, + feats_down_body->points[i].z); + pbody_list[i] = point_this; + if (extrinsic_est_en) { + if (!use_imu_as_input) { point_this = kf_output.x_.offset_R_L_I.normalized() * point_this + kf_output.x_.offset_T_L_I; - } - else - { + } else { point_this = kf_input.x_.offset_R_L_I.normalized() * point_this + kf_input.x_.offset_T_L_I; } - } - else - { + } else { point_this = Lidar_R_wrt_IMU * point_this + Lidar_T_wrt_IMU; } M3D point_crossmat; point_crossmat << SKEW_SYM_MATRX(point_this); - crossmat_list[i]=point_crossmat; + crossmat_list[i] = point_crossmat; } - - if (!use_imu_as_input) - { + + if (!use_imu_as_input) { bool imu_upda_cov = false; effct_feat_num = 0; /**** point by point update ****/ double pcl_beg_time = Measures.lidar_beg_time; idx = -1; - for (k = 0; k < time_seq.size(); k++) - { - PointType &point_body = feats_down_body->points[idx+time_seq[k]]; + for (k = 0; k < time_seq.size(); k++) { + PointType &point_body = feats_down_body->points[idx + time_seq[k]]; time_current = point_body.curvature / 1000.0 + pcl_beg_time; - if (is_first_frame) - { - if(imu_en) - { - while (time_current > imu_next.header.stamp.toSec()) - { + if (is_first_frame) { + if (imu_en) { + while (time_current > get_time_sec(imu_next.header.stamp)) { imu_last = imu_next; imu_next = *(imu_deque.front()); imu_deque.pop_front(); // imu_deque.pop(); } - angvel_avr< imu_next.header.stamp.toSec(); - while (imu_comes) - { + if (imu_en) { + bool imu_comes = time_current > get_time_sec(imu_next.header.stamp); + while (imu_comes) { imu_upda_cov = true; - angvel_avr< imu_next.header.stamp.toSec(); + time_predict_last_const = get_time_sec(imu_last.header.stamp); // big problem + imu_comes = time_current > get_time_sec(imu_next.header.stamp); // if (!imu_comes) { - double dt_cov = imu_last.header.stamp.toSec() - time_update_last; + double dt_cov = get_time_sec(imu_last.header.stamp) - time_update_last; - if (dt_cov > 0.0) - { - time_update_last = imu_last.header.stamp.toSec(); + if (dt_cov > 0.0) { + time_update_last = get_time_sec(imu_last.header.stamp); double propag_imu_start = omp_get_wtime(); kf_output.predict(dt_cov, Q_output, input_in, false, true); @@ -1046,13 +946,11 @@ int main(int argc, char** argv) double dt = time_current - time_predict_last_const; double propag_state_start = omp_get_wtime(); - if(!prop_at_freq_of_imu) - { + if (!prop_at_freq_of_imu) { double dt_cov = time_current - time_update_last; - if (dt_cov > 0.0) - { + if (dt_cov > 0.0) { kf_output.predict(dt_cov, Q_output, input_in, false, true); - time_update_last = time_current; + time_update_last = time_current; } } kf_output.predict(dt, Q_output, input_in, true, false); @@ -1066,20 +964,17 @@ int main(int argc, char** argv) double t_update_start = omp_get_wtime(); - if (feats_down_size < 1) - { - ROS_WARN("No point, skip this scan!\n"); + if (feats_down_size < 1) { + RCLCPP_WARN(logger, "No point, skip this scan!\n"); idx += time_seq[k]; continue; } - if (!kf_output.update_iterated_dyn_share_modified()) - { - idx = idx+time_seq[k]; + if (!kf_output.update_iterated_dyn_share_modified()) { + idx = idx + time_seq[k]; continue; } - if(prop_at_freq_of_imu) - { + if (prop_at_freq_of_imu) { double dt_cov = time_current - time_update_last; if (!imu_en && (dt_cov >= imu_time_inte)) // (point_cov_not_prop && imu_prop_cov) { @@ -1092,50 +987,46 @@ int main(int argc, char** argv) } solve_start = omp_get_wtime(); - - if (publish_odometry_without_downsample) - { + + if (publish_odometry_without_downsample) { /******* Publish odometry *******/ - publish_odometry(pubOdomAftMapped); - if (runtime_pos_log) - { + publish_odometry(pubOdomAftMapped, tf_broadcaster); + if (runtime_pos_log) { state_out = kf_output.x_; euler_cur = SO3ToEuler(state_out.rot); - fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose() << " " << state_out.vel.transpose() \ - <<" "<points.size()<points.size() << endl; } } - for (int j = 0; j < time_seq[k]; j++) - { - PointType &point_body_j = feats_down_body->points[idx+j+1]; - PointType &point_world_j = feats_down_world->points[idx+j+1]; + for (int j = 0; j < time_seq[k]; j++) { + PointType &point_body_j = feats_down_body->points[idx + j + 1]; + PointType &point_world_j = feats_down_world->points[idx + j + 1]; pointBodyToWorld(&point_body_j, &point_world_j); } - + solve_time += omp_get_wtime() - solve_start; - + update_time += omp_get_wtime() - t_update_start; idx += time_seq[k]; // cout << "pbp output effect feat num:" << effct_feat_num << endl; } - } - else - { + } else { bool imu_prop_cov = false; effct_feat_num = 0; double pcl_beg_time = Measures.lidar_beg_time; idx = -1; - for (k = 0; k < time_seq.size(); k++) - { - PointType &point_body = feats_down_body->points[idx+time_seq[k]]; + for (k = 0; k < time_seq.size(); k++) { + PointType &point_body = feats_down_body->points[idx + time_seq[k]]; time_current = point_body.curvature / 1000.0 + pcl_beg_time; - if (is_first_frame) - { - while (time_current > imu_next.header.stamp.toSec()) - { + if (is_first_frame) { + while (time_current > get_time_sec(imu_next.header.stamp)) { imu_last = imu_next; imu_next = *(imu_deque.front()); imu_deque.pop_front(); @@ -1146,75 +1037,74 @@ int main(int argc, char** argv) is_first_frame = false; t_last = time_current; - time_update_last = time_current; + time_update_last = time_current; // if(prop_at_freq_of_imu) { - input_in.gyro< imu_next.header.stamp.toSec()) // && !imu_deque.empty()) + + while (time_current > get_time_sec(imu_next.header.stamp)) // && !imu_deque.empty()) { imu_last = imu_next; imu_next = *(imu_deque.front()); imu_deque.pop_front(); - input_in.gyro< 0.0) - { - kf_input.predict(dt_cov, Q_input, input_in, false, true); - time_update_last = imu_last.header.stamp.toSec(); //time_current; + double dt_cov = get_time_sec(imu_last.header.stamp) - time_update_last; + if (dt_cov > 0.0) { + kf_input.predict(dt_cov, Q_input, input_in, false, true); + time_update_last = get_time_sec(imu_last.header.stamp); //time_current; } - kf_input.predict(dt, Q_input, input_in, true, false); - t_last = imu_last.header.stamp.toSec(); + kf_input.predict(dt, Q_input, input_in, true, false); + t_last = get_time_sec(imu_last.header.stamp); imu_prop_cov = true; // imu_upda_cov = true; - } + } double dt = time_current - t_last; t_last = time_current; double propag_start = omp_get_wtime(); - - if(!prop_at_freq_of_imu) - { + + if (!prop_at_freq_of_imu) { double dt_cov = time_current - time_update_last; - if (dt_cov > 0.0) - { - kf_input.predict(dt_cov, Q_input, input_in, false, true); - time_update_last = time_current; + if (dt_cov > 0.0) { + kf_input.predict(dt_cov, Q_input, input_in, false, true); + time_update_last = time_current; } } - kf_input.predict(dt, Q_input, input_in, true, false); + kf_input.predict(dt, Q_input, input_in, true, false); propag_time += omp_get_wtime() - propag_start; @@ -1225,17 +1115,15 @@ int main(int argc, char** argv) // } double t_update_start = omp_get_wtime(); - - if (feats_down_size < 1) - { - ROS_WARN("No point, skip this scan!\n"); + + if (feats_down_size < 1) { + RCLCPP_WARN(logger, "No point, skip this scan!\n"); idx += time_seq[k]; continue; } - if (!kf_input.update_iterated_dyn_share_modified()) - { - idx = idx+time_seq[k]; + if (!kf_input.update_iterated_dyn_share_modified()) { + idx = idx + time_seq[k]; continue; } @@ -1253,96 +1141,96 @@ int main(int argc, char** argv) // imu_prop_cov = false; // } // } - if (publish_odometry_without_downsample) - { + if (publish_odometry_without_downsample) { /******* Publish odometry *******/ - publish_odometry(pubOdomAftMapped); - if (runtime_pos_log) - { + publish_odometry(pubOdomAftMapped, tf_broadcaster); + if (runtime_pos_log) { state_in = kf_input.x_; euler_cur = SO3ToEuler(state_in.rot); - fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_in.pos.transpose() << " " << state_in.vel.transpose() \ - <<" "<points.size()<points.size() << endl; } } - for (int j = 0; j < time_seq[k]; j++) - { - PointType &point_body_j = feats_down_body->points[idx+j+1]; - PointType &point_world_j = feats_down_world->points[idx+j+1]; - pointBodyToWorld(&point_body_j, &point_world_j); + for (int j = 0; j < time_seq[k]; j++) { + PointType &point_body_j = feats_down_body->points[idx + j + 1]; + PointType &point_world_j = feats_down_world->points[idx + j + 1]; + pointBodyToWorld(&point_body_j, &point_world_j); } solve_time += omp_get_wtime() - solve_start; - + update_time += omp_get_wtime() - t_update_start; idx = idx + time_seq[k]; - } + } } /******* Publish odometry downsample *******/ - if (!publish_odometry_without_downsample) - { - publish_odometry(pubOdomAftMapped); + if (!publish_odometry_without_downsample) { + publish_odometry(pubOdomAftMapped, tf_broadcaster); } /*** add the feature points to map kdtree ***/ t3 = omp_get_wtime(); - - if(feats_down_size > 4) - { + + if (feats_down_size > 4) { map_incremental(); } t5 = omp_get_wtime(); /******* Publish points *******/ - if (path_en) publish_path(pubPath); - if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes); + if (path_en) publish_path(pubPath); + if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes); if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body); - + /*** Debug variables Logging ***/ - if (runtime_pos_log) - { - frame_num ++; + if (runtime_pos_log) { + frame_num++; aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; - {aver_time_icp = aver_time_icp * (frame_num - 1)/frame_num + update_time/frame_num;} - aver_time_match = aver_time_match * (frame_num - 1)/frame_num + (match_time)/frame_num; - aver_time_solve = aver_time_solve * (frame_num - 1)/frame_num + solve_time/frame_num; - aver_time_propag = aver_time_propag * (frame_num - 1)/frame_num + propag_time / frame_num; + { aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + update_time / frame_num; } + aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num; + aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + solve_time / frame_num; + aver_time_propag = aver_time_propag * (frame_num - 1) / frame_num + propag_time / frame_num; T1[time_log_counter] = Measures.lidar_beg_time; s_plot[time_log_counter] = t5 - t0; s_plot2[time_log_counter] = feats_undistort->points.size(); s_plot3[time_log_counter] = aver_time_consu; - time_log_counter ++; - printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f propogate: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu, aver_time_icp, aver_time_propag); - if (!publish_odometry_without_downsample) - { - if (!use_imu_as_input) - { + time_log_counter++; + printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f propogate: %0.6f \n", + t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp, + aver_time_propag); + if (!publish_odometry_without_downsample) { + if (!use_imu_as_input) { state_out = kf_output.x_; euler_cur = SO3ToEuler(state_out.rot); - fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose() << " " << state_out.vel.transpose() \ - <<" "<points.size()<points.size() << endl; + } else { state_in = kf_input.x_; euler_cur = SO3ToEuler(state_in.rot); - fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_in.pos.transpose() << " " << state_in.vel.transpose() \ - <<" "<points.size()<points.size() << endl; } } dump_lio_state_to_log(fp); } } - status = ros::ok(); rate.sleep(); } //--------------------------save map----------------------------------- /* 1. make sure you have enough memories - /* 2. noted that pcd save will influence the real-time performences **/ - if (pcl_wait_save->size() > 0 && pcd_save_en) - { + 2. noted that pcd save will influence the real-time performences **/ + if (pcl_wait_save->size() > 0 && pcd_save_en) { string file_name = string("scans.pcd"); string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); pcl::PCDWriter pcd_writer; diff --git a/src/parameters.cpp b/src/parameters.cpp index dd140a3..cef2530 100755 --- a/src/parameters.cpp +++ b/src/parameters.cpp @@ -8,83 +8,139 @@ int pcd_index = 0; std::string lid_topic, imu_topic; bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame; bool use_imu_as_input, space_down_sample, publish_odometry_without_downsample; -int init_map_size, con_frame_num; +int init_map_size, con_frame_num; double match_s, satu_acc, satu_gyro, cut_frame_time_interval; -float plane_thr; +float plane_thr; double filter_size_surf_min, filter_size_map_min, fov_deg; -double cube_len; -float DET_RANGE; -bool imu_en, gravity_align, non_station_start; +double cube_len; +float DET_RANGE; +bool imu_en, gravity_align, non_station_start; double imu_time_inte; double laser_point_cov, acc_norm; double vel_cov, acc_cov_input, gyr_cov_input; double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov; -double imu_meas_acc_cov, imu_meas_omg_cov; -int lidar_type, pcd_save_interval; +double imu_meas_acc_cov, imu_meas_omg_cov; +int lidar_type, pcd_save_interval; std::vector gravity_init, gravity; std::vector extrinT; std::vector extrinR; -bool runtime_pos_log, pcd_save_en, path_en, extrinsic_est_en = true; -bool scan_pub_en, scan_body_pub_en; +bool runtime_pos_log, pcd_save_en, path_en, extrinsic_est_en = true; +bool scan_pub_en, scan_body_pub_en; shared_ptr p_pre; double time_lag_imu_to_lidar = 0.0; -void readParameters(ros::NodeHandle &nh) -{ - p_pre.reset(new Preprocess()); - nh.param("prop_at_freq_of_imu", prop_at_freq_of_imu, 1); - nh.param("use_imu_as_input", use_imu_as_input, 1); - nh.param("check_satu", check_satu, 1); - nh.param("init_map_size", init_map_size, 100); - nh.param("space_down_sample", space_down_sample, 1); - nh.param("mapping/satu_acc",satu_acc,3.0); - nh.param("mapping/satu_gyro",satu_gyro,35.0); - nh.param("mapping/acc_norm",acc_norm,1.0); - nh.param("mapping/plane_thr", plane_thr, 0.05f); - nh.param("point_filter_num", p_pre->point_filter_num, 2); - nh.param("common/lid_topic",lid_topic,"/livox/lidar"); - nh.param("common/imu_topic", imu_topic,"/livox/imu"); - nh.param("common/con_frame",con_frame,false); - nh.param("common/con_frame_num",con_frame_num,1); - nh.param("common/cut_frame",cut_frame,false); - nh.param("common/cut_frame_time_interval",cut_frame_time_interval,0.1); - nh.param("common/time_lag_imu_to_lidar",time_lag_imu_to_lidar,0.0); - nh.param("filter_size_surf",filter_size_surf_min,0.5); - nh.param("filter_size_map",filter_size_map_min,0.5); - nh.param("cube_side_length",cube_len,200); - nh.param("mapping/det_range",DET_RANGE,300.f); - nh.param("mapping/fov_degree",fov_deg,180); - nh.param("mapping/imu_en",imu_en,true); - nh.param("mapping/start_in_aggressive_motion",non_station_start,false); - nh.param("mapping/extrinsic_est_en",extrinsic_est_en,true); - nh.param("mapping/imu_time_inte",imu_time_inte,0.005); - nh.param("mapping/lidar_meas_cov",laser_point_cov,0.1); - nh.param("mapping/acc_cov_input",acc_cov_input,0.1); - nh.param("mapping/vel_cov",vel_cov,20); - nh.param("mapping/gyr_cov_input",gyr_cov_input,0.1); - nh.param("mapping/gyr_cov_output",gyr_cov_output,0.1); - nh.param("mapping/acc_cov_output",acc_cov_output,0.1); - nh.param("mapping/b_gyr_cov",b_gyr_cov,0.0001); - nh.param("mapping/b_acc_cov",b_acc_cov,0.0001); - nh.param("mapping/imu_meas_acc_cov",imu_meas_acc_cov,0.1); - nh.param("mapping/imu_meas_omg_cov",imu_meas_omg_cov,0.1); - nh.param("preprocess/blind", p_pre->blind, 1.0); - nh.param("preprocess/lidar_type", lidar_type, 1); - nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); - nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); - nh.param("preprocess/timestamp_unit", p_pre->time_unit, 1); - nh.param("mapping/match_s", match_s, 81); - nh.param("mapping/gravity_align", gravity_align, true); - nh.param>("mapping/gravity", gravity, std::vector()); - nh.param>("mapping/gravity_init", gravity_init, std::vector()); - nh.param>("mapping/extrinsic_T", extrinT, std::vector()); - nh.param>("mapping/extrinsic_R", extrinR, std::vector()); - nh.param("odometry/publish_odometry_without_downsample", publish_odometry_without_downsample, false); - nh.param("publish/path_en",path_en, true); - nh.param("publish/scan_publish_en",scan_pub_en,1); - nh.param("publish/scan_bodyframe_pub_en",scan_body_pub_en,1); - nh.param("runtime_pos_log_enable", runtime_pos_log, 0); - nh.param("pcd_save/pcd_save_en", pcd_save_en, false); - nh.param("pcd_save/interval", pcd_save_interval, -1); +void readParameters(shared_ptr &nh) { + p_pre.reset(new Preprocess()); + + nh->declare_parameter("prop_at_freq_of_imu", true); + nh->declare_parameter("use_imu_as_input", true); + nh->declare_parameter("check_satu", true); + nh->declare_parameter("init_map_size", 100); + nh->declare_parameter("space_down_sample", true); + nh->declare_parameter("mapping.satu_acc", 3.0); + nh->declare_parameter("mapping.satu_gyro", 35.0); + nh->declare_parameter("mapping.acc_norm", 1.0); + nh->declare_parameter("mapping.plane_thr", 0.05f); + nh->declare_parameter("point_filter_num", 2); + nh->declare_parameter("common.lid_topic", "/livox/lidar"); + nh->declare_parameter("common.imu_topic", "/livox/imu"); + nh->declare_parameter("common.con_frame", false); + nh->declare_parameter("common.con_frame_num", 1); + nh->declare_parameter("common.cut_frame", false); + nh->declare_parameter("common.cut_frame_time_interval", 0.1); + nh->declare_parameter("common.time_lag_imu_to_lidar", 0.0); + nh->declare_parameter("filter_size_surf", 0.5); + nh->declare_parameter("filter_size_map", 0.5); + nh->declare_parameter("cube_side_length", 200); + nh->declare_parameter("mapping.det_range", 300.f); + nh->declare_parameter("mapping.fov_degree", 180); + nh->declare_parameter("mapping.imu_en", true); + nh->declare_parameter("mapping.start_in_aggressive_motion", false); + nh->declare_parameter("mapping.extrinsic_est_en", true); + nh->declare_parameter("mapping.imu_time_inte", 0.005); + nh->declare_parameter("mapping.lidar_meas_cov", 0.1); + nh->declare_parameter("mapping.acc_cov_input", 0.1); + nh->declare_parameter("mapping.vel_cov", 20); + nh->declare_parameter("mapping.gyr_cov_input", 0.1); + nh->declare_parameter("mapping.gyr_cov_output", 0.1); + nh->declare_parameter("mapping.acc_cov_output", 0.1); + nh->declare_parameter("mapping.b_gyr_cov", 0.0001); + nh->declare_parameter("mapping.b_acc_cov", 0.0001); + nh->declare_parameter("mapping.imu_meas_acc_cov", 0.1); + nh->declare_parameter("mapping.imu_meas_omg_cov", 0.1); + nh->declare_parameter("preprocess.blind", 1.0); + nh->declare_parameter("preprocess.lidar_type", 1); + nh->declare_parameter("preprocess.scan_line", 16); + nh->declare_parameter("preprocess.scan_rate", 10); + nh->declare_parameter("preprocess.timestamp_unit", 1); + nh->declare_parameter("mapping.match_s", 81); + nh->declare_parameter("mapping.gravity_align", true); + nh->declare_parameter>("mapping.gravity", {0, 0, -9.810}); + nh->declare_parameter>("mapping.gravity_init", {0, 0, -9.810}); + nh->declare_parameter>("mapping.extrinsic_T", {0, 0, 0}); + nh->declare_parameter>("mapping.extrinsic_R", {1, 0, 0, 0, 1, 0, 0, 0, 1}); + nh->declare_parameter("odometry.publish_odometry_without_downsample", false); + nh->declare_parameter("publish.path_en", true); + nh->declare_parameter("publish.scan_publish_en", true); + nh->declare_parameter("publish.scan_bodyframe_pub_en", true); + nh->declare_parameter("runtime_pos_log_enable", false); + nh->declare_parameter("pcd_save.pcd_save_en", false); + nh->declare_parameter("pcd_save.interval", -1); + + // 使用get_parameter方法获取参数值 + nh->get_parameter("prop_at_freq_of_imu", prop_at_freq_of_imu); + nh->get_parameter("use_imu_as_input", use_imu_as_input); + nh->get_parameter("check_satu", check_satu); + nh->get_parameter("init_map_size", init_map_size); + nh->get_parameter("space_down_sample", space_down_sample); + nh->get_parameter("mapping.satu_acc", satu_acc); + nh->get_parameter("mapping.satu_gyro", satu_gyro); + nh->get_parameter("mapping.acc_norm", acc_norm); + nh->get_parameter("mapping.plane_thr", plane_thr); + nh->get_parameter("point_filter_num", p_pre->point_filter_num); + nh->get_parameter("common.lid_topic", lid_topic); + nh->get_parameter("common.imu_topic", imu_topic); + nh->get_parameter("common.con_frame", con_frame); + nh->get_parameter("common.con_frame_num", con_frame_num); + nh->get_parameter("common.cut_frame", cut_frame); + nh->get_parameter("common.cut_frame_time_interval", cut_frame_time_interval); + nh->get_parameter("common.time_lag_imu_to_lidar", time_lag_imu_to_lidar); + nh->get_parameter("filter_size_surf", filter_size_surf_min); + nh->get_parameter("filter_size_map", filter_size_map_min); + nh->get_parameter("cube_side_length", cube_len); + nh->get_parameter("mapping.det_range", DET_RANGE); + nh->get_parameter("mapping.fov_degree", fov_deg); + nh->get_parameter("mapping.imu_en", imu_en); + nh->get_parameter("mapping.start_in_aggressive_motion", non_station_start); + nh->get_parameter("mapping.extrinsic_est_en", extrinsic_est_en); + nh->get_parameter("mapping.imu_time_inte", imu_time_inte); + nh->get_parameter("mapping.lidar_meas_cov", laser_point_cov); + nh->get_parameter("mapping.acc_cov_input", acc_cov_input); + nh->get_parameter("mapping.vel_cov", vel_cov); + nh->get_parameter("mapping.gyr_cov_input", gyr_cov_input); + nh->get_parameter("mapping.gyr_cov_output", gyr_cov_output); + nh->get_parameter("mapping.acc_cov_output", acc_cov_output); + nh->get_parameter("mapping.b_gyr_cov", b_gyr_cov); + nh->get_parameter("mapping.b_acc_cov", b_acc_cov); + nh->get_parameter("mapping.imu_meas_acc_cov", imu_meas_acc_cov); + nh->get_parameter("mapping.imu_meas_omg_cov", imu_meas_omg_cov); + nh->get_parameter("preprocess.blind", p_pre->blind); + nh->get_parameter("preprocess.lidar_type", lidar_type); + nh->get_parameter("preprocess.scan_line", p_pre->N_SCANS); + nh->get_parameter("preprocess.scan_rate", p_pre->SCAN_RATE); + nh->get_parameter("preprocess.timestamp_unit", p_pre->time_unit); + nh->get_parameter("mapping.match_s", match_s); + nh->get_parameter("mapping.gravity_align", gravity_align); + nh->get_parameter("mapping.gravity", gravity); + nh->get_parameter("mapping.gravity_init", gravity_init); + nh->get_parameter("mapping.extrinsic_T", extrinT); + nh->get_parameter("mapping.extrinsic_R", extrinR); + nh->get_parameter("odometry.publish_odometry_without_downsample", publish_odometry_without_downsample); + nh->get_parameter("publish.path_en", path_en); + nh->get_parameter("publish.scan_publish_en", scan_pub_en); + nh->get_parameter("publish.scan_bodyframe_pub_en", scan_body_pub_en); + nh->get_parameter("runtime_pos_log_enable", runtime_pos_log); + nh->get_parameter("pcd_save.pcd_save_en", pcd_save_en); + nh->get_parameter("pcd_save.interval", pcd_save_interval); } diff --git a/src/parameters.h b/src/parameters.h index 88eee85..dcdda0d 100755 --- a/src/parameters.h +++ b/src/parameters.h @@ -1,7 +1,8 @@ // #ifndef PARAM_H // #define PARAM_H #pragma once -#include + +#include #include #include #include @@ -16,25 +17,25 @@ extern std::string lid_topic, imu_topic; extern bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame; extern bool use_imu_as_input, space_down_sample; extern bool extrinsic_est_en, publish_odometry_without_downsample; -extern int init_map_size, con_frame_num; +extern int init_map_size, con_frame_num; extern double match_s, satu_acc, satu_gyro, cut_frame_time_interval; -extern float plane_thr; +extern float plane_thr; extern double filter_size_surf_min, filter_size_map_min, fov_deg; -extern double cube_len; -extern float DET_RANGE; -extern bool imu_en, gravity_align, non_station_start; +extern double cube_len; +extern float DET_RANGE; +extern bool imu_en, gravity_align, non_station_start; extern double imu_time_inte; extern double laser_point_cov, acc_norm; extern double acc_cov_input, gyr_cov_input, vel_cov; extern double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov; -extern double imu_meas_acc_cov, imu_meas_omg_cov; -extern int lidar_type, pcd_save_interval; +extern double imu_meas_acc_cov, imu_meas_omg_cov; +extern int lidar_type, pcd_save_interval; extern std::vector gravity_init, gravity; extern std::vector extrinT; extern std::vector extrinR; -extern bool runtime_pos_log, pcd_save_en, path_en; -extern bool scan_pub_en, scan_body_pub_en; +extern bool runtime_pos_log, pcd_save_en, path_en; +extern bool scan_pub_en, scan_body_pub_en; extern shared_ptr p_pre; extern double time_lag_imu_to_lidar; -void readParameters(ros::NodeHandle &n); +void readParameters(shared_ptr &nh); diff --git a/src/preprocess.cpp b/src/preprocess.cpp index 99b5bb8..b8a2e90 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -4,176 +4,168 @@ #define RETURN0AND1 0x10 Preprocess::Preprocess() - :lidar_type(AVIA), blind(0.01), point_filter_num(1) -{ - inf_bound = 10; - N_SCANS = 6; - SCAN_RATE = 10; - group_size = 8; - disA = 0.01; - disA = 0.1; // B? - p2l_ratio = 225; - limit_maxmid =6.25; - limit_midmin =6.25; - limit_maxmin = 3.24; - jump_up_limit = 170.0; - jump_down_limit = 8.0; - cos160 = 160.0; - edgea = 2; - edgeb = 0.1; - smallp_intersect = 172.5; - smallp_ratio = 1.2; - given_offset_time = false; - - jump_up_limit = cos(jump_up_limit/180*M_PI); - jump_down_limit = cos(jump_down_limit/180*M_PI); - cos160 = cos(cos160/180*M_PI); - smallp_intersect = cos(smallp_intersect/180*M_PI); + : lidar_type(AVIA), blind(0.01), point_filter_num(1) { + inf_bound = 10; + N_SCANS = 6; + SCAN_RATE = 10; + group_size = 8; + disA = 0.01; + disA = 0.1; // B? + p2l_ratio = 225; + limit_maxmid = 6.25; + limit_midmin = 6.25; + limit_maxmin = 3.24; + jump_up_limit = 170.0; + jump_down_limit = 8.0; + cos160 = 160.0; + edgea = 2; + edgeb = 0.1; + smallp_intersect = 172.5; + smallp_ratio = 1.2; + given_offset_time = false; + + jump_up_limit = cos(jump_up_limit / 180 * M_PI); + jump_down_limit = cos(jump_down_limit / 180 * M_PI); + cos160 = cos(cos160 / 180 * M_PI); + smallp_intersect = cos(smallp_intersect / 180 * M_PI); } Preprocess::~Preprocess() {} -void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) -{ - lidar_type = lid_type; - blind = bld; - point_filter_num = pfilt_num; +void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) { + lidar_type = lid_type; + blind = bld; + point_filter_num = pfilt_num; } -void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) -{ - avia_handler(msg); - *pcl_out = pl_surf; +void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) { + avia_handler(msg); + *pcl_out = pl_surf; } -void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) -{ - switch (time_unit) - { - case SEC: - time_unit_scale = 1.e3f; - break; - case MS: - time_unit_scale = 1.f; - break; - case US: - time_unit_scale = 1.e-3f; - break; - case NS: - time_unit_scale = 1.e-6f; - break; - default: - time_unit_scale = 1.f; - break; - } - - switch (lidar_type) - { - case OUST64: - oust64_handler(msg); - break; - - case VELO16: - velodyne_handler(msg); - break; - - case HESAIxt32: - hesai_handler(msg); - break; - - default: - printf("Error LiDAR Type"); - break; - } - *pcl_out = pl_surf; +void Preprocess::process(const sensor_msgs::msg::PointCloud2::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) { + switch (time_unit) { + case SEC: + time_unit_scale = 1.e3f; + break; + case MS: + time_unit_scale = 1.f; + break; + case US: + time_unit_scale = 1.e-3f; + break; + case NS: + time_unit_scale = 1.e-6f; + break; + default: + time_unit_scale = 1.f; + break; + } + + switch (lidar_type) { + case OUST64: + oust64_handler(msg); + break; + + case VELO16: + velodyne_handler(msg); + break; + + case HESAIxt32: + hesai_handler(msg); + break; + + default: + printf("Error LiDAR Type"); + break; + } + *pcl_out = pl_surf; } -void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) -{ - pl_surf.clear(); - pl_corn.clear(); - pl_full.clear(); - double t1 = omp_get_wtime(); - int plsize = msg->point_num; - - pl_corn.reserve(plsize); - pl_surf.reserve(plsize); - pl_full.resize(plsize); - - uint valid_num = 0; - - for(uint i=1; ipoints[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) - { - valid_num ++; - if (valid_num % point_filter_num == 0) - { - pl_full[i].x = msg->points[i].x; - pl_full[i].y = msg->points[i].y; - pl_full[i].z = msg->points[i].z; - pl_full[i].intensity = msg->points[i].reflectivity; - pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms - - if (i==0) pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0; - else pl_full[i].curvature = fabs(pl_full[i].curvature - pl_full[i-1].curvature) < 1.0 ? pl_full[i].curvature : pl_full[i-1].curvature + 0.004166667f; - - if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) - || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) - || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7) - && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind))) - { - pl_surf.push_back(pl_full[i]); +void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg) { + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + double t1 = omp_get_wtime(); + int plsize = msg->point_num; + + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + pl_full.resize(plsize); + + uint valid_num = 0; + + for (uint i = 1; i < plsize; i++) { + if ((msg->points[i].line < N_SCANS) && + ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) { + valid_num++; + if (valid_num % point_filter_num == 0) { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / + float(1000000); // use curvature as time of each laser points, curvature unit: ms + + if (i == 0) pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0; + else pl_full[i].curvature = + fabs(pl_full[i].curvature - pl_full[i - 1].curvature) < 1.0 ? pl_full[i].curvature : + pl_full[i - 1].curvature + 0.004166667f; + + if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) + || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) + || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7) + && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > + (blind * blind))) { + pl_surf.push_back(pl_full[i]); + } + } } - } } - } } -void Preprocess::oust64_handler(const sensor_msgs::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.size(); - pl_corn.reserve(plsize); - pl_surf.reserve(plsize); - - - double time_stamp = msg->header.stamp.toSec(); - // cout << "===================================" << endl; - // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); - for (int i = 0; i < pl_orig.points.size(); i++) - { - if (i % point_filter_num != 0) continue; - - double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; - - if (range < (blind * blind)) continue; - - Eigen::Vector3d pt_vec; - PointType added_pt; - 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.normal_x = 0; - added_pt.normal_y = 0; - added_pt.normal_z = 0; - added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms - - pl_surf.points.push_back(added_pt); - } - - // pub_func(pl_surf, pub_full, msg->header.stamp); - // pub_func(pl_surf, pub_corn, msg->header.stamp); +void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) { + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + + + double time_stamp = rclcpp::Time(msg->header.stamp).seconds(); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) { + if (i % point_filter_num != 0) continue; + + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < (blind * blind)) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + 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.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms + + pl_surf.points.push_back(added_pt); + } + + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); } -void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ +void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); @@ -183,93 +175,80 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) int plsize = pl_orig.points.size(); if (plsize == 0) return; pl_surf.reserve(plsize); - + /*** These variables only works when no point timestamps given ***/ double omega_l = 0.361 * SCAN_RATE; // scan angular velocity - std::vector is_first(N_SCANS,true); + std::vector is_first(N_SCANS, true); std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point std::vector time_last(N_SCANS, 0.0); // last offset time /*****************************************************************/ - if (pl_orig.points[plsize - 1].time > 0) - { - given_offset_time = true; - } - else - { - given_offset_time = false; - double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; - double yaw_end = yaw_first; - int layer_first = pl_orig.points[0].ring; - for (uint i = plsize - 1; i > 0; i--) - { - if (pl_orig.points[i].ring == layer_first) - { - yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; - break; + if (pl_orig.points[plsize - 1].time > 0) { + given_offset_time = true; + } else { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) { + if (pl_orig.points[i].ring == layer_first) { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } } - } } - for (int i = 0; i < plsize; i++) - { - PointType added_pt; - // cout<<"!!!!!!"< (blind * blind)) { + pl_surf.points.push_back(added_pt); + } } - else - { - added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l; - } - - if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l; - - yaw_last[layer] = yaw_angle; - time_last[layer]=added_pt.curvature; - } - - if (i % point_filter_num == 0) - { - 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); - } - } } - + } -void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ +void Preprocess::hesai_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); @@ -279,519 +258,422 @@ void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) int plsize = pl_orig.points.size(); if (plsize == 0) return; pl_surf.reserve(plsize); - + /*** These variables only works when no point timestamps given ***/ double omega_l = 0.361 * SCAN_RATE; // scan angular velocity - std::vector is_first(N_SCANS,true); + std::vector is_first(N_SCANS, true); std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point std::vector time_last(N_SCANS, 0.0); // last offset time /*****************************************************************/ - if (pl_orig.points[plsize - 1].timestamp > 0) - { - given_offset_time = true; - } - else - { - given_offset_time = false; - double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; - double yaw_end = yaw_first; - int layer_first = pl_orig.points[0].ring; - for (uint i = plsize - 1; i > 0; i--) - { - if (pl_orig.points[i].ring == layer_first) - { - yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; - break; + if (pl_orig.points[plsize - 1].timestamp > 0) { + given_offset_time = true; + } else { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) { + if (pl_orig.points[i].ring == layer_first) { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } } - } } double time_head = pl_orig.points[0].timestamp; - - for (int i = 0; i < plsize; i++) - { - PointType added_pt; - // cout<<"!!!!!!"< (blind * blind)) - { - pl_surf.points.push_back(added_pt); + if (i % point_filter_num == 0) { + 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); + } } - } } - + } -void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) -{ - int plsize = pl.size(); - int plsize2; - if(plsize == 0) - { - printf("something wrong\n"); - return; - } - uint head = 0; - - while(types[head].range < blind) - { - head++; - } - - // Surf - plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; - - Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); - Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); - - uint i_nex = 0, i2; - uint last_i = 0; uint last_i_nex = 0; - int last_state = 0; - int plane_type; - - for(uint i=head; i &pl, vector &types) { + int plsize = pl.size(); + int plsize2; + if (plsize == 0) { + printf("something wrong\n"); + return; } + uint head = 0; - i2 = i; + while (types[head].range < blind) { + head++; + } - plane_type = plane_judge(pl, types, i, i_nex, curr_direct); - - if(plane_type == 1) - { - for(uint j=i; j<=i_nex; j++) - { - if(j!=i && j!=i_nex) - { - types[j].ftype = Real_Plane; - } - else - { - types[j].ftype = Poss_Plane; - } - } - - // if(last_state==1 && fabs(last_direct.sum())>0.5) - if(last_state==1 && last_direct.norm()>0.1) - { - double mod = last_direct.transpose() * curr_direct; - if(mod>-0.707 && mod<0.707) - { - types[i].ftype = Edge_Plane; + // Surf + plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex = 0, i2; + uint last_i = 0; + uint last_i_nex = 0; + int last_state = 0; + int plane_type; + + for (uint i = head; i < plsize2; i++) { + if (types[i].range < blind) { + continue; } - else + + i2 = i; + + plane_type = plane_judge(pl, types, i, i_nex, curr_direct); + + if (plane_type == 1) { + for (uint j = i; j <= i_nex; j++) { + if (j != i && j != i_nex) { + types[j].ftype = Real_Plane; + } else { + types[j].ftype = Poss_Plane; + } + } + + // if(last_state==1 && fabs(last_direct.sum())>0.5) + if (last_state == 1 && last_direct.norm() > 0.1) { + double mod = last_direct.transpose() * curr_direct; + if (mod > -0.707 && mod < 0.707) { + types[i].ftype = Edge_Plane; + } else { + types[i].ftype = Real_Plane; + } + } + + i = i_nex - 1; + last_state = 1; + } else // if(plane_type == 2) { - types[i].ftype = Real_Plane; + i = i_nex; + last_state = 0; } - } - - i = i_nex - 1; - last_state = 1; - } - else // if(plane_type == 2) - { - i = i_nex; - last_state = 0; - } - - last_i = i2; - last_i_nex = i_nex; - last_direct = curr_direct; - } - - plsize2 = plsize > 3 ? plsize - 3 : 0; - for(uint i=head+3; i=Real_Plane) - { - continue; - } - if(types[i-1].dista<1e-16 || types[i].dista<1e-16) - { - continue; + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; } - Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); - Eigen::Vector3d vecs[2]; - - for(int j=0; j<2; j++) - { - int m = -1; - if(j == 1) - { - m = 1; - } + plsize2 = plsize > 3 ? plsize - 3 : 0; + for (uint i = head + 3; i < plsize2; i++) { + if (types[i].range < blind || types[i].ftype >= Real_Plane) { + continue; + } - if(types[i+m].range < blind) - { - if(types[i].range > inf_bound) - { - types[i].edj[j] = Nr_inf; + if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) { + continue; } - else - { - types[i].edj[j] = Nr_blind; + + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; + + for (int j = 0; j < 2; j++) { + int m = -1; + if (j == 1) { + m = 1; + } + + if (types[i + m].range < blind) { + if (types[i].range > inf_bound) { + types[i].edj[j] = Nr_inf; + } else { + types[i].edj[j] = Nr_blind; + } + continue; + } + + vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z); + vecs[j] = vecs[j] - vec_a; + + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if (types[i].angle[j] < jump_up_limit) { + types[i].edj[j] = Nr_180; + } else if (types[i].angle[j] > jump_down_limit) { + types[i].edj[j] = Nr_zero; + } } - continue; - } - - vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z); - vecs[j] = vecs[j] - vec_a; - - types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); - if(types[i].angle[j] < jump_up_limit) - { - types[i].edj[j] = Nr_180; - } - else if(types[i].angle[j] > jump_down_limit) - { - types[i].edj[j] = Nr_zero; - } - } - types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); - if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista) - { - if(types[i].intersect > cos160) - { - if(edge_jump_judge(pl, types, i, Prev)) - { - types[i].ftype = Edge_Jump; + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && + types[i].dista > 4 * types[i - 1].dista) { + if (types[i].intersect > cos160) { + if (edge_jump_judge(pl, types, i, Prev)) { + types[i].ftype = Edge_Jump; + } + } + } else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && + types[i - 1].dista > 4 * types[i].dista) { + if (types[i].intersect > cos160) { + if (edge_jump_judge(pl, types, i, Next)) { + types[i].ftype = Edge_Jump; + } + } + } else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) { + if (edge_jump_judge(pl, types, i, Prev)) { + types[i].ftype = Edge_Jump; + } + } else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) { + if (edge_jump_judge(pl, types, i, Next)) { + types[i].ftype = Edge_Jump; + } + + } else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) { + if (types[i].ftype == Nor) { + types[i].ftype = Wire; + } } - } } - else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista) - { - if(types[i].intersect > cos160) - { - if(edge_jump_judge(pl, types, i, Next)) - { - types[i].ftype = Edge_Jump; + + plsize2 = plsize - 1; + double ratio; + for (uint i = head + 1; i < plsize2; i++) { + if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind) { + continue; } - } - } - else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf) - { - if(edge_jump_judge(pl, types, i, Prev)) - { - types[i].ftype = Edge_Jump; - } - } - else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor) - { - if(edge_jump_judge(pl, types, i, Next)) - { - types[i].ftype = Edge_Jump; - } - - } - else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor) - { - if(types[i].ftype == Nor) - { - types[i].ftype = Wire; - } - } - } - - plsize2 = plsize-1; - double ratio; - for(uint i=head+1; i types[i].dista) - { - ratio = types[i-1].dista / types[i].dista; - } - else - { - ratio = types[i].dista / types[i-1].dista; - } - - if(types[i].intersect types[i].dista) { + ratio = types[i - 1].dista / types[i].dista; + } else { + ratio = types[i].dista / types[i - 1].dista; + } + + if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) { + if (types[i - 1].ftype == Nor) { + types[i - 1].ftype = Real_Plane; + } + if (types[i + 1].ftype == Nor) { + types[i + 1].ftype = Real_Plane; + } + types[i].ftype = Real_Plane; + } } - types[i].ftype = Real_Plane; - } } - } - - int last_surface = -1; - for(uint j=head; j &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) -{ - double group_dis = disA*types[i_cur].range + disB; - group_dis = group_dis * group_dis; - // i_nex = i_cur; - - double two_dis; - vector disarr; - disarr.reserve(20); - - for(i_nex=i_cur; i_nex &types, uint i_cur, uint &i_nex, + Eigen::Vector3d &curr_direct) { + double group_dis = disA * types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) { + if (types[i_nex].range < blind) { + curr_direct.setZero(); + return 2; + } + disarr.push_back(types[i_nex].dista); } - disarr.push_back(types[i_nex].dista); - } - - for(;;) - { - if((i_cur >= pl.size()) || (i_nex >= pl.size())) break; - - if(types[i_nex].range < blind) - { - curr_direct.setZero(); - return 2; + + for (;;) { + if ((i_cur >= pl.size()) || (i_nex >= pl.size())) break; + + if (types[i_nex].range < blind) { + curr_direct.setZero(); + return 2; + } + vx = pl[i_nex].x - pl[i_cur].x; + vy = pl[i_nex].y - pl[i_cur].y; + vz = pl[i_nex].z - pl[i_cur].z; + two_dis = vx * vx + vy * vy + vz * vz; + if (two_dis >= group_dis) { + break; + } + disarr.push_back(types[i_nex].dista); + i_nex++; } - vx = pl[i_nex].x - pl[i_cur].x; - vy = pl[i_nex].y - pl[i_cur].y; - vz = pl[i_nex].z - pl[i_cur].z; - two_dis = vx*vx + vy*vy + vz*vz; - if(two_dis >= group_dis) - { - break; + + double leng_wid = 0; + double v1[3], v2[3]; + for (uint j = i_cur + 1; j < i_nex; j++) { + if ((j >= pl.size()) || (i_cur >= pl.size())) break; + v1[0] = pl[j].x - pl[i_cur].x; + v1[1] = pl[j].y - pl[i_cur].y; + v1[2] = pl[j].z - pl[i_cur].z; + + v2[0] = v1[1] * vz - vy * v1[2]; + v2[1] = v1[2] * vx - v1[0] * vz; + v2[2] = v1[0] * vy - vx * v1[1]; + + double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]; + if (lw > leng_wid) { + leng_wid = lw; + } } - disarr.push_back(types[i_nex].dista); - i_nex++; - } - - double leng_wid = 0; - double v1[3], v2[3]; - for(uint j=i_cur+1; j= pl.size()) || (i_cur >= pl.size())) break; - v1[0] = pl[j].x - pl[i_cur].x; - v1[1] = pl[j].y - pl[i_cur].y; - v1[2] = pl[j].z - pl[i_cur].z; - - v2[0] = v1[1]*vz - vy*v1[2]; - v2[1] = v1[2]*vx - v1[0]*vz; - v2[2] = v1[0]*vy - vx*v1[1]; - - double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]; - if(lw > leng_wid) - { - leng_wid = lw; + + + if ((two_dis * two_dis / leng_wid) < p2l_ratio) { + curr_direct.setZero(); + return 0; } - } - - - if((two_dis*two_dis/leng_wid) < p2l_ratio) - { - curr_direct.setZero(); - return 0; - } - - uint disarrsize = disarr.size(); - for(uint j=0; j=limit_maxmid || dismid_min>=limit_midmin) - { - curr_direct.setZero(); - return 0; + + if (disarr[disarr.size() - 2] < 1e-16) { + curr_direct.setZero(); + return 0; } - } - else - { - double dismax_min = disarr[0] / disarr[disarrsize-2]; - if(dismax_min >= limit_maxmin) - { - curr_direct.setZero(); - return 0; + + if (lidar_type == AVIA) { + double dismax_mid = disarr[0] / disarr[disarrsize / 2]; + double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2]; + + if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) { + curr_direct.setZero(); + return 0; + } + } else { + double dismax_min = disarr[0] / disarr[disarrsize - 2]; + if (dismax_min >= limit_maxmin) { + curr_direct.setZero(); + return 0; + } } - } - - curr_direct << vx, vy, vz; - curr_direct.normalize(); - return 1; + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; } -bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) -{ - if(nor_dir == 0) - { - if(types[i-1].range &types, uint i, Surround nor_dir) { + if (nor_dir == 0) { + if (types[i - 1].range < blind || types[i - 2].range < blind) { + return false; + } + } else if (nor_dir == 1) { + if (types[i + 1].range < blind || types[i + 2].range < blind) { + return false; + } } - } - else if(nor_dir == 1) - { - if(types[i+1].rangeedgea*d2 || (d1-d2)>edgeb) - { - return false; - } - - return true; + + d1 = sqrt(d1); + d2 = sqrt(d2); + + if (d1 > edgea * d2 || (d1 - d2) > edgeb) { + return false; + } + + return true; } diff --git a/src/preprocess.h b/src/preprocess.h index 3d68371..98ff3a5 100755 --- a/src/preprocess.h +++ b/src/preprocess.h @@ -1,9 +1,9 @@ #pragma once -#include +#include #include -#include -#include +#include +#include using namespace std; @@ -12,134 +12,158 @@ using namespace std; typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud PointCloudXYZI; -enum LID_TYPE{AVIA = 1, VELO16, OUST64, HESAIxt32}; //{1, 2, 3, 4} -enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; -enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; -enum Surround{Prev, Next}; -enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; - -struct orgtype -{ - double range; - double dista; - double angle[2]; - double intersect; - E_jump edj[2]; - Feature ftype; - orgtype() - { - range = 0; - edj[Prev] = Nr_nor; - edj[Next] = Nr_nor; - ftype = Nor; - intersect = 2; - } +enum LID_TYPE { + AVIA = 1, VELO16, OUST64, HESAIxt32 +}; //{1, 2, 3, 4} +enum TIME_UNIT { + SEC = 0, MS = 1, US = 2, NS = 3 +}; +enum Feature { + Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint +}; +enum Surround { + Prev, Next +}; +enum E_jump { + Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind +}; + +struct orgtype { + double range; + double dista; + double angle[2]; + double intersect; + E_jump edj[2]; + Feature ftype; + + orgtype() { + range = 0; + edj[Prev] = Nr_nor; + edj[Next] = Nr_nor; + ftype = Nor; + intersect = 2; + } }; namespace velodyne_ros { - struct EIGEN_ALIGN16 Point { - PCL_ADD_POINT4D; - float intensity; - float time; - uint16_t ring; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + float time; + uint16_t ring; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; } // namespace velodyne_ros POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) - (float, time, time) - (std::uint16_t, ring, ring) + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (float, time, time) + (std::uint16_t, ring, ring) ) namespace hesai_ros { - struct EIGEN_ALIGN16 Point { - PCL_ADD_POINT4D; - float intensity; - double timestamp; - uint16_t ring; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + double timestamp; + uint16_t ring; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; } // namespace velodyne_ros POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) - (double, timestamp, timestamp) - (std::uint16_t, ring, ring) + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (double, timestamp, timestamp) + (std::uint16_t, ring, ring) ) namespace ouster_ros { - struct EIGEN_ALIGN16 Point { - PCL_ADD_POINT4D; - float intensity; - uint32_t t; - uint16_t reflectivity; - uint8_t ring; - uint16_t ambient; - uint32_t range; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + uint32_t t; + uint16_t reflectivity; + uint8_t ring; + uint16_t ambient; + uint32_t range; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; } // namespace ouster_ros // clang-format off POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) - // use std::uint32_t to avoid conflicting with pcl::uint32_t - (std::uint32_t, t, t) - (std::uint16_t, reflectivity, reflectivity) - (std::uint8_t, ring, ring) - (std::uint16_t, ambient, ambient) - (std::uint32_t, range, range) + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + // use std::uint32_t to avoid conflicting with pcl::uint32_t + (std::uint32_t, t, t) + (std::uint16_t, reflectivity, reflectivity) + (std::uint8_t, ring, ring) + (std::uint16_t, ambient, ambient) + (std::uint32_t, range, range) ) -class Preprocess -{ - public: +class Preprocess { +public: // EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Preprocess(); - ~Preprocess(); - - void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); - void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); - void set(bool feat_en, int lid_type, double bld, int pfilt_num); - - // sensor_msgs::PointCloud2::ConstPtr pointcloud; - PointCloudXYZI pl_full, pl_corn, pl_surf; - PointCloudXYZI pl_buff[128]; //maximum 128 line lidar - vector typess[128]; //maximum 128 line lidar - float time_unit_scale; - int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; - double blind; - bool given_offset_time; - ros::Publisher pub_full, pub_surf, pub_corn; - - - private: - void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); - void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void give_feature(PointCloudXYZI &pl, vector &types); - void pub_func(PointCloudXYZI &pl, const ros::Time &ct); - int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); - bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); - bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); - - int group_size; - double disA, disB, inf_bound; - double limit_maxmid, limit_midmin, limit_maxmin; - double p2l_ratio; - double jump_up_limit, jump_down_limit; - double cos160; - double edgea, edgeb; - double smallp_intersect, smallp_ratio; - double vx, vy, vz; + Preprocess(); + + ~Preprocess(); + + void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); + + void process(const sensor_msgs::msg::PointCloud2::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); + + void set(bool feat_en, int lid_type, double bld, int pfilt_num); + + // sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud; + PointCloudXYZI pl_full, pl_corn, pl_surf; + PointCloudXYZI pl_buff[128]; //maximum 128 line lidar + vector typess[128]; //maximum 128 line lidar + float time_unit_scale; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; + double blind; + bool given_offset_time; + //ros::Publisher pub_full, pub_surf, pub_corn; + + +private: + void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg); + + void oust64_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg); + + void velodyne_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg); + + void hesai_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg); + + void give_feature(PointCloudXYZI &pl, vector &types); + + void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct); + + int + plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); + + bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, + Eigen::Vector3d &curr_direct); + + bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); + + int group_size; + double disA, disB, inf_bound; + double limit_maxmid, limit_midmin, limit_maxmin; + double p2l_ratio; + double jump_up_limit, jump_down_limit; + double cos160; + double edgea, edgeb; + double smallp_intersect, smallp_ratio; + double vx, vy, vz; };