From a6b34af4eede621c063413257099a19866ac9e11 Mon Sep 17 00:00:00 2001 From: Christoph Gruber Date: Thu, 10 Jun 2021 14:27:34 +0200 Subject: [PATCH 01/15] migration to ROS2 --- CMakeLists.txt | 162 +++--- LICENSE | 1 + README.md | 81 ++- config/params.yaml | 254 ++++----- .../config => config}/robot.urdf.xacro | 15 +- config/rviz2.rviz | 263 +++++++++ include/lio_sam/utility.hpp | 442 +++++++++++++++ include/utility.h | 342 ------------ launch/include/config/rviz.rviz | 527 ------------------ launch/include/module_loam.launch | 10 - launch/include/module_navsat.launch | 20 - .../module_robot_state_publisher.launch | 11 - launch/include/module_rviz.launch | 8 - .../include/rosconsole/rosconsole_error.conf | 2 - .../include/rosconsole/rosconsole_info.conf | 2 - .../include/rosconsole/rosconsole_warn.conf | 2 - launch/run.launch | 20 - launch/run.launch.py | 73 +++ msg/CloudInfo.msg | 29 + msg/cloud_info.msg | 29 - package.xml | 72 +-- src/featureExtraction.cpp | 107 ++-- src/imageProjection.cpp | 181 +++--- src/imuPreintegration.cpp | 225 ++++---- src/mapOptmization.cpp | 407 +++++++------- srv/save_map.srv | 4 - 26 files changed, 1528 insertions(+), 1761 deletions(-) rename {launch/include/config => config}/robot.urdf.xacro (70%) create mode 100644 config/rviz2.rviz create mode 100644 include/lio_sam/utility.hpp delete mode 100644 include/utility.h delete mode 100644 launch/include/config/rviz.rviz delete mode 100644 launch/include/module_loam.launch delete mode 100644 launch/include/module_navsat.launch delete mode 100644 launch/include/module_robot_state_publisher.launch delete mode 100644 launch/include/module_rviz.launch delete mode 100644 launch/include/rosconsole/rosconsole_error.conf delete mode 100644 launch/include/rosconsole/rosconsole_info.conf delete mode 100644 launch/include/rosconsole/rosconsole_warn.conf delete mode 100644 launch/run.launch create mode 100644 launch/run.launch.py create mode 100644 msg/CloudInfo.msg delete mode 100644 msg/cloud_info.msg delete mode 100644 srv/save_map.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index a260eab6..04be5d63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,102 +1,102 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(lio_sam) -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++11") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") - -find_package(catkin REQUIRED COMPONENTS - tf - roscpp - rospy - cv_bridge - # pcl library - pcl_conversions - # msgs - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - message_generation - visualization_msgs +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Release) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(OpenCV REQUIRED) +find_package(PCL REQUIRED) +find_package(GTSAM REQUIRED) +find_package(Eigen REQUIRED) + +include_directories( + include/lio_sam ) -find_package(OpenMP REQUIRED) -find_package(PCL REQUIRED QUIET) -find_package(OpenCV REQUIRED QUIET) -find_package(GTSAM REQUIRED QUIET) +rosidl_generate_interfaces(${PROJECT_NAME} "msg/CloudInfo.msg" DEPENDENCIES std_msgs sensor_msgs) + +add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp) +ament_target_dependencies(${PROJECT_NAME}_featureExtraction rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) +rosidl_target_interfaces(${PROJECT_NAME}_featureExtraction ${PROJECT_NAME} "rosidl_typesupport_cpp") + +add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp) +ament_target_dependencies(${PROJECT_NAME}_imageProjection rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) +rosidl_target_interfaces(${PROJECT_NAME}_imageProjection ${PROJECT_NAME} "rosidl_typesupport_cpp") -add_message_files( - DIRECTORY msg - FILES - cloud_info.msg +add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp) +ament_target_dependencies(${PROJECT_NAME}_imuPreintegration rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL GTSAM Eigen) +target_link_libraries(${PROJECT_NAME}_imuPreintegration gtsam) +rosidl_target_interfaces(${PROJECT_NAME}_imuPreintegration ${PROJECT_NAME} "rosidl_typesupport_cpp") + +add_executable(${PROJECT_NAME}_mapOptimization src/mapOptmization.cpp) +ament_target_dependencies(${PROJECT_NAME}_mapOptimization rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL GTSAM) +target_link_libraries(${PROJECT_NAME}_mapOptimization gtsam) +rosidl_target_interfaces(${PROJECT_NAME}_mapOptimization ${PROJECT_NAME} "rosidl_typesupport_cpp") + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ ) -add_service_files( - DIRECTORY srv - FILES - save_map.srv +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME}/ ) -generate_messages( - DEPENDENCIES - geometry_msgs - std_msgs - nav_msgs - sensor_msgs +install( + TARGETS ${PROJECT_NAME}_imageProjection + DESTINATION lib/${PROJECT_NAME} ) -catkin_package( - INCLUDE_DIRS include - DEPENDS PCL GTSAM - - CATKIN_DEPENDS - std_msgs - nav_msgs - geometry_msgs - sensor_msgs - message_runtime - message_generation - visualization_msgs +install( + TARGETS ${PROJECT_NAME}_imuPreintegration + DESTINATION lib/${PROJECT_NAME} ) -# include directories -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${GTSAM_INCLUDE_DIR} +install( + TARGETS ${PROJECT_NAME}_featureExtraction + DESTINATION lib/${PROJECT_NAME} ) -# link directories -link_directories( - include - ${PCL_LIBRARY_DIRS} - ${OpenCV_LIBRARY_DIRS} - ${GTSAM_LIBRARY_DIRS} +install( + TARGETS ${PROJECT_NAME}_mapOptimization + DESTINATION lib/${PROJECT_NAME} ) -########### -## Build ## -########### +install( + DIRECTORY "include/" + DESTINATION include +) -# Range Image Projection -add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp) -add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) -target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) +ament_export_include_directories(include) -# Feature Association -add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp) -add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) -target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() -# Mapping Optimization -add_executable(${PROJECT_NAME}_mapOptmization src/mapOptmization.cpp) -add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) -target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS}) -target_link_libraries(${PROJECT_NAME}_mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam) +ament_package() -# IMU Preintegration -add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp) -target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam) diff --git a/LICENSE b/LICENSE index 5df67dcf..d4c5d8db 100755 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,7 @@ BSD 3-Clause License Copyright (c) 2020, Tixiao Shan +Copyright (c) 2021, Christoph Gruber All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/README.md b/README.md index 1df3ea4b..2bc31da6 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,8 @@ - [**System architecture**](#system-architecture) + - [**Notes on ROS2 branch**](#notes-on-ros2-branch) + - [**Package dependency**](#dependency) - [**Package install**](#install) @@ -51,34 +53,47 @@ We design a system that maintains two graphs and runs up to 10x faster than real - The factor graph in "mapOptimization.cpp" optimizes lidar odometry factor and GPS factor. This factor graph is maintained consistently throughout the whole test. - The factor graph in "imuPreintegration.cpp" optimizes IMU and lidar odometry factor and estimates IMU bias. This factor graph is reset periodically and guarantees real-time odometry estimation at IMU frequency. +## Notes on ROS2 branch + +There are some features of the original ROS1 version that are currently missing in this ROS2 version, namely: +- The service for saving maps +- Testing with Velodyne lidars and Microstrain IMUs +- A launch file for the navsat module/GPS factor +- The rviz2 configuration misses many elements + +This branch was tested with an Ouster lidar and a Xsens IMU using the following ROS2 drivers: +- [ros2_ouster_drivers](https://github.com/ros-drivers/ros2_ouster_drivers) +- [bluespace_ai_xsens_ros_mti_driver](https://github.com/bluespace-ai/bluespace_ai_xsens_ros_mti_driver) + +In these tests, the IMU was mounted on the bottom of the lidar such that their x-axes pointed in the same direction. The parameters `extrinsicRot` and `extrinsicRPY` in `params.yaml` correspond to this constellation. + ## Dependency -- [ROS](http://wiki.ros.org/ROS/Installation) (tested with Kinetic and Melodic) +- [ROS2](https://docs.ros.org/en/foxy/Installation.html) (tested with Foxy on Ubuntu 20.04) ``` - sudo apt-get install -y ros-kinetic-navigation - sudo apt-get install -y ros-kinetic-robot-localization - sudo apt-get install -y ros-kinetic-robot-state-publisher + sudo apt install ros-foxy-perception-pcl \ + ros-foxy-pcl-msgs \ + ros-foxy-vision-opencv \ + ros-foxy-xacro ``` - [gtsam](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library) ``` - wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.2.zip - cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/ - cd ~/Downloads/gtsam-4.0.2/ - mkdir build && cd build - cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. - sudo make install -j8 + # Add GTSAM-PPA + sudo add-apt-repository ppa:borglab/gtsam-release-4.0 + sudo apt install libgtsam-dev libgtsam-unstable-dev ``` ## Install Use the following commands to download and compile the package. -``` -cd ~/catkin_ws/src -git clone https://github.com/TixiaoShan/LIO-SAM.git -cd .. -catkin_make -``` + ``` + cd ~/ros2_ws/src + git clone https://github.com/TixiaoShan/LIO-SAM.git + git checkout ros2 + cd .. + colcon build + ``` ## Prepare lidar data @@ -106,34 +121,20 @@ The user needs to prepare the point cloud data in the correct format for cloud d ## Sample datasets - * Download some sample datasets to test the functionality of the package. The datasets below is configured to run using the default settings: - - **Walking dataset:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)] - - **Park dataset:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)] - - **Garden dataset:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)] +For data protection reasons, no data set can currently be made available for ROS2. - * The datasets below need the parameters to be configured. In these datasets, the point cloud topic is "points_raw." The IMU topic is "imu_correct," which gives the IMU data in ROS REP105 standard. Because no IMU transformation is needed for this dataset, the following configurations need to be changed to run this dataset successfully: - - The "imuTopic" parameter in "config/params.yaml" needs to be set to "imu_correct". - - The "extrinsicRot" and "extrinsicRPY" in "config/params.yaml" needs to be set as identity matrices. - - **Rotation dataset:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)] - - **Campus dataset (large):** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)] - - **Campus dataset (small):** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)] - - * Ouster (OS1-128) dataset. No extrinsics need to be changed for this dataset if you are using the default settings. Please follow the Ouster notes below to configure the package to run with Ouster data. A video of the dataset can be found on [YouTube](https://youtu.be/O7fKgZQzkEo): - - **Rooftop dataset:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)] - - * KITTI dataset. The extrinsics can be found in the Notes KITTI section below. To generate more bags using other KITTI raw data, you can use the python script provided in "config/doc/kitti2bag". - - **2011_09_30_drive_0028:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)] +README.md of the master branch contains some links to ROS1 rosbags. Using [ros1_bridge](https://github.com/ros2/ros1_bridge) with these rosbags and this ROS2 branch of LIO-SAM did however not succeeed for timing reasons. ## Run the package 1. Run the launch file: ``` -roslaunch lio_sam run.launch +ros2 launch lio_sam run.launch.py ``` 2. Play existing bag files: ``` -rosbag play your-bag.bag -r 3 +ros2 bag play your-bag.bag ``` ## Other notes @@ -179,17 +180,6 @@ rosbag play your-bag.bag -r 3 drawing

-## Service - - /lio_sam/save_map - - save map as a PCD file. - ``` bash - rosservice call [service] [resolution] [destination] - ``` - - Example: - ``` bash - $ rosservice call /lio_sam/save_map 0.2 "/Downloads/LOAM/" - ``` - ## Issues - **Zigzag or jerking behavior**: if your lidar and IMU data formats are consistent with the requirement of LIO-SAM, this problem is likely caused by un-synced timestamp of lidar and IMU data. @@ -233,7 +223,6 @@ Part of the code is adapted from [LeGO-LOAM](https://github.com/RobustFieldAuton ## Related Package - [Lidar-IMU calibration](https://github.com/chennuo0125-HIT/lidar_imu_calib) - - [LIO-SAM with ROS2](https://github.com/CAKGOD/lio_sam_ros2) - [LIO-SAM with Scan Context](https://github.com/gisbi-kim/SC-LIO-SAM) ## Acknowledgement diff --git a/config/params.yaml b/config/params.yaml index 23f87558..567c6215 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -1,161 +1,93 @@ -lio_sam: - - # Topics - pointCloudTopic: "points_raw" # Point cloud data - imuTopic: "imu_raw" # IMU data - odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU - gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file - - # Frames - lidarFrame: "base_link" - baselinkFrame: "base_link" - odometryFrame: "odom" - mapFrame: "map" - - # GPS Settings - useImuHeadingInitialization: true # if using GPS data, set to "true" - useGpsElevation: false # if GPS elevation is bad, set to "false" - gpsCovThreshold: 2.0 # m^2, threshold for using GPS data - poseCovThreshold: 25.0 # m^2, threshold for using GPS data - - # Export settings - savePCD: false # https://github.com/TixiaoShan/LIO-SAM/issues/3 - savePCDDirectory: "/Downloads/LOAM/" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation - - # Sensor Settings - sensor: velodyne # lidar sensor type, either 'velodyne' or 'ouster' - N_SCAN: 16 # number of lidar channel (i.e., 16, 32, 64, 128) - Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) - downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 - lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used - lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used - - # IMU Settings - imuAccNoise: 3.9939570888238808e-03 - imuGyrNoise: 1.5636343949698187e-03 - imuAccBiasN: 6.4356659353532566e-05 - imuGyrBiasN: 3.5640318696367613e-05 - imuGravity: 9.80511 - imuRPYWeight: 0.01 - - # Extrinsics (lidar -> IMU) - extrinsicTrans: [0.0, 0.0, 0.0] - extrinsicRot: [-1, 0, 0, - 0, 1, 0, - 0, 0, -1] - extrinsicRPY: [0, 1, 0, - -1, 0, 0, - 0, 0, 1] - # extrinsicRot: [1, 0, 0, - # 0, 1, 0, - # 0, 0, 1] - # extrinsicRPY: [1, 0, 0, - # 0, 1, 0, - # 0, 0, 1] - - # LOAM feature threshold - edgeThreshold: 1.0 - surfThreshold: 0.1 - edgeFeatureMinValidNum: 10 - surfFeatureMinValidNum: 100 - - # voxel filter paprams - odometrySurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor - mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor - mappingSurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor - - # robot motion constraint (in case you are using a 2D robot) - z_tollerance: 1000 # meters - rotation_tollerance: 1000 # radians - - # CPU Params - numberOfCores: 4 # number of cores for mapping optimization - mappingProcessInterval: 0.15 # seconds, regulate mapping frequency - - # Surrounding map - surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold - surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold - surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses - surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled) - - # Loop closure - loopClosureEnableFlag: true - loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency - surroundingKeyframeSize: 50 # submap size (when loop closure enabled) - historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure - historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure - historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure - historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment - - # Visualization - globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius - globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density - globalMapVisualizationLeafSize: 1.0 # meters, global map visualization cloud density - - - - -# Navsat (convert GPS coordinates to Cartesian) -navsat: - frequency: 50 - wait_for_datum: false - delay: 0.0 - magnetic_declination_radians: 0 - yaw_offset: 0 - zero_altitude: true - broadcast_utm_transform: false - broadcast_utm_transform_as_parent_frame: false - publish_filtered_gps: false - -# EKF for Navsat -ekf_gps: - publish_tf: false - map_frame: map - odom_frame: odom - base_link_frame: base_link - world_frame: odom - - frequency: 50 - two_d_mode: false - sensor_timeout: 0.01 - # ------------------------------------- - # External IMU: - # ------------------------------------- - imu0: imu_correct - # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link - imu0_config: [false, false, false, - true, true, true, - false, false, false, - false, false, true, - true, true, true] - imu0_differential: false - imu0_queue_size: 50 - imu0_remove_gravitational_acceleration: true - # ------------------------------------- - # Odometry (From Navsat): - # ------------------------------------- - odom0: odometry/gps - odom0_config: [true, true, true, - false, false, false, - false, false, false, - false, false, false, - false, false, false] - odom0_differential: false - odom0_queue_size: 10 - - # x y z r p y x_dot y_dot z_dot r_dot p_dot y_dot x_ddot y_ddot z_ddot - process_noise_covariance: [ 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 10.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] +/lio_sam*: + ros__parameters: + + # Topics + pointCloudTopic: "/points" # Point cloud data + imuTopic: "/imu/data" # IMU data + odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU + gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file + + # Frames + lidarFrame: "lidar_link" + baselinkFrame: "base_link" + odometryFrame: "odom" + mapFrame: "map" + + # GPS Settings + useImuHeadingInitialization: false # if using GPS data, set to "true" + useGpsElevation: false # if GPS elevation is bad, set to "false" + gpsCovThreshold: 2.0 # m^2, threshold for using GPS data + poseCovThreshold: 25.0 # m^2, threshold for using GPS data + + # Export settings + savePCD: false # https://github.com/TixiaoShan/LIO-SAM/issues/3 + savePCDDirectory: "/Downloads/LOAM/" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation + + # Sensor Settings + sensor: ouster # lidar sensor type, either 'velodyne' or 'ouster' + N_SCAN: 64 # number of lidar channel (i.e., 16, 32, 64, 128) + Horizon_SCAN: 512 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) + downsampleRate: 1 # default: 1. Downsample your data if too many + # points. i.e., 16 = 64 / 4, 16 = 16 / 1 + lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used + lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used + + # IMU Settings + imuAccNoise: 3.9939570888238808e-03 + imuGyrNoise: 1.5636343949698187e-03 + imuAccBiasN: 6.4356659353532566e-05 + imuGyrBiasN: 3.5640318696367613e-05 + + imuGravity: 9.80511 + imuRPYWeight: 0.01 + + extrinsicTrans: [ 0.0, 0.0, 0.0 ] + extrinsicRot: [ 1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, -1.0 ] + extrinsicRPY: [ 1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, -1.0 ] + + # LOAM feature threshold + edgeThreshold: 1.0 + surfThreshold: 0.1 + edgeFeatureMinValidNum: 10 + surfFeatureMinValidNum: 100 + + # voxel filter paprams + odometrySurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor + mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor + mappingSurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor + + # robot motion constraint (in case you are using a 2D robot) + z_tollerance: 1000.0 # meters + rotation_tollerance: 1000.0 # radians + + # CPU Params + numberOfCores: 4 # number of cores for mapping optimization + mappingProcessInterval: 0.15 # seconds, regulate mapping frequency + + # Surrounding map + surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold + surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold + surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses + surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization + # (when loop closure disabled) + + # Loop closure + loopClosureEnableFlag: true + loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency + surroundingKeyframeSize: 50 # submap size (when loop closure enabled) + historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from + # current pose will be considerd for loop closure + historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be + # considered for loop closure + historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a + # submap for loop closure + historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment + + # Visualization + globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius + globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density + globalMapVisualizationLeafSize: 1.0 # meters, global map visualization cloud density diff --git a/launch/include/config/robot.urdf.xacro b/config/robot.urdf.xacro similarity index 70% rename from launch/include/config/robot.urdf.xacro rename to config/robot.urdf.xacro index 1e3f31a7..604363e3 100644 --- a/launch/include/config/robot.urdf.xacro +++ b/config/robot.urdf.xacro @@ -11,6 +11,13 @@ + + + + + + + @@ -18,10 +25,10 @@ - - - - + + + + diff --git a/config/rviz2.rviz b/config/rviz2.rviz new file mode 100644 index 00000000..efbc2d4c --- /dev/null +++ b/config/rviz2.rviz @@ -0,0 +1,263 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /TF1 + - /MarkerArray1 + - /PointCloud22 + - /PointCloud22/Topic1 + Splitter Ratio: 0.35384616255760193 + Tree Height: 787 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + 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: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: -0.8519169092178345 + Min Value: -4.6142706871032715 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 1000 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 92.95751190185547 + Min Color: 0; 0; 0 + Min Intensity: 5.530245304107666 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lio_sam/mapping/cloud_registered + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + chassis_link: + Value: true + imu_link: + Value: true + map: + Value: true + navsat_link: + Value: true + odom: + Value: true + os0_32/os_sensor: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 85; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lio_sam/mapping/path + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lio_sam/mapping/loop_closure_constraints + 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_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: System Default + Value: /points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - 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 + 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: + 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_default_plugins/Orbit + Distance: 142.68260192871094 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.47039806842803955 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5903979539871216 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000020a0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004130000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/include/lio_sam/utility.hpp b/include/lio_sam/utility.hpp new file mode 100644 index 00000000..6c9e15d6 --- /dev/null +++ b/include/lio_sam/utility.hpp @@ -0,0 +1,442 @@ +#pragma once +#ifndef _UTILITY_LIDAR_ODOMETRY_H_ +#define _UTILITY_LIDAR_ODOMETRY_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +typedef pcl::PointXYZI PointType; + +enum class SensorType { VELODYNE, OUSTER }; + +class ParamServer : public rclcpp::Node +{ +public: + std::string robot_id; + + //Topics + string pointCloudTopic; + string imuTopic; + string odomTopic; + string gpsTopic; + + //Frames + string lidarFrame; + string baselinkFrame; + string odometryFrame; + string mapFrame; + + // GPS Settings + bool useImuHeadingInitialization; + bool useGpsElevation; + float gpsCovThreshold; + float poseCovThreshold; + + // Save pcd + bool savePCD; + string savePCDDirectory; + + // Lidar Sensor Configuration + SensorType sensor = SensorType::OUSTER; + int N_SCAN; + int Horizon_SCAN; + int downsampleRate; + float lidarMinRange; + float lidarMaxRange; + + // IMU + float imuAccNoise; + float imuGyrNoise; + float imuAccBiasN; + float imuGyrBiasN; + float imuGravity; + float imuRPYWeight; + vector extRotV; + vector extRPYV; + vector extTransV; + Eigen::Matrix3d extRot; + Eigen::Matrix3d extRPY; + Eigen::Vector3d extTrans; + Eigen::Quaterniond extQRPY; + + // LOAM + float edgeThreshold; + float surfThreshold; + int edgeFeatureMinValidNum; + int surfFeatureMinValidNum; + + // voxel filter paprams + float odometrySurfLeafSize; + float mappingCornerLeafSize; + float mappingSurfLeafSize ; + + float z_tollerance; + float rotation_tollerance; + + // CPU Params + int numberOfCores; + double mappingProcessInterval; + + // Surrounding map + float surroundingkeyframeAddingDistThreshold; + float surroundingkeyframeAddingAngleThreshold; + float surroundingKeyframeDensity; + float surroundingKeyframeSearchRadius; + + // Loop closure + bool loopClosureEnableFlag; + float loopClosureFrequency; + int surroundingKeyframeSize; + float historyKeyframeSearchRadius; + float historyKeyframeSearchTimeDiff; + int historyKeyframeSearchNum; + float historyKeyframeFitnessScore; + + // global map visualization radius + float globalMapVisualizationSearchRadius; + float globalMapVisualizationPoseDensity; + float globalMapVisualizationLeafSize; + + ParamServer(std::string node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) + { + declare_parameter("pointCloudTopic", "points"); + get_parameter("pointCloudTopic", pointCloudTopic); + declare_parameter("imuTopic", "imu/data"); + get_parameter("imuTopic", imuTopic); + declare_parameter("odomTopic", "lio_sam/odometry/imu"); + get_parameter("odomTopic", odomTopic); + declare_parameter("gpsTopic", "lio_sam/odometry/gps"); + get_parameter("gpsTopic", gpsTopic); + + declare_parameter("lidarFrame", "laser_data_frame"); + get_parameter("lidarFrame", lidarFrame); + declare_parameter("baselinkFrame", "base_link"); + get_parameter("baselinkFrame", baselinkFrame); + declare_parameter("odometryFrame", "odom"); + get_parameter("odometryFrame", odometryFrame); + declare_parameter("mapFrame", "map"); + get_parameter("mapFrame", mapFrame); + + declare_parameter("useImuHeadingInitialization", false); + get_parameter("useImuHeadingInitialization", useImuHeadingInitialization); + declare_parameter("useGpsElevation", false); + get_parameter("useGpsElevation", useGpsElevation); + declare_parameter("gpsCovThreshold", 2.0); + get_parameter("gpsCovThreshold", gpsCovThreshold); + declare_parameter("poseCovThreshold", 25.0); + get_parameter("poseCovThreshold", poseCovThreshold); + + declare_parameter("savePCD", false); + get_parameter("savePCD", savePCD); + + declare_parameter("savePCDDirectory", "/Downloads/LOAM/"); + get_parameter("savePCDDirectory", savePCDDirectory); + + declare_parameter("N_SCAN", 64); + get_parameter("N_SCAN", N_SCAN); + declare_parameter("Horizon_SCAN", 512); + get_parameter("Horizon_SCAN", Horizon_SCAN); + declare_parameter("downsampleRate", 1); + get_parameter("downsampleRate", downsampleRate); + declare_parameter("lidarMinRange", 5.5); + get_parameter("lidarMinRange", lidarMinRange); + declare_parameter("lidarMaxRange", 1000.0); + get_parameter("lidarMaxRange", lidarMaxRange); + + declare_parameter("imuAccNoise", 9e-4); + get_parameter("imuAccNoise", imuAccNoise); + declare_parameter("imuGyrNoise", 1.6e-4); + get_parameter("imuGyrNoise", imuGyrNoise); + declare_parameter("imuAccBiasN", 5e-4); + get_parameter("imuAccBiasN", imuAccBiasN); + declare_parameter("imuGyrBiasN", 7e-5); + get_parameter("imuGyrBiasN", imuGyrBiasN); + declare_parameter("imuGravity", 9.80511); + get_parameter("imuGravity", imuGravity); + declare_parameter("imuRPYWeight", 0.01); + get_parameter("imuRPYWeight", imuRPYWeight); + + double ida[] = { 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0}; + std::vector < double > id(ida, std::end(ida)); + declare_parameter("extrinsicRot", id); + get_parameter("extrinsicRot", extRotV); + declare_parameter("extrinsicRPY", id); + get_parameter("extrinsicRPY", extRPYV); + double zea[] = {0.0, 0.0, 0.0}; + std::vector < double > ze(zea, std::end(zea)); + declare_parameter("extrinsicTrans", ze); + get_parameter("extrinsicTrans", extTransV); + + extRot = Eigen::Map>(extRotV.data(), 3, 3); + extRPY = Eigen::Map>(extRPYV.data(), 3, 3); + extTrans = Eigen::Map>(extTransV.data(), 3, 1); + extQRPY = Eigen::Quaterniond(extRPY); + + declare_parameter("edgeThreshold", 1.0); + get_parameter("edgeThreshold", edgeThreshold); + declare_parameter("surfThreshold", 0.1); + get_parameter("surfThreshold", surfThreshold); + declare_parameter("edgeFeatureMinValidNum", 10); + get_parameter("edgeFeatureMinValidNum", edgeFeatureMinValidNum); + declare_parameter("surfFeatureMinValidNum", 100); + get_parameter("surfFeatureMinValidNum", surfFeatureMinValidNum); + + declare_parameter("odometrySurfLeafSize", 0.4); + get_parameter("odometrySurfLeafSize", odometrySurfLeafSize); + declare_parameter("mappingCornerLeafSize", 0.2); + get_parameter("mappingCornerLeafSize", mappingCornerLeafSize); + declare_parameter("mappingSurfLeafSize", 0.4); + get_parameter("mappingSurfLeafSize", mappingSurfLeafSize); + + declare_parameter("z_tollerance", 1000.0); + get_parameter("z_tollerance", z_tollerance); + declare_parameter("rotation_tollerance", 1000.0); + get_parameter("rotation_tollerance", rotation_tollerance); + + declare_parameter("numberOfCores", 4); + get_parameter("numberOfCores", numberOfCores); + declare_parameter("mappingProcessInterval", 0.15); + get_parameter("mappingProcessInterval", mappingProcessInterval); + + declare_parameter("surroundingkeyframeAddingDistThreshold", 1.0); + get_parameter("surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold); + declare_parameter("surroundingkeyframeAddingAngleThreshold", 0.2); + get_parameter("surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold); + declare_parameter("surroundingKeyframeDensity", 2.0); + get_parameter("surroundingKeyframeDensity", surroundingKeyframeDensity); + declare_parameter("surroundingKeyframeSearchRadius", 50.0); + get_parameter("surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius); + + declare_parameter("loopClosureEnableFlag", true); + get_parameter("loopClosureEnableFlag", loopClosureEnableFlag); + declare_parameter("loopClosureFrequency", 1.0); + get_parameter("loopClosureFrequency", loopClosureFrequency); + declare_parameter("surroundingKeyframeSize", 50); + get_parameter("surroundingKeyframeSize", surroundingKeyframeSize); + declare_parameter("historyKeyframeSearchRadius", 15.0); + get_parameter("historyKeyframeSearchRadius", historyKeyframeSearchRadius); + declare_parameter("historyKeyframeSearchTimeDiff", 30.0); + get_parameter("historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff); + declare_parameter("historyKeyframeSearchNum", 25); + get_parameter("historyKeyframeSearchNum", historyKeyframeSearchNum); + declare_parameter("historyKeyframeFitnessScore", 0.3); + get_parameter("historyKeyframeFitnessScore", historyKeyframeFitnessScore); + + declare_parameter("globalMapVisualizationSearchRadius", 1000.0); + get_parameter("globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius); + declare_parameter("globalMapVisualizationPoseDensity", 10.0); + get_parameter("globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity); + declare_parameter("globalMapVisualizationLeafSize", 1.0); + get_parameter("globalMapVisualizationLeafSize", globalMapVisualizationLeafSize); + + usleep(100); + } + + sensor_msgs::msg::Imu imuConverter(const sensor_msgs::msg::Imu& imu_in) + { + sensor_msgs::msg::Imu imu_out = imu_in; + // rotate acceleration + Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z); + acc = extRot * acc; + imu_out.linear_acceleration.x = acc.x(); + imu_out.linear_acceleration.y = acc.y(); + imu_out.linear_acceleration.z = acc.z(); + // rotate gyroscope + Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z); + gyr = extRot * gyr; + imu_out.angular_velocity.x = gyr.x(); + imu_out.angular_velocity.y = gyr.y(); + imu_out.angular_velocity.z = gyr.z(); + // rotate roll pitch yaw + Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z); + Eigen::Quaterniond q_final = q_from * extQRPY; + imu_out.orientation.x = q_final.x(); + imu_out.orientation.y = q_final.y(); + imu_out.orientation.z = q_final.z(); + imu_out.orientation.w = q_final.w(); + + if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1) + { + RCLCPP_ERROR(get_logger(), "Invalid quaternion, please use a 9-axis IMU!"); + rclcpp::shutdown(); + } + + return imu_out; + } +}; + + +sensor_msgs::msg::PointCloud2 publishCloud(rclcpp::Publisher::SharedPtr thisPub, pcl::PointCloud::Ptr thisCloud, rclcpp::Time thisStamp, std::string thisFrame) +{ + sensor_msgs::msg::PointCloud2 tempCloud; + pcl::toROSMsg(*thisCloud, tempCloud); + tempCloud.header.stamp = thisStamp; + tempCloud.header.frame_id = thisFrame; + if (thisPub->get_subscription_count() != 0) + thisPub->publish(tempCloud); + return tempCloud; +} + +template +double stamp2Sec(const T& stamp) +{ + return rclcpp::Time(stamp).seconds(); +} + + +template +void imuAngular2rosAngular(sensor_msgs::msg::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z) +{ + *angular_x = thisImuMsg->angular_velocity.x; + *angular_y = thisImuMsg->angular_velocity.y; + *angular_z = thisImuMsg->angular_velocity.z; +} + + +template +void imuAccel2rosAccel(sensor_msgs::msg::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z) +{ + *acc_x = thisImuMsg->linear_acceleration.x; + *acc_y = thisImuMsg->linear_acceleration.y; + *acc_z = thisImuMsg->linear_acceleration.z; +} + + +template +void imuRPY2rosRPY(sensor_msgs::msg::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw) +{ + double imuRoll, imuPitch, imuYaw; + tf2::Quaternion orientation; + tf2::fromMsg(thisImuMsg->orientation, orientation); + tf2::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw); + + *rosRoll = imuRoll; + *rosPitch = imuPitch; + *rosYaw = imuYaw; +} + + +float pointDistance(PointType p) +{ + return sqrt(p.x*p.x + p.y*p.y + p.z*p.z); +} + + +float pointDistance(PointType p1, PointType p2) +{ + return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z)); +} + +rmw_qos_profile_t qos_profile{ + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false +}; + +auto qos = rclcpp::QoS( + rclcpp::QoSInitialization( + qos_profile.history, + qos_profile.depth + ), + qos_profile); + +rmw_qos_profile_t qos_profile_imu{ + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 2000, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false +}; + +auto qos_imu = rclcpp::QoS( + rclcpp::QoSInitialization( + qos_profile_imu.history, + qos_profile_imu.depth + ), + qos_profile_imu); + +rmw_qos_profile_t qos_profile_lidar{ + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 5, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false +}; + +auto qos_lidar = rclcpp::QoS( + rclcpp::QoSInitialization( + qos_profile_lidar.history, + qos_profile_lidar.depth + ), + qos_profile_lidar); + +#endif diff --git a/include/utility.h b/include/utility.h deleted file mode 100644 index c1f5feff..00000000 --- a/include/utility.h +++ /dev/null @@ -1,342 +0,0 @@ -#pragma once -#ifndef _UTILITY_LIDAR_ODOMETRY_H_ -#define _UTILITY_LIDAR_ODOMETRY_H_ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; - -typedef pcl::PointXYZI PointType; - -enum class SensorType { VELODYNE, OUSTER }; - -class ParamServer -{ -public: - - ros::NodeHandle nh; - - std::string robot_id; - - //Topics - string pointCloudTopic; - string imuTopic; - string odomTopic; - string gpsTopic; - - //Frames - string lidarFrame; - string baselinkFrame; - string odometryFrame; - string mapFrame; - - // GPS Settings - bool useImuHeadingInitialization; - bool useGpsElevation; - float gpsCovThreshold; - float poseCovThreshold; - - // Save pcd - bool savePCD; - string savePCDDirectory; - - // Lidar Sensor Configuration - SensorType sensor; - int N_SCAN; - int Horizon_SCAN; - int downsampleRate; - float lidarMinRange; - float lidarMaxRange; - - // IMU - float imuAccNoise; - float imuGyrNoise; - float imuAccBiasN; - float imuGyrBiasN; - float imuGravity; - float imuRPYWeight; - vector extRotV; - vector extRPYV; - vector extTransV; - Eigen::Matrix3d extRot; - Eigen::Matrix3d extRPY; - Eigen::Vector3d extTrans; - Eigen::Quaterniond extQRPY; - - // LOAM - float edgeThreshold; - float surfThreshold; - int edgeFeatureMinValidNum; - int surfFeatureMinValidNum; - - // voxel filter paprams - float odometrySurfLeafSize; - float mappingCornerLeafSize; - float mappingSurfLeafSize ; - - float z_tollerance; - float rotation_tollerance; - - // CPU Params - int numberOfCores; - double mappingProcessInterval; - - // Surrounding map - float surroundingkeyframeAddingDistThreshold; - float surroundingkeyframeAddingAngleThreshold; - float surroundingKeyframeDensity; - float surroundingKeyframeSearchRadius; - - // Loop closure - bool loopClosureEnableFlag; - float loopClosureFrequency; - int surroundingKeyframeSize; - float historyKeyframeSearchRadius; - float historyKeyframeSearchTimeDiff; - int historyKeyframeSearchNum; - float historyKeyframeFitnessScore; - - // global map visualization radius - float globalMapVisualizationSearchRadius; - float globalMapVisualizationPoseDensity; - float globalMapVisualizationLeafSize; - - ParamServer() - { - nh.param("/robot_id", robot_id, "roboat"); - - nh.param("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw"); - nh.param("lio_sam/imuTopic", imuTopic, "imu_correct"); - nh.param("lio_sam/odomTopic", odomTopic, "odometry/imu"); - nh.param("lio_sam/gpsTopic", gpsTopic, "odometry/gps"); - - nh.param("lio_sam/lidarFrame", lidarFrame, "base_link"); - nh.param("lio_sam/baselinkFrame", baselinkFrame, "base_link"); - nh.param("lio_sam/odometryFrame", odometryFrame, "odom"); - nh.param("lio_sam/mapFrame", mapFrame, "map"); - - nh.param("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false); - nh.param("lio_sam/useGpsElevation", useGpsElevation, false); - nh.param("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0); - nh.param("lio_sam/poseCovThreshold", poseCovThreshold, 25.0); - - nh.param("lio_sam/savePCD", savePCD, false); - nh.param("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/"); - - std::string sensorStr; - nh.param("lio_sam/sensor", sensorStr, ""); - if (sensorStr == "velodyne") - { - sensor = SensorType::VELODYNE; - } - else if (sensorStr == "ouster") - { - sensor = SensorType::OUSTER; - } - else - { - ROS_ERROR_STREAM( - "Invalid sensor type (must be either 'velodyne' or 'ouster'): " << sensorStr); - ros::shutdown(); - } - - nh.param("lio_sam/N_SCAN", N_SCAN, 16); - nh.param("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800); - nh.param("lio_sam/downsampleRate", downsampleRate, 1); - nh.param("lio_sam/lidarMinRange", lidarMinRange, 1.0); - nh.param("lio_sam/lidarMaxRange", lidarMaxRange, 1000.0); - - nh.param("lio_sam/imuAccNoise", imuAccNoise, 0.01); - nh.param("lio_sam/imuGyrNoise", imuGyrNoise, 0.001); - nh.param("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002); - nh.param("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003); - nh.param("lio_sam/imuGravity", imuGravity, 9.80511); - nh.param("lio_sam/imuRPYWeight", imuRPYWeight, 0.01); - nh.param>("lio_sam/extrinsicRot", extRotV, vector()); - nh.param>("lio_sam/extrinsicRPY", extRPYV, vector()); - nh.param>("lio_sam/extrinsicTrans", extTransV, vector()); - extRot = Eigen::Map>(extRotV.data(), 3, 3); - extRPY = Eigen::Map>(extRPYV.data(), 3, 3); - extTrans = Eigen::Map>(extTransV.data(), 3, 1); - extQRPY = Eigen::Quaterniond(extRPY); - - nh.param("lio_sam/edgeThreshold", edgeThreshold, 0.1); - nh.param("lio_sam/surfThreshold", surfThreshold, 0.1); - nh.param("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10); - nh.param("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100); - - nh.param("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2); - nh.param("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2); - nh.param("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2); - - nh.param("lio_sam/z_tollerance", z_tollerance, FLT_MAX); - nh.param("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX); - - nh.param("lio_sam/numberOfCores", numberOfCores, 2); - nh.param("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15); - - nh.param("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0); - nh.param("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2); - nh.param("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0); - nh.param("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0); - - nh.param("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false); - nh.param("lio_sam/loopClosureFrequency", loopClosureFrequency, 1.0); - nh.param("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50); - nh.param("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0); - nh.param("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0); - nh.param("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25); - nh.param("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3); - - nh.param("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3); - nh.param("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0); - nh.param("lio_sam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0); - - usleep(100); - } - - sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in) - { - sensor_msgs::Imu imu_out = imu_in; - // rotate acceleration - Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z); - acc = extRot * acc; - imu_out.linear_acceleration.x = acc.x(); - imu_out.linear_acceleration.y = acc.y(); - imu_out.linear_acceleration.z = acc.z(); - // rotate gyroscope - Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z); - gyr = extRot * gyr; - imu_out.angular_velocity.x = gyr.x(); - imu_out.angular_velocity.y = gyr.y(); - imu_out.angular_velocity.z = gyr.z(); - // rotate roll pitch yaw - Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z); - Eigen::Quaterniond q_final = q_from * extQRPY; - imu_out.orientation.x = q_final.x(); - imu_out.orientation.y = q_final.y(); - imu_out.orientation.z = q_final.z(); - imu_out.orientation.w = q_final.w(); - - if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1) - { - ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!"); - ros::shutdown(); - } - - return imu_out; - } -}; - - -sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame) -{ - sensor_msgs::PointCloud2 tempCloud; - pcl::toROSMsg(*thisCloud, tempCloud); - tempCloud.header.stamp = thisStamp; - tempCloud.header.frame_id = thisFrame; - if (thisPub->getNumSubscribers() != 0) - thisPub->publish(tempCloud); - return tempCloud; -} - -template -double ROS_TIME(T msg) -{ - return msg->header.stamp.toSec(); -} - - -template -void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z) -{ - *angular_x = thisImuMsg->angular_velocity.x; - *angular_y = thisImuMsg->angular_velocity.y; - *angular_z = thisImuMsg->angular_velocity.z; -} - - -template -void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z) -{ - *acc_x = thisImuMsg->linear_acceleration.x; - *acc_y = thisImuMsg->linear_acceleration.y; - *acc_z = thisImuMsg->linear_acceleration.z; -} - - -template -void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw) -{ - double imuRoll, imuPitch, imuYaw; - tf::Quaternion orientation; - tf::quaternionMsgToTF(thisImuMsg->orientation, orientation); - tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw); - - *rosRoll = imuRoll; - *rosPitch = imuPitch; - *rosYaw = imuYaw; -} - - -float pointDistance(PointType p) -{ - return sqrt(p.x*p.x + p.y*p.y + p.z*p.z); -} - - -float pointDistance(PointType p1, PointType p2) -{ - return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z)); -} - -#endif diff --git a/launch/include/config/rviz.rviz b/launch/include/config/rviz.rviz deleted file mode 100644 index 85dd826a..00000000 --- a/launch/include/config/rviz.rviz +++ /dev/null @@ -1,527 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 191 - Name: Displays - Property Tree Widget: - Expanded: - - /links1 - - /Odometry1 - - /Point Cloud1 - - /Feature Cloud1 - - /Mapping1 - Splitter Ratio: 0.5600000023841858 - Tree Height: 824 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/TF - Enabled: false - Frame Timeout: 999 - Frames: - All Enabled: false - Marker Scale: 3 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Class: rviz/Axes - Enabled: true - Length: 2 - Name: map - Radius: 0.5 - Reference Frame: map - Value: true - - Class: rviz/Axes - Enabled: true - Length: 1 - Name: base_link - Radius: 0.30000001192092896 - Reference Frame: base_link - Value: true - Enabled: true - Name: links - - Class: rviz/Group - Displays: - - Angle Tolerance: 0.10000000149011612 - 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: false - Enabled: true - Keep: 300 - Name: Odom IMU - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /odometry/imu - Unreliable: false - Value: true - - Angle Tolerance: 0.10000000149011612 - 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: false - Enabled: true - Keep: 300 - Name: Odom GPS - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /odometry/gps - Unreliable: false - Value: true - - Angle Tolerance: 0.10000000149011612 - 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: false - Enabled: false - Keep: 300 - Name: Odom lidar - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 0.25 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /lio_sam/mapping/odometry - Unreliable: false - Value: false - Enabled: false - Name: Odometry - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 128 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Velodyne - Position Transformer: XYZ - Queue Size: 10 - Selectable: false - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: /lio_sam/deskew/cloud_deskewed - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - 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; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0.7900000214576721 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Reg Cloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: /lio_sam/mapping/cloud_registered - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Name: Point Cloud - - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15.077729225158691 - Min Color: 0; 0; 0 - Min Intensity: 0.019985778257250786 - Name: Edge Feature - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: /lio_sam/feature/cloud_corner - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - 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; 85; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15.100607872009277 - Min Color: 0; 0; 0 - Min Intensity: 0.0007188677554950118 - Name: Plannar Feature - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: /lio_sam/feature/cloud_surface - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: false - Name: Feature Cloud - - Class: rviz/Group - Displays: - - Alpha: 0.30000001192092896 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 7.4858574867248535 - Min Value: -1.2309398651123047 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 300 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 128 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Map (cloud) - Position Transformer: XYZ - Queue Size: 10 - Selectable: false - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: /lio_sam/mapping/cloud_registered - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 36.61034393310547 - Min Value: -2.3476977348327637 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15.100604057312012 - Min Color: 0; 0; 0 - Min Intensity: 0.014545874670147896 - Name: Map (local) - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 2 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /lio_sam/mapping/map_local - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15.100096702575684 - Min Color: 0; 0; 0 - Min Intensity: 0.007923072203993797 - Name: Map (global) - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: /lio_sam/mapping/map_global - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 85; 255; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: Path (global) - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /lio_sam/mapping/path - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 85; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: Path (local) - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /lio_sam/imu/path - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 20.802837371826172 - Min Value: -0.03934507071971893 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15.100604057312012 - Min Color: 0; 0; 0 - Min Intensity: 0.014545874670147896 - Name: Loop closure - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 2 - Size (m): 0.30000001192092896 - Style: Flat Squares - Topic: /lio_sam/mapping/icp_loop_closure_corrected_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /lio_sam/mapping/loop_closure_constraints - Name: Loop constraint - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Mapping - Enabled: true - Global Options: - Background Color: 0; 0; 0 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/FocusCamera - - Class: rviz/Measure - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 97.43701171875 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.1253981590270996 - Target Frame: base_link - Value: Orbit (rviz) - Yaw: 3.3316705226898193 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1190 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001a30000043efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000002e000001340000000000000000fb000000100044006900730070006c00610079007301000000470000043e000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000286000000a70000006e00ffffff000000010000011100000435fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002e00000435000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000040fc0100000002fb0000000800540069006d00650000000000000004b00000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005f20000043e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1948 - X: 725 - Y: 300 diff --git a/launch/include/module_loam.launch b/launch/include/module_loam.launch deleted file mode 100644 index eba219fd..00000000 --- a/launch/include/module_loam.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/launch/include/module_navsat.launch b/launch/include/module_navsat.launch deleted file mode 100644 index 16ca1d34..00000000 --- a/launch/include/module_navsat.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/include/module_robot_state_publisher.launch b/launch/include/module_robot_state_publisher.launch deleted file mode 100644 index c6ac3e2a..00000000 --- a/launch/include/module_robot_state_publisher.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/launch/include/module_rviz.launch b/launch/include/module_rviz.launch deleted file mode 100644 index 3ec52da3..00000000 --- a/launch/include/module_rviz.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/launch/include/rosconsole/rosconsole_error.conf b/launch/include/rosconsole/rosconsole_error.conf deleted file mode 100644 index 45663862..00000000 --- a/launch/include/rosconsole/rosconsole_error.conf +++ /dev/null @@ -1,2 +0,0 @@ -# Set the default ros output to warning and higher -log4j.logger.ros = ERROR \ No newline at end of file diff --git a/launch/include/rosconsole/rosconsole_info.conf b/launch/include/rosconsole/rosconsole_info.conf deleted file mode 100644 index 888786c1..00000000 --- a/launch/include/rosconsole/rosconsole_info.conf +++ /dev/null @@ -1,2 +0,0 @@ -# Set the default ros output to warning and higher -log4j.logger.ros = INFO \ No newline at end of file diff --git a/launch/include/rosconsole/rosconsole_warn.conf b/launch/include/rosconsole/rosconsole_warn.conf deleted file mode 100644 index 10f954c4..00000000 --- a/launch/include/rosconsole/rosconsole_warn.conf +++ /dev/null @@ -1,2 +0,0 @@ -# Set the default ros output to warning and higher -log4j.logger.ros = WARN \ No newline at end of file diff --git a/launch/run.launch b/launch/run.launch deleted file mode 100644 index 59a77202..00000000 --- a/launch/run.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/launch/run.launch.py b/launch/run.launch.py new file mode 100644 index 00000000..39c79514 --- /dev/null +++ b/launch/run.launch.py @@ -0,0 +1,73 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node + + +def generate_launch_description(): + + share_dir = get_package_share_directory('lio_sam') + parameter_file = LaunchConfiguration('params_file') + xacro_path = os.path.join(share_dir, 'config', 'robot.urdf.xacro') + rviz_config_file = os.path.join(share_dir, 'config', 'rviz2.rviz') + + params_declare = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join( + share_dir, 'config', 'params.yaml'), + description='FPath to the ROS2 parameters file to use.') + + print("urdf_file_name : {}".format(xacro_path)) + + return LaunchDescription([ + params_declare, + Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments='0.0 0.0 0.0 0.0 0.0 0.0 map odom'.split(' '), + parameters=[parameter_file], + output='screen' + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'robot_description': Command(['xacro', ' ', xacro_path]) + }] + ), + Node( + package='lio_sam', + executable='lio_sam_imuPreintegration', + parameters=[parameter_file], + output='screen' + ), + Node( + package='lio_sam', + executable='lio_sam_imageProjection', + parameters=[parameter_file], + output='screen' + ), + Node( + package='lio_sam', + executable='lio_sam_featureExtraction', + parameters=[parameter_file], + output='screen' + ), + Node( + package='lio_sam', + executable='lio_sam_mapOptimization', + parameters=[parameter_file], + output='screen' + ), + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_file], + output='screen' + ) + ]) diff --git a/msg/CloudInfo.msg b/msg/CloudInfo.msg new file mode 100644 index 00000000..9060d13d --- /dev/null +++ b/msg/CloudInfo.msg @@ -0,0 +1,29 @@ +# Cloud Info +std_msgs/Header header + +int32[] start_ring_index +int32[] end_ring_index + +int32[] point_col_ind # point column index in range image +float32[] point_range # point range + +int64 imu_available +int64 odom_available + +# Attitude for LOAM initialization +float32 imu_roll_init +float32 imu_pitch_init +float32 imu_yaw_init + +# Initial guess from imu pre-integration +float32 initial_guess_x +float32 initial_guess_y +float32 initial_guess_z +float32 initial_guess_roll +float32 initial_guess_pitch +float32 initial_guess_yaw + +# Point cloud messages +sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed +sensor_msgs/PointCloud2 cloud_corner # extracted corner feature +sensor_msgs/PointCloud2 cloud_surface # extracted surface feature diff --git a/msg/cloud_info.msg b/msg/cloud_info.msg deleted file mode 100644 index 51ab297d..00000000 --- a/msg/cloud_info.msg +++ /dev/null @@ -1,29 +0,0 @@ -# Cloud Info -Header header - -int32[] startRingIndex -int32[] endRingIndex - -int32[] pointColInd # point column index in range image -float32[] pointRange # point range - -int64 imuAvailable -int64 odomAvailable - -# Attitude for LOAM initialization -float32 imuRollInit -float32 imuPitchInit -float32 imuYawInit - -# Initial guess from imu pre-integration -float32 initialGuessX -float32 initialGuessY -float32 initialGuessZ -float32 initialGuessRoll -float32 initialGuessPitch -float32 initialGuessYaw - -# Point cloud messages -sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed -sensor_msgs/PointCloud2 cloud_corner # extracted corner feature -sensor_msgs/PointCloud2 cloud_surface # extracted surface feature \ No newline at end of file diff --git a/package.xml b/package.xml index 7109c3ac..c5b33502 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,6 @@ - + + lio_sam 1.0.0 Lidar Odometry @@ -10,39 +11,48 @@ Tixiao Shan - catkin - - roscpp - roscpp - rospy - rospy - - tf - tf - - cv_bridge - cv_bridge - - pcl_conversions - pcl_conversions + ament_cmake + rosidl_default_generators + rclcpp + rclpy std_msgs - std_msgs sensor_msgs - sensor_msgs - geometry_msgs - geometry_msgs nav_msgs - nav_msgs - visualization_msgs - visualization_msgs - - message_generation - message_generation - message_runtime - message_runtime - + geometry_msgs + tf2_geometry_msgs + tf2_sensor_msgs + tf2_eigen + tf2_ros + tf2 + OpenCV + PCL GTSAM - GTSAM - + Eigen + + rosidl_default_runtime + rclcpp + rclpy + std_msgs + sensor_msgs + nav_msgs + geometry_msgs + tf2_geometry_msgs + tf2_sensor_msgs + tf2_eigen + tf2_ros + tf2 + OpenCV + PCL + GTSAM + Eigen + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/src/featureExtraction.cpp b/src/featureExtraction.cpp index 7d444d94..0a6af036 100644 --- a/src/featureExtraction.cpp +++ b/src/featureExtraction.cpp @@ -1,5 +1,5 @@ -#include "utility.h" -#include "lio_sam/cloud_info.h" +#include "utility.hpp" +#include "lio_sam/msg/cloud_info.hpp" struct smoothness_t{ float value; @@ -17,11 +17,11 @@ class FeatureExtraction : public ParamServer public: - ros::Subscriber subLaserCloudInfo; + rclcpp::Subscription::SharedPtr subLaserCloudInfo; - ros::Publisher pubLaserCloudInfo; - ros::Publisher pubCornerPoints; - ros::Publisher pubSurfacePoints; + rclcpp::Publisher::SharedPtr pubLaserCloudInfo; + rclcpp::Publisher::SharedPtr pubCornerPoints; + rclcpp::Publisher::SharedPtr pubSurfacePoints; pcl::PointCloud::Ptr extractedCloud; pcl::PointCloud::Ptr cornerCloud; @@ -29,22 +29,28 @@ class FeatureExtraction : public ParamServer pcl::VoxelGrid downSizeFilter; - lio_sam::cloud_info cloudInfo; - std_msgs::Header cloudHeader; + lio_sam::msg::CloudInfo cloudInfo; + std_msgs::msg::Header cloudHeader; std::vector cloudSmoothness; float *cloudCurvature; int *cloudNeighborPicked; int *cloudLabel; - FeatureExtraction() + FeatureExtraction(const rclcpp::NodeOptions & options) : + ParamServer("lio_sam_featureExtraction", options) { - subLaserCloudInfo = nh.subscribe("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay()); + subLaserCloudInfo = create_subscription( + "lio_sam/deskew/cloud_info", qos, + std::bind(&FeatureExtraction::laserCloudInfoHandler, this, std::placeholders::_1)); + + pubLaserCloudInfo = create_publisher( + "lio_sam/feature/cloud_info", qos); + pubCornerPoints = create_publisher( + "lio_sam/feature/cloud_corner", 1); + pubSurfacePoints = create_publisher( + "lio_sam/feature/cloud_surface", 1); - pubLaserCloudInfo = nh.advertise ("lio_sam/feature/cloud_info", 1); - pubCornerPoints = nh.advertise("lio_sam/feature/cloud_corner", 1); - pubSurfacePoints = nh.advertise("lio_sam/feature/cloud_surface", 1); - initializationValue(); } @@ -63,7 +69,7 @@ class FeatureExtraction : public ParamServer cloudLabel = new int[N_SCAN*Horizon_SCAN]; } - void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn) + void laserCloudInfoHandler(const lio_sam::msg::CloudInfo::SharedPtr msgIn) { cloudInfo = *msgIn; // new cloud info cloudHeader = msgIn->header; // new cloud header @@ -83,12 +89,12 @@ class FeatureExtraction : public ParamServer int cloudSize = extractedCloud->points.size(); for (int i = 5; i < cloudSize - 5; i++) { - float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4] - + cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2] - + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10 - + cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2] - + cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4] - + cloudInfo.pointRange[i+5]; + float diffRange = cloudInfo.point_range[i-5] + cloudInfo.point_range[i-4] + + cloudInfo.point_range[i-3] + cloudInfo.point_range[i-2] + + cloudInfo.point_range[i-1] - cloudInfo.point_range[i] * 10 + + cloudInfo.point_range[i+1] + cloudInfo.point_range[i+2] + + cloudInfo.point_range[i+3] + cloudInfo.point_range[i+4] + + cloudInfo.point_range[i+5]; cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ; @@ -107,10 +113,9 @@ class FeatureExtraction : public ParamServer for (int i = 5; i < cloudSize - 6; ++i) { // occluded points - float depth1 = cloudInfo.pointRange[i]; - float depth2 = cloudInfo.pointRange[i+1]; - int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i])); - + float depth1 = cloudInfo.point_range[i]; + float depth2 = cloudInfo.point_range[i+1]; + int columnDiff = std::abs(int(cloudInfo.point_col_ind[i+1] - cloudInfo.point_col_ind[i])); if (columnDiff < 10){ // 10 pixel diff in range image if (depth1 - depth2 > 0.3){ @@ -130,10 +135,10 @@ class FeatureExtraction : public ParamServer } } // parallel beam - float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i])); - float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i])); + float diff1 = std::abs(float(cloudInfo.point_range[i-1] - cloudInfo.point_range[i])); + float diff2 = std::abs(float(cloudInfo.point_range[i+1] - cloudInfo.point_range[i])); - if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i]) + if (diff1 > 0.02 * cloudInfo.point_range[i] && diff2 > 0.02 * cloudInfo.point_range[i]) cloudNeighborPicked[i] = 1; } } @@ -153,8 +158,8 @@ class FeatureExtraction : public ParamServer for (int j = 0; j < 6; j++) { - int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6; - int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1; + int sp = (cloudInfo.start_ring_index[i] * (6 - j) + cloudInfo.end_ring_index[i] * j) / 6; + int ep = (cloudInfo.start_ring_index[i] * (5 - j) + cloudInfo.end_ring_index[i] * (j + 1)) / 6 - 1; if (sp >= ep) continue; @@ -178,14 +183,14 @@ class FeatureExtraction : public ParamServer cloudNeighborPicked[ind] = 1; for (int l = 1; l <= 5; l++) { - int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1])); + int columnDiff = std::abs(int(cloudInfo.point_col_ind[ind + l] - cloudInfo.point_col_ind[ind + l - 1])); if (columnDiff > 10) break; cloudNeighborPicked[ind + l] = 1; } for (int l = -1; l >= -5; l--) { - int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1])); + int columnDiff = std::abs(int(cloudInfo.point_col_ind[ind + l] - cloudInfo.point_col_ind[ind + l + 1])); if (columnDiff > 10) break; cloudNeighborPicked[ind + l] = 1; @@ -203,16 +208,14 @@ class FeatureExtraction : public ParamServer cloudNeighborPicked[ind] = 1; for (int l = 1; l <= 5; l++) { - - int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1])); + int columnDiff = std::abs(int(cloudInfo.point_col_ind[ind + l] - cloudInfo.point_col_ind[ind + l - 1])); if (columnDiff > 10) break; cloudNeighborPicked[ind + l] = 1; } for (int l = -1; l >= -5; l--) { - - int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1])); + int columnDiff = std::abs(int(cloudInfo.point_col_ind[ind + l] - cloudInfo.point_col_ind[ind + l + 1])); if (columnDiff > 10) break; @@ -239,10 +242,10 @@ class FeatureExtraction : public ParamServer void freeCloudInfoMemory() { - cloudInfo.startRingIndex.clear(); - cloudInfo.endRingIndex.clear(); - cloudInfo.pointColInd.clear(); - cloudInfo.pointRange.clear(); + cloudInfo.start_ring_index.clear(); + cloudInfo.end_ring_index.clear(); + cloudInfo.point_col_ind.clear(); + cloudInfo.point_range.clear(); } void publishFeatureCloud() @@ -250,23 +253,29 @@ class FeatureExtraction : public ParamServer // free cloud info memory freeCloudInfoMemory(); // save newly extracted features - cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame); - cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame); + cloudInfo.cloud_corner = publishCloud(pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame); + cloudInfo.cloud_surface = publishCloud(pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame); // publish to mapOptimization - pubLaserCloudInfo.publish(cloudInfo); + pubLaserCloudInfo->publish(cloudInfo); } }; int main(int argc, char** argv) { - ros::init(argc, argv, "lio_sam"); + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + rclcpp::executors::SingleThreadedExecutor exec; + + auto FE = std::make_shared(options); + + exec.add_node(FE); - FeatureExtraction FE; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> Feature Extraction Started.\033[0m"); - ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m"); - - ros::spin(); + exec.spin(); + rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/src/imageProjection.cpp b/src/imageProjection.cpp index ae96dae4..2b3b452d 100644 --- a/src/imageProjection.cpp +++ b/src/imageProjection.cpp @@ -1,5 +1,5 @@ -#include "utility.h" -#include "lio_sam/cloud_info.h" +#include "utility.hpp" +#include "lio_sam/msg/cloud_info.hpp" struct VelodynePointXYZIRT { @@ -42,20 +42,20 @@ class ImageProjection : public ParamServer std::mutex imuLock; std::mutex odoLock; - ros::Subscriber subLaserCloud; - ros::Publisher pubLaserCloud; - - ros::Publisher pubExtractedCloud; - ros::Publisher pubLaserCloudInfo; + rclcpp::Subscription::SharedPtr subLaserCloud; + rclcpp::Publisher::SharedPtr pubLaserCloud; + + rclcpp::Publisher::SharedPtr pubExtractedCloud; + rclcpp::Publisher::SharedPtr pubLaserCloudInfo; - ros::Subscriber subImu; - std::deque imuQueue; + rclcpp::Subscription::SharedPtr subImu; + std::deque imuQueue; - ros::Subscriber subOdom; - std::deque odomQueue; + rclcpp::Subscription::SharedPtr subOdom; + std::deque odomQueue; - std::deque cloudQueue; - sensor_msgs::PointCloud2 currentCloudMsg; + std::deque cloudQueue; + sensor_msgs::msg::PointCloud2 currentCloudMsg; double *imuTime = new double[queueLength]; double *imuRotX = new double[queueLength]; @@ -79,22 +79,30 @@ class ImageProjection : public ParamServer float odomIncreY; float odomIncreZ; - lio_sam::cloud_info cloudInfo; + lio_sam::msg::CloudInfo cloudInfo; double timeScanCur; double timeScanEnd; - std_msgs::Header cloudHeader; + std_msgs::msg::Header cloudHeader; public: - ImageProjection(): - deskewFlag(0) + ImageProjection(const rclcpp::NodeOptions & options) : + ParamServer("lio_sam_imageProjection", options), deskewFlag(0) { - subImu = nh.subscribe(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay()); - subOdom = nh.subscribe(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay()); - subLaserCloud = nh.subscribe(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay()); - - pubExtractedCloud = nh.advertise ("lio_sam/deskew/cloud_deskewed", 1); - pubLaserCloudInfo = nh.advertise ("lio_sam/deskew/cloud_info", 1); + subImu = create_subscription( + imuTopic, qos_imu, + std::bind(&ImageProjection::imuHandler, this, std::placeholders::_1)); + subOdom = create_subscription( + odomTopic + "_incremental", qos_imu, + std::bind(&ImageProjection::odometryHandler, this, std::placeholders::_1)); + subLaserCloud = create_subscription( + pointCloudTopic, qos_lidar, + std::bind(&ImageProjection::cloudHandler, this, std::placeholders::_1)); + + pubExtractedCloud = create_publisher( + "lio_sam/deskew/cloud_deskewed", 1); + pubLaserCloudInfo = create_publisher( + "lio_sam/deskew/cloud_info", qos); allocateMemory(); resetParameters(); @@ -111,11 +119,11 @@ class ImageProjection : public ParamServer fullCloud->points.resize(N_SCAN*Horizon_SCAN); - cloudInfo.startRingIndex.assign(N_SCAN, 0); - cloudInfo.endRingIndex.assign(N_SCAN, 0); + cloudInfo.start_ring_index.assign(N_SCAN, 0); + cloudInfo.end_ring_index.assign(N_SCAN, 0); - cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0); - cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0); + cloudInfo.point_col_ind.assign(N_SCAN*Horizon_SCAN, 0); + cloudInfo.point_range.assign(N_SCAN*Horizon_SCAN, 0); resetParameters(); } @@ -142,9 +150,9 @@ class ImageProjection : public ParamServer ~ImageProjection(){} - void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg) + void imuHandler(const sensor_msgs::msg::Imu::SharedPtr imuMsg) { - sensor_msgs::Imu thisImu = imuConverter(*imuMsg); + sensor_msgs::msg::Imu thisImu = imuConverter(*imuMsg); std::lock_guard lock1(imuLock); imuQueue.push_back(thisImu); @@ -167,13 +175,13 @@ class ImageProjection : public ParamServer // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl; } - void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg) + void odometryHandler(const nav_msgs::msg::Odometry::SharedPtr odometryMsg) { std::lock_guard lock2(odoLock); odomQueue.push_back(*odometryMsg); } - void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) + void cloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudMsg) { if (!cachePointCloud(laserCloudMsg)) return; @@ -190,7 +198,7 @@ class ImageProjection : public ParamServer resetParameters(); } - bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) + bool cachePointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr& laserCloudMsg) { // cache point cloud cloudQueue.push_back(*laserCloudMsg); @@ -224,20 +232,20 @@ class ImageProjection : public ParamServer } else { - ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor)); - ros::shutdown(); + RCLCPP_ERROR_STREAM(get_logger(), "Unknown sensor type: " << int(sensor)); + rclcpp::shutdown(); } // get timestamp cloudHeader = currentCloudMsg.header; - timeScanCur = cloudHeader.stamp.toSec(); + timeScanCur = stamp2Sec(cloudHeader.stamp); timeScanEnd = timeScanCur + laserCloudIn->points.back().time; // check dense flag if (laserCloudIn->is_dense == false) { - ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!"); - ros::shutdown(); + RCLCPP_ERROR(get_logger(), "Point cloud is not in dense format, please remove NaN points first!"); + rclcpp::shutdown(); } // check ring channel @@ -255,8 +263,8 @@ class ImageProjection : public ParamServer } if (ringFlag == -1) { - ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!"); - ros::shutdown(); + RCLCPP_ERROR(get_logger(), "Point cloud ring channel not available, please configure your point cloud data!"); + rclcpp::shutdown(); } } @@ -273,7 +281,7 @@ class ImageProjection : public ParamServer } } if (deskewFlag == -1) - ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!"); + RCLCPP_WARN(get_logger(), "Point cloud timestamp not available, deskew function disabled, system will drift significantly!"); } return true; @@ -285,9 +293,11 @@ class ImageProjection : public ParamServer std::lock_guard lock2(odoLock); // make sure IMU data available for the scan - if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd) + if (imuQueue.empty() || + stamp2Sec(imuQueue.front().header.stamp) > timeScanCur || + stamp2Sec(imuQueue.back().header.stamp) < timeScanEnd) { - ROS_DEBUG("Waiting for IMU data ..."); + RCLCPP_INFO(get_logger(), "Waiting for IMU data ..."); return false; } @@ -300,11 +310,11 @@ class ImageProjection : public ParamServer void imuDeskewInfo() { - cloudInfo.imuAvailable = false; + cloudInfo.imu_available = false; while (!imuQueue.empty()) { - if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01) + if (stamp2Sec(imuQueue.front().header.stamp) < timeScanCur - 0.01) imuQueue.pop_front(); else break; @@ -317,13 +327,12 @@ class ImageProjection : public ParamServer for (int i = 0; i < (int)imuQueue.size(); ++i) { - sensor_msgs::Imu thisImuMsg = imuQueue[i]; - double currentImuTime = thisImuMsg.header.stamp.toSec(); + sensor_msgs::msg::Imu thisImuMsg = imuQueue[i]; + double currentImuTime = stamp2Sec(thisImuMsg.header.stamp); // get roll, pitch, and yaw estimation for this scan if (currentImuTime <= timeScanCur) - imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit); - + imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imu_roll_init, &cloudInfo.imu_pitch_init, &cloudInfo.imu_yaw_init); if (currentImuTime > timeScanEnd + 0.01) break; @@ -354,16 +363,16 @@ class ImageProjection : public ParamServer if (imuPointerCur <= 0) return; - cloudInfo.imuAvailable = true; + cloudInfo.imu_available = true; } void odomDeskewInfo() { - cloudInfo.odomAvailable = false; + cloudInfo.odom_available = false; while (!odomQueue.empty()) { - if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01) + if (stamp2Sec(odomQueue.front().header.stamp) < timeScanCur - 0.01) odomQueue.pop_front(); else break; @@ -372,51 +381,51 @@ class ImageProjection : public ParamServer if (odomQueue.empty()) return; - if (odomQueue.front().header.stamp.toSec() > timeScanCur) + if (stamp2Sec(odomQueue.front().header.stamp) > timeScanCur) return; // get start odometry at the beinning of the scan - nav_msgs::Odometry startOdomMsg; + nav_msgs::msg::Odometry startOdomMsg; for (int i = 0; i < (int)odomQueue.size(); ++i) { startOdomMsg = odomQueue[i]; - if (ROS_TIME(&startOdomMsg) < timeScanCur) + if (stamp2Sec(startOdomMsg.header.stamp) < timeScanCur) continue; else break; } - tf::Quaternion orientation; - tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation); + tf2::Quaternion orientation; + tf2::fromMsg(startOdomMsg.pose.pose.orientation, orientation); double roll, pitch, yaw; - tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); + tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw); // Initial guess used in mapOptimization - cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x; - cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y; - cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z; - cloudInfo.initialGuessRoll = roll; - cloudInfo.initialGuessPitch = pitch; - cloudInfo.initialGuessYaw = yaw; + cloudInfo.initial_guess_x = startOdomMsg.pose.pose.position.x; + cloudInfo.initial_guess_y = startOdomMsg.pose.pose.position.y; + cloudInfo.initial_guess_z = startOdomMsg.pose.pose.position.z; + cloudInfo.initial_guess_roll = roll; + cloudInfo.initial_guess_pitch = pitch; + cloudInfo.initial_guess_yaw = yaw; - cloudInfo.odomAvailable = true; + cloudInfo.odom_available = true; // get end odometry at the end of the scan odomDeskewFlag = false; - if (odomQueue.back().header.stamp.toSec() < timeScanEnd) + if (stamp2Sec(odomQueue.back().header.stamp) < timeScanEnd) return; - nav_msgs::Odometry endOdomMsg; + nav_msgs::msg::Odometry endOdomMsg; for (int i = 0; i < (int)odomQueue.size(); ++i) { endOdomMsg = odomQueue[i]; - if (ROS_TIME(&endOdomMsg) < timeScanEnd) + if (stamp2Sec(endOdomMsg.header.stamp) < timeScanEnd) continue; else break; @@ -427,8 +436,8 @@ class ImageProjection : public ParamServer Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw); - tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation); - tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); + tf2::fromMsg(endOdomMsg.pose.pose.orientation, orientation); + tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw); Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw); Eigen::Affine3f transBt = transBegin.inverse() * transEnd; @@ -484,7 +493,7 @@ class ImageProjection : public ParamServer PointType deskewPoint(PointType *point, double relTime) { - if (deskewFlag == -1 || cloudInfo.imuAvailable == false) + if (deskewFlag == -1 || cloudInfo.imu_available == false) return *point; double pointTime = timeScanCur + relTime; @@ -565,44 +574,48 @@ class ImageProjection : public ParamServer // extract segmented cloud for lidar odometry for (int i = 0; i < N_SCAN; ++i) { - cloudInfo.startRingIndex[i] = count - 1 + 5; - + cloudInfo.start_ring_index[i] = count - 1 + 5; for (int j = 0; j < Horizon_SCAN; ++j) { if (rangeMat.at(i,j) != FLT_MAX) { // mark the points' column index for marking occlusion later - cloudInfo.pointColInd[count] = j; + cloudInfo.point_col_ind[count] = j; // save range info - cloudInfo.pointRange[count] = rangeMat.at(i,j); + cloudInfo.point_range[count] = rangeMat.at(i,j); // save extracted cloud extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); // size of extracted cloud ++count; } } - cloudInfo.endRingIndex[i] = count -1 - 5; + cloudInfo.end_ring_index[i] = count -1 - 5; } } void publishClouds() { cloudInfo.header = cloudHeader; - cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame); - pubLaserCloudInfo.publish(cloudInfo); + cloudInfo.cloud_deskewed = publishCloud(pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame); + pubLaserCloudInfo->publish(cloudInfo); } }; int main(int argc, char** argv) { - ros::init(argc, argv, "lio_sam"); + rclcpp::init(argc, argv); - ImageProjection IP; - - ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m"); + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + rclcpp::executors::MultiThreadedExecutor exec; - ros::MultiThreadedSpinner spinner(3); - spinner.spin(); - + auto IP = std::make_shared(options); + exec.add_node(IP); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> Image Projection Started.\033[0m"); + + exec.spin(); + + rclcpp::shutdown(); return 0; } diff --git a/src/imuPreintegration.cpp b/src/imuPreintegration.cpp index 5273f7fe..20e66be4 100644 --- a/src/imuPreintegration.cpp +++ b/src/imuPreintegration.cpp @@ -1,4 +1,4 @@ -#include "utility.h" +#include "utility.hpp" #include #include @@ -25,72 +25,60 @@ class TransformFusion : public ParamServer public: std::mutex mtx; - ros::Subscriber subImuOdometry; - ros::Subscriber subLaserOdometry; + rclcpp::Subscription::SharedPtr subImuOdometry; + rclcpp::Subscription::SharedPtr subLaserOdometry; - ros::Publisher pubImuOdometry; - ros::Publisher pubImuPath; + rclcpp::Publisher::SharedPtr pubImuOdometry; + rclcpp::Publisher::SharedPtr pubImuPath; - Eigen::Affine3f lidarOdomAffine; - Eigen::Affine3f imuOdomAffineFront; - Eigen::Affine3f imuOdomAffineBack; + Eigen::Isometry3d lidarOdomAffine; + Eigen::Isometry3d imuOdomAffineFront; + Eigen::Isometry3d imuOdomAffineBack; - tf::TransformListener tfListener; - tf::StampedTransform lidar2Baselink; + std::shared_ptr tfBuffer; + std::shared_ptr tfBroadcaster; + std::shared_ptr tfListener; + tf2::Stamped lidar2Baselink; double lidarOdomTime = -1; - deque imuOdomQueue; + deque imuOdomQueue; - TransformFusion() + TransformFusion(const rclcpp::NodeOptions & options) : ParamServer("lio_sam_transformFusion", options) { - if(lidarFrame != baselinkFrame) - { - try - { - tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0)); - tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink); - } - catch (tf::TransformException ex) - { - ROS_ERROR("%s",ex.what()); - } - } + tfBuffer = std::make_shared(get_clock()); + tfListener = std::make_shared(*tfBuffer); + + subLaserOdometry = create_subscription( + "lio_sam/mapping/odometry", qos, + std::bind(&TransformFusion::lidarOdometryHandler, this, std::placeholders::_1)); + subImuOdometry = create_subscription( + odomTopic+"_incremental", qos_imu, + std::bind(&TransformFusion::imuOdometryHandler, this, std::placeholders::_1)); - subLaserOdometry = nh.subscribe("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay()); - subImuOdometry = nh.subscribe(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay()); + pubImuOdometry = create_publisher(odomTopic, qos_imu); + pubImuPath = create_publisher("lio_sam/imu/path", qos); - pubImuOdometry = nh.advertise(odomTopic, 2000); - pubImuPath = nh.advertise ("lio_sam/imu/path", 1); + tfBroadcaster = std::make_unique(this); } - Eigen::Affine3f odom2affine(nav_msgs::Odometry odom) + Eigen::Isometry3d odom2affine(nav_msgs::msg::Odometry odom) { - double x, y, z, roll, pitch, yaw; - x = odom.pose.pose.position.x; - y = odom.pose.pose.position.y; - z = odom.pose.pose.position.z; - tf::Quaternion orientation; - tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation); - tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); - return pcl::getTransformation(x, y, z, roll, pitch, yaw); + tf2::Transform t; + tf2::fromMsg(odom.pose.pose, t); + return tf2::transformToEigen(tf2::toMsg(t)); } - void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) + void lidarOdometryHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg) { std::lock_guard lock(mtx); lidarOdomAffine = odom2affine(*odomMsg); - lidarOdomTime = odomMsg->header.stamp.toSec(); + lidarOdomTime = stamp2Sec(odomMsg->header.stamp); } - void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) + void imuOdometryHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg) { - // static tf - static tf::TransformBroadcaster tfMap2Odom; - static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0)); - tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame)); - std::lock_guard lock(mtx); imuOdomQueue.push_back(*odomMsg); @@ -100,54 +88,67 @@ class TransformFusion : public ParamServer return; while (!imuOdomQueue.empty()) { - if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime) + if (stamp2Sec(imuOdomQueue.front().header.stamp) <= lidarOdomTime) imuOdomQueue.pop_front(); else break; } - Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front()); - Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back()); - Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack; - Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre; - float x, y, z, roll, pitch, yaw; - pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw); - + Eigen::Isometry3d imuOdomAffineFront = odom2affine(imuOdomQueue.front()); + Eigen::Isometry3d imuOdomAffineBack = odom2affine(imuOdomQueue.back()); + Eigen::Isometry3d imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack; + Eigen::Isometry3d imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre; + auto t = tf2::eigenToTransform(imuOdomAffineLast); + tf2::Stamped tCur; + tf2::convert(t, tCur); + // publish latest odometry - nav_msgs::Odometry laserOdometry = imuOdomQueue.back(); - laserOdometry.pose.pose.position.x = x; - laserOdometry.pose.pose.position.y = y; - laserOdometry.pose.pose.position.z = z; - laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); - pubImuOdometry.publish(laserOdometry); + nav_msgs::msg::Odometry laserOdometry = imuOdomQueue.back(); + laserOdometry.pose.pose.position.x = t.transform.translation.x; + laserOdometry.pose.pose.position.y = t.transform.translation.y; + laserOdometry.pose.pose.position.z = t.transform.translation.z; + laserOdometry.pose.pose.orientation = t.transform.rotation; + pubImuOdometry->publish(laserOdometry); // publish tf - static tf::TransformBroadcaster tfOdom2BaseLink; - tf::Transform tCur; - tf::poseMsgToTF(laserOdometry.pose.pose, tCur); if(lidarFrame != baselinkFrame) - tCur = tCur * lidar2Baselink; - tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame); - tfOdom2BaseLink.sendTransform(odom_2_baselink); + { + try + { + tf2::fromMsg(tfBuffer->lookupTransform( + lidarFrame, baselinkFrame, rclcpp::Time(0)), lidar2Baselink); + } + catch (tf2::TransformException ex) + { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + } + tf2::Stamped tb( + tCur * lidar2Baselink, tf2_ros::fromMsg(odomMsg->header.stamp), odometryFrame); + tCur = tb; + } + geometry_msgs::msg::TransformStamped ts; + tf2::convert(tCur, ts); + ts.child_frame_id = baselinkFrame; + tfBroadcaster->sendTransform(ts); // publish IMU path - static nav_msgs::Path imuPath; + static nav_msgs::msg::Path imuPath; static double last_path_time = -1; - double imuTime = imuOdomQueue.back().header.stamp.toSec(); + double imuTime = stamp2Sec(imuOdomQueue.back().header.stamp); if (imuTime - last_path_time > 0.1) { last_path_time = imuTime; - geometry_msgs::PoseStamped pose_stamped; + geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.header.stamp = imuOdomQueue.back().header.stamp; pose_stamped.header.frame_id = odometryFrame; pose_stamped.pose = laserOdometry.pose.pose; imuPath.poses.push_back(pose_stamped); - while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0) + while(!imuPath.poses.empty() && stamp2Sec(imuPath.poses.front().header.stamp) < lidarOdomTime - 1.0) imuPath.poses.erase(imuPath.poses.begin()); - if (pubImuPath.getNumSubscribers() != 0) + if (pubImuPath->get_subscription_count() != 0) { imuPath.header.stamp = imuOdomQueue.back().header.stamp; imuPath.header.frame_id = odometryFrame; - pubImuPath.publish(imuPath); + pubImuPath->publish(imuPath); } } } @@ -159,9 +160,9 @@ class IMUPreintegration : public ParamServer std::mutex mtx; - ros::Subscriber subImu; - ros::Subscriber subOdometry; - ros::Publisher pubImuOdometry; + rclcpp::Subscription::SharedPtr subImu; + rclcpp::Subscription::SharedPtr subOdometry; + rclcpp::Publisher::SharedPtr pubImuOdometry; bool systemInitialized = false; @@ -176,8 +177,8 @@ class IMUPreintegration : public ParamServer gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_; gtsam::PreintegratedImuMeasurements *imuIntegratorImu_; - std::deque imuQueOpt; - std::deque imuQueImu; + std::deque imuQueOpt; + std::deque imuQueImu; gtsam::Pose3 prevPose_; gtsam::Vector3 prevVel_; @@ -202,12 +203,17 @@ class IMUPreintegration : public ParamServer gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z())); gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z())); - IMUPreintegration() + IMUPreintegration(const rclcpp::NodeOptions & options) : + ParamServer("lio_sam_imu_preintegration", options) { - subImu = nh.subscribe (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay()); - subOdometry = nh.subscribe("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay()); + subImu = create_subscription( + imuTopic, qos_imu, + std::bind(&IMUPreintegration::imuHandler, this, std::placeholders::_1)); + subOdometry = create_subscription( + "lio_sam/mapping/odometry_incremental", qos, + std::bind(&IMUPreintegration::odometryHandler, this, std::placeholders::_1)); - pubImuOdometry = nh.advertise (odomTopic+"_incremental", 2000); + pubImuOdometry = create_publisher(odomTopic+"_incremental", qos_imu); boost::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU(imuGravity); p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous @@ -247,11 +253,11 @@ class IMUPreintegration : public ParamServer systemInitialized = false; } - void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) + void odometryHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg) { std::lock_guard lock(mtx); - double currentCorrectionTime = ROS_TIME(odomMsg); + double currentCorrectionTime = stamp2Sec(odomMsg->header.stamp); // make sure we have imu data to integrate if (imuQueOpt.empty()) @@ -276,9 +282,9 @@ class IMUPreintegration : public ParamServer // pop old IMU message while (!imuQueOpt.empty()) { - if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t) + if (stamp2Sec(imuQueOpt.front().header.stamp) < currentCorrectionTime - delta_t) { - lastImuT_opt = ROS_TIME(&imuQueOpt.front()); + lastImuT_opt = stamp2Sec(imuQueOpt.front().header.stamp); imuQueOpt.pop_front(); } else @@ -349,8 +355,8 @@ class IMUPreintegration : public ParamServer while (!imuQueOpt.empty()) { // pop and integrate imu data that is between two optimizations - sensor_msgs::Imu *thisImu = &imuQueOpt.front(); - double imuTime = ROS_TIME(thisImu); + sensor_msgs::msg::Imu *thisImu = &imuQueOpt.front(); + double imuTime = stamp2Sec(thisImu->header.stamp); if (imuTime < currentCorrectionTime - delta_t) { double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt); @@ -406,9 +412,9 @@ class IMUPreintegration : public ParamServer prevBiasOdom = prevBias_; // first pop imu message older than current correction data double lastImuQT = -1; - while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t) + while (!imuQueImu.empty() && stamp2Sec(imuQueImu.front().header.stamp) < currentCorrectionTime - delta_t) { - lastImuQT = ROS_TIME(&imuQueImu.front()); + lastImuQT = stamp2Sec(imuQueImu.front().header.stamp); imuQueImu.pop_front(); } // repropogate @@ -419,8 +425,8 @@ class IMUPreintegration : public ParamServer // integrate imu message from the beginning of this optimization for (int i = 0; i < (int)imuQueImu.size(); ++i) { - sensor_msgs::Imu *thisImu = &imuQueImu[i]; - double imuTime = ROS_TIME(thisImu); + sensor_msgs::msg::Imu *thisImu = &imuQueImu[i]; + double imuTime = stamp2Sec(thisImu->header.stamp); double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT); imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z), @@ -438,7 +444,7 @@ class IMUPreintegration : public ParamServer Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z()); if (vel.norm() > 30) { - ROS_WARN("Large velocity, reset IMU-preintegration!"); + RCLCPP_WARN(get_logger(), "Large velocity, reset IMU-preintegration!"); return true; } @@ -446,18 +452,18 @@ class IMUPreintegration : public ParamServer Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z()); if (ba.norm() > 1.0 || bg.norm() > 1.0) { - ROS_WARN("Large bias, reset IMU-preintegration!"); + RCLCPP_WARN(get_logger(), "Large bias, reset IMU-preintegration!"); return true; } return false; } - void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw) + void imuHandler(const sensor_msgs::msg::Imu::SharedPtr imu_raw) { std::lock_guard lock(mtx); - sensor_msgs::Imu thisImu = imuConverter(*imu_raw); + sensor_msgs::msg::Imu thisImu = imuConverter(*imu_raw); imuQueOpt.push_back(thisImu); imuQueImu.push_back(thisImu); @@ -465,7 +471,7 @@ class IMUPreintegration : public ParamServer if (doneFirstOpt == false) return; - double imuTime = ROS_TIME(&thisImu); + double imuTime = stamp2Sec(thisImu.header.stamp); double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu); lastImuT_imu = imuTime; @@ -477,7 +483,7 @@ class IMUPreintegration : public ParamServer gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); // publish odometry - nav_msgs::Odometry odometry; + auto odometry = nav_msgs::msg::Odometry(); odometry.header.stamp = thisImu.header.stamp; odometry.header.frame_id = odometryFrame; odometry.child_frame_id = "odom_imu"; @@ -500,23 +506,28 @@ class IMUPreintegration : public ParamServer odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x(); odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y(); odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z(); - pubImuOdometry.publish(odometry); + pubImuOdometry->publish(odometry); } }; int main(int argc, char** argv) -{ - ros::init(argc, argv, "roboat_loam"); - - IMUPreintegration ImuP; +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + rclcpp::executors::MultiThreadedExecutor e; + + auto ImuP = std::make_shared(options); + auto TF = std::make_shared(options); + e.add_node(ImuP); + e.add_node(TF); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> IMU Preintegration Started.\033[0m"); - TransformFusion TF; + e.spin(); - ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m"); - - ros::MultiThreadedSpinner spinner(4); - spinner.spin(); - + rclcpp::shutdown(); return 0; } diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp index dd9ab9c9..7c58eb8a 100644 --- a/src/mapOptmization.cpp +++ b/src/mapOptmization.cpp @@ -1,6 +1,5 @@ -#include "utility.h" -#include "lio_sam/cloud_info.h" -#include "lio_sam/save_map.h" +#include "utility.hpp" +#include "lio_sam/msg/cloud_info.hpp" #include #include @@ -60,27 +59,25 @@ class mapOptimization : public ParamServer Values isamCurrentEstimate; Eigen::MatrixXd poseCovariance; - ros::Publisher pubLaserCloudSurround; - ros::Publisher pubLaserOdometryGlobal; - ros::Publisher pubLaserOdometryIncremental; - ros::Publisher pubKeyPoses; - ros::Publisher pubPath; + rclcpp::Publisher::SharedPtr pubLaserCloudSurround; + rclcpp::Publisher::SharedPtr pubLaserOdometryGlobal; + rclcpp::Publisher::SharedPtr pubLaserOdometryIncremental; + rclcpp::Publisher::SharedPtr pubKeyPoses; + rclcpp::Publisher::SharedPtr pubPath; - ros::Publisher pubHistoryKeyFrames; - ros::Publisher pubIcpKeyFrames; - ros::Publisher pubRecentKeyFrames; - ros::Publisher pubRecentKeyFrame; - ros::Publisher pubCloudRegisteredRaw; - ros::Publisher pubLoopConstraintEdge; + rclcpp::Publisher::SharedPtr pubHistoryKeyFrames; + rclcpp::Publisher::SharedPtr pubIcpKeyFrames; + rclcpp::Publisher::SharedPtr pubRecentKeyFrames; + rclcpp::Publisher::SharedPtr pubRecentKeyFrame; + rclcpp::Publisher::SharedPtr pubCloudRegisteredRaw; + rclcpp::Publisher::SharedPtr pubLoopConstraintEdge; - ros::Subscriber subCloud; - ros::Subscriber subGPS; - ros::Subscriber subLoop; + rclcpp::Subscription::SharedPtr subCloud; + rclcpp::Subscription::SharedPtr subGPS; + rclcpp::Subscription::SharedPtr subLoop; - ros::ServiceServer srvSaveMap; - - std::deque gpsQueue; - lio_sam::cloud_info cloudInfo; + std::deque gpsQueue; + lio_sam::msg::CloudInfo cloudInfo; vector::Ptr> cornerCloudKeyFrames; vector::Ptr> surfCloudKeyFrames; @@ -121,8 +118,8 @@ class mapOptimization : public ParamServer pcl::VoxelGrid downSizeFilterSurf; pcl::VoxelGrid downSizeFilterICP; pcl::VoxelGrid downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization - - ros::Time timeLaserInfoStamp; + + rclcpp::Time timeLaserInfoStamp; double timeLaserInfoCur; float transformTobeMapped[6]; @@ -131,7 +128,7 @@ class mapOptimization : public ParamServer std::mutex mtxLoopInfo; bool isDegenerate = false; - cv::Mat matP; + Eigen::Matrix matP; int laserCloudCornerFromMapDSNum = 0; int laserCloudSurfFromMapDSNum = 0; @@ -143,41 +140,48 @@ class mapOptimization : public ParamServer vector> loopIndexQueue; vector loopPoseQueue; vector loopNoiseQueue; - deque loopInfoVec; + deque loopInfoVec; - nav_msgs::Path globalPath; + nav_msgs::msg::Path globalPath; Eigen::Affine3f transPointAssociateToMap; Eigen::Affine3f incrementalOdometryAffineFront; Eigen::Affine3f incrementalOdometryAffineBack; + std::unique_ptr br; - mapOptimization() + mapOptimization(const rclcpp::NodeOptions & options) : ParamServer("lio_sam_mapOptimization", options) { ISAM2Params parameters; parameters.relinearizeThreshold = 0.1; parameters.relinearizeSkip = 1; isam = new ISAM2(parameters); - pubKeyPoses = nh.advertise("lio_sam/mapping/trajectory", 1); - pubLaserCloudSurround = nh.advertise("lio_sam/mapping/map_global", 1); - pubLaserOdometryGlobal = nh.advertise ("lio_sam/mapping/odometry", 1); - pubLaserOdometryIncremental = nh.advertise ("lio_sam/mapping/odometry_incremental", 1); - pubPath = nh.advertise("lio_sam/mapping/path", 1); - - subCloud = nh.subscribe("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay()); - subGPS = nh.subscribe (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay()); - subLoop = nh.subscribe("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay()); - - srvSaveMap = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this); - - pubHistoryKeyFrames = nh.advertise("lio_sam/mapping/icp_loop_closure_history_cloud", 1); - pubIcpKeyFrames = nh.advertise("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1); - pubLoopConstraintEdge = nh.advertise("/lio_sam/mapping/loop_closure_constraints", 1); - - pubRecentKeyFrames = nh.advertise("lio_sam/mapping/map_local", 1); - pubRecentKeyFrame = nh.advertise("lio_sam/mapping/cloud_registered", 1); - pubCloudRegisteredRaw = nh.advertise("lio_sam/mapping/cloud_registered_raw", 1); + pubKeyPoses = create_publisher("lio_sam/mapping/trajectory", 1); + pubLaserCloudSurround = create_publisher("lio_sam/mapping/map_global", 1); + pubLaserOdometryGlobal = create_publisher("lio_sam/mapping/odometry", qos); + pubLaserOdometryIncremental = create_publisher( + "lio_sam/mapping/odometry_incremental", qos); + pubPath = create_publisher("lio_sam/mapping/path", 1); + br = std::make_unique(this); + + subCloud = create_subscription( + "lio_sam/feature/cloud_info", qos, + std::bind(&mapOptimization::laserCloudInfoHandler, this, std::placeholders::_1)); + subGPS = create_subscription( + gpsTopic, 200, + std::bind(&mapOptimization::gpsHandler, this, std::placeholders::_1)); + subLoop = create_subscription( + "lio_loop/loop_closure_detection", qos, + std::bind(&mapOptimization::loopInfoHandler, this, std::placeholders::_1)); + + pubHistoryKeyFrames = create_publisher("lio_sam/mapping/icp_loop_closure_history_cloud", 1); + pubIcpKeyFrames = create_publisher("lio_sam/mapping/icp_loop_closure_history_cloud", 1); + pubLoopConstraintEdge = create_publisher("/lio_sam/mapping/loop_closure_constraints", 1); + + pubRecentKeyFrames = create_publisher("lio_sam/mapping/map_local", 1); + pubRecentKeyFrame = create_publisher("lio_sam/mapping/cloud_registered", 1); + pubCloudRegisteredRaw = create_publisher("lio_sam/mapping/cloud_registered_raw", 1); downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize); downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize); @@ -227,14 +231,14 @@ class mapOptimization : public ParamServer transformTobeMapped[i] = 0; } - matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0)); + matP.setZero(); } - void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn) + void laserCloudInfoHandler(const lio_sam::msg::CloudInfo::SharedPtr msgIn) { // extract time stamp timeLaserInfoStamp = msgIn->header.stamp; - timeLaserInfoCur = msgIn->header.stamp.toSec(); + timeLaserInfoCur = stamp2Sec(msgIn->header.stamp); // extract info and feature cloud cloudInfo = *msgIn; @@ -266,7 +270,7 @@ class mapOptimization : public ParamServer } } - void gpsHandler(const nav_msgs::Odometry::ConstPtr& gpsMsg) + void gpsHandler(const nav_msgs::msg::Odometry::SharedPtr gpsMsg) { gpsQueue.push_back(*gpsMsg); } @@ -313,7 +317,7 @@ class mapOptimization : public ParamServer } Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint) - { + { return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw); } @@ -334,108 +338,48 @@ class mapOptimization : public ParamServer return thisPose6D; } - - - - - - - - - - - - - - - bool saveMapService(lio_sam::save_mapRequest& req, lio_sam::save_mapResponse& res) - { - string saveMapDirectory; - - cout << "****************************************************" << endl; - cout << "Saving map to pcd files ..." << endl; - if(req.destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory; - else saveMapDirectory = std::getenv("HOME") + req.destination; - cout << "Save destination: " << saveMapDirectory << endl; - // create directory and remove old files; - int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str()); - unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str()); - // save key frame transformations - pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D); - pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D); - // extract global point cloud map - pcl::PointCloud::Ptr globalCornerCloud(new pcl::PointCloud()); - pcl::PointCloud::Ptr globalCornerCloudDS(new pcl::PointCloud()); - pcl::PointCloud::Ptr globalSurfCloud(new pcl::PointCloud()); - pcl::PointCloud::Ptr globalSurfCloudDS(new pcl::PointCloud()); - pcl::PointCloud::Ptr globalMapCloud(new pcl::PointCloud()); - for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) { - *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); - *globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); - cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ..."; - } - - if(req.resolution != 0) - { - cout << "\n\nSave resolution: " << req.resolution << endl; - - // down-sample and save corner cloud - downSizeFilterCorner.setInputCloud(globalCornerCloud); - downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution); - downSizeFilterCorner.filter(*globalCornerCloudDS); - pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS); - // down-sample and save surf cloud - downSizeFilterSurf.setInputCloud(globalSurfCloud); - downSizeFilterSurf.setLeafSize(req.resolution, req.resolution, req.resolution); - downSizeFilterSurf.filter(*globalSurfCloudDS); - pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS); - } - else - { - // save corner cloud - pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud); - // save surf cloud - pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud); - } - - // save global point cloud map - *globalMapCloud += *globalCornerCloud; - *globalMapCloud += *globalSurfCloud; - - int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud); - res.success = ret == 0; - - downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize); - downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize); - - cout << "****************************************************" << endl; - cout << "Saving map to pcd files completed\n" << endl; - - return true; - } - void visualizeGlobalMapThread() { - ros::Rate rate(0.2); - while (ros::ok()){ + rclcpp::Rate rate(0.2); + while (rclcpp::ok()){ rate.sleep(); publishGlobalMap(); } - if (savePCD == false) return; - - lio_sam::save_mapRequest req; - lio_sam::save_mapResponse res; - - if(!saveMapService(req, res)){ - cout << "Fail to save map" << endl; + cout << "****************************************************" << endl; + cout << "Saving map to pcd files ..." << endl; + savePCDDirectory = std::getenv("HOME") + savePCDDirectory; + int unused = system((std::string("exec rm -r ") + savePCDDirectory).c_str()); + unused = system((std::string("mkdir ") + savePCDDirectory).c_str()); + pcl::io::savePCDFileASCII(savePCDDirectory + "trajectory.pcd", *cloudKeyPoses3D); + pcl::io::savePCDFileASCII(savePCDDirectory + "transformations.pcd", *cloudKeyPoses6D); + pcl::PointCloud::Ptr globalCornerCloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr globalCornerCloudDS(new pcl::PointCloud()); + pcl::PointCloud::Ptr globalSurfCloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr globalSurfCloudDS(new pcl::PointCloud()); + pcl::PointCloud::Ptr globalMapCloud(new pcl::PointCloud()); + for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) { + *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); + *globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); + cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ..."; } + downSizeFilterCorner.setInputCloud(globalCornerCloud); + downSizeFilterCorner.filter(*globalCornerCloudDS); + pcl::io::savePCDFileASCII(savePCDDirectory + "cloudCorner.pcd", *globalCornerCloudDS); + downSizeFilterSurf.setInputCloud(globalSurfCloud); + downSizeFilterSurf.filter(*globalSurfCloudDS); + pcl::io::savePCDFileASCII(savePCDDirectory + "cloudSurf.pcd", *globalSurfCloudDS); + *globalMapCloud += *globalCornerCloud; + *globalMapCloud += *globalSurfCloud; + pcl::io::savePCDFileASCII(savePCDDirectory + "cloudGlobal.pcd", *globalMapCloud); + cout << "****************************************************" << endl; + cout << "Saving map to pcd files completed" << endl; } void publishGlobalMap() { - if (pubLaserCloudSurround.getNumSubscribers() == 0) + if (pubLaserCloudSurround->get_subscription_count() == 0) return; if (cloudKeyPoses3D->points.empty() == true) @@ -482,7 +426,7 @@ class mapOptimization : public ParamServer downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames); downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS); - publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame); + publishCloud(pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame); } @@ -501,8 +445,8 @@ class mapOptimization : public ParamServer if (loopClosureEnableFlag == false) return; - ros::Rate rate(loopClosureFrequency); - while (ros::ok()) + rclcpp::Rate rate(loopClosureFrequency); + while (rclcpp::ok()) { rate.sleep(); performLoopClosure(); @@ -510,7 +454,7 @@ class mapOptimization : public ParamServer } } - void loopInfoHandler(const std_msgs::Float64MultiArray::ConstPtr& loopMsg) + void loopInfoHandler(const std_msgs::msg::Float64MultiArray::SharedPtr loopMsg) { std::lock_guard lock(mtxLoopInfo); if (loopMsg->data.size() != 2) @@ -547,8 +491,8 @@ class mapOptimization : public ParamServer loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum); if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000) return; - if (pubHistoryKeyFrames.getNumSubscribers() != 0) - publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame); + if (pubHistoryKeyFrames->get_subscription_count() != 0) + publishCloud(pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame); } // ICP Settings @@ -569,11 +513,11 @@ class mapOptimization : public ParamServer return; // publish corrected cloud - if (pubIcpKeyFrames.getNumSubscribers() != 0) + if (pubIcpKeyFrames->get_subscription_count() != 0) { pcl::PointCloud::Ptr closed_cloud(new pcl::PointCloud()); pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation()); - publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame); + publishCloud(pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame); } // Get pose transformation @@ -720,14 +664,14 @@ class mapOptimization : public ParamServer { if (loopIndexContainer.empty()) return; - - visualization_msgs::MarkerArray markerArray; + + visualization_msgs::msg::MarkerArray markerArray; // loop nodes - visualization_msgs::Marker markerNode; + visualization_msgs::msg::Marker markerNode; markerNode.header.frame_id = odometryFrame; markerNode.header.stamp = timeLaserInfoStamp; - markerNode.action = visualization_msgs::Marker::ADD; - markerNode.type = visualization_msgs::Marker::SPHERE_LIST; + markerNode.action = visualization_msgs::msg::Marker::ADD; + markerNode.type = visualization_msgs::msg::Marker::SPHERE_LIST; markerNode.ns = "loop_nodes"; markerNode.id = 0; markerNode.pose.orientation.w = 1; @@ -735,11 +679,11 @@ class mapOptimization : public ParamServer markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1; markerNode.color.a = 1; // loop edges - visualization_msgs::Marker markerEdge; + visualization_msgs::msg::Marker markerEdge; markerEdge.header.frame_id = odometryFrame; markerEdge.header.stamp = timeLaserInfoStamp; - markerEdge.action = visualization_msgs::Marker::ADD; - markerEdge.type = visualization_msgs::Marker::LINE_LIST; + markerEdge.action = visualization_msgs::msg::Marker::ADD; + markerEdge.type = visualization_msgs::msg::Marker::LINE_LIST; markerEdge.ns = "loop_edges"; markerEdge.id = 1; markerEdge.pose.orientation.w = 1; @@ -751,7 +695,7 @@ class mapOptimization : public ParamServer { int key_cur = it->first; int key_pre = it->second; - geometry_msgs::Point p; + geometry_msgs::msg::Point p; p.x = copy_cloudKeyPoses6D->points[key_cur].x; p.y = copy_cloudKeyPoses6D->points[key_cur].y; p.z = copy_cloudKeyPoses6D->points[key_cur].z; @@ -766,7 +710,7 @@ class mapOptimization : public ParamServer markerArray.markers.push_back(markerNode); markerArray.markers.push_back(markerEdge); - pubLoopConstraintEdge.publish(markerArray); + pubLoopConstraintEdge->publish(markerArray); } @@ -788,24 +732,25 @@ class mapOptimization : public ParamServer // initialization if (cloudKeyPoses3D->points.empty()) { - transformTobeMapped[0] = cloudInfo.imuRollInit; - transformTobeMapped[1] = cloudInfo.imuPitchInit; - transformTobeMapped[2] = cloudInfo.imuYawInit; + transformTobeMapped[0] = cloudInfo.imu_roll_init; + transformTobeMapped[1] = cloudInfo.imu_pitch_init; + transformTobeMapped[2] = cloudInfo.imu_yaw_init; if (!useImuHeadingInitialization) transformTobeMapped[2] = 0; - lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return; + lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); // save imu before return; return; } // use imu pre-integration estimation for pose guess static bool lastImuPreTransAvailable = false; static Eigen::Affine3f lastImuPreTransformation; - if (cloudInfo.odomAvailable == true) + if (cloudInfo.odom_available == true) { - Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ, - cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw); + Eigen::Affine3f transBack = pcl::getTransformation( + cloudInfo.initial_guess_x, cloudInfo.initial_guess_y, cloudInfo.initial_guess_z, + cloudInfo.initial_guess_roll, cloudInfo.initial_guess_pitch, cloudInfo.initial_guess_yaw); if (lastImuPreTransAvailable == false) { lastImuPreTransformation = transBack; @@ -819,15 +764,15 @@ class mapOptimization : public ParamServer lastImuPreTransformation = transBack; - lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return; + lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); // save imu before return; return; } } // use imu incremental estimation for pose guess (only rotation) - if (cloudInfo.imuAvailable == true) + if (cloudInfo.imu_available == true) { - Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); + Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack; Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped); @@ -835,7 +780,7 @@ class mapOptimization : public ParamServer pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); - lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return; + lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); // save imu before return; return; } } @@ -1181,6 +1126,7 @@ class mapOptimization : public ParamServer cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0)); cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0)); cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0)); + cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0)); PointType pointOri, coeff; @@ -1301,31 +1247,31 @@ class mapOptimization : public ParamServer transformUpdate(); } else { - ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum); + RCLCPP_WARN(get_logger(), "Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum); } } void transformUpdate() { - if (cloudInfo.imuAvailable == true) + if (cloudInfo.imu_available == true) { - if (std::abs(cloudInfo.imuPitchInit) < 1.4) + if (std::abs(cloudInfo.imu_pitch_init) < 1.4) { double imuWeight = imuRPYWeight; - tf::Quaternion imuQuaternion; - tf::Quaternion transformQuaternion; + tf2::Quaternion imuQuaternion; + tf2::Quaternion transformQuaternion; double rollMid, pitchMid, yawMid; // slerp roll transformQuaternion.setRPY(transformTobeMapped[0], 0, 0); - imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0); - tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); + imuQuaternion.setRPY(cloudInfo.imu_roll_init, 0, 0); + tf2::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); transformTobeMapped[0] = rollMid; // slerp pitch transformQuaternion.setRPY(0, transformTobeMapped[1], 0); - imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0); - tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); + imuQuaternion.setRPY(0, cloudInfo.imu_pitch_init, 0); + tf2::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); transformTobeMapped[1] = pitchMid; } } @@ -1407,19 +1353,19 @@ class mapOptimization : public ParamServer while (!gpsQueue.empty()) { - if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2) + if (stamp2Sec(gpsQueue.front().header.stamp) < timeLaserInfoCur - 0.2) { // message too old gpsQueue.pop_front(); } - else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2) + else if (stamp2Sec(gpsQueue.front().header.stamp) > timeLaserInfoCur + 0.2) { // message too new break; } else { - nav_msgs::Odometry thisGPS = gpsQueue.front(); + nav_msgs::msg::Odometry thisGPS = gpsQueue.front(); gpsQueue.pop_front(); // GPS too noisy, skip @@ -1605,13 +1551,14 @@ class mapOptimization : public ParamServer void updatePath(const PointTypePose& pose_in) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time); + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.stamp = rclcpp::Time(pose_in.time * 1e9); pose_stamped.header.frame_id = odometryFrame; pose_stamped.pose.position.x = pose_in.x; pose_stamped.pose.position.y = pose_in.y; pose_stamped.pose.position.z = pose_in.z; - tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw); + tf2::Quaternion q; + q.setRPY(pose_in.roll, pose_in.pitch, pose_in.yaw); pose_stamped.pose.orientation.x = q.x(); pose_stamped.pose.orientation.y = q.y(); pose_stamped.pose.orientation.z = q.z(); @@ -1623,26 +1570,33 @@ class mapOptimization : public ParamServer void publishOdometry() { // Publish odometry for ROS (global) - nav_msgs::Odometry laserOdometryROS; + nav_msgs::msg::Odometry laserOdometryROS; laserOdometryROS.header.stamp = timeLaserInfoStamp; laserOdometryROS.header.frame_id = odometryFrame; laserOdometryROS.child_frame_id = "odom_mapping"; laserOdometryROS.pose.pose.position.x = transformTobeMapped[3]; laserOdometryROS.pose.pose.position.y = transformTobeMapped[4]; laserOdometryROS.pose.pose.position.z = transformTobeMapped[5]; - laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); - pubLaserOdometryGlobal.publish(laserOdometryROS); - + tf2::Quaternion quat_tf; + quat_tf.setRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); + geometry_msgs::msg::Quaternion quat_msg; + tf2::convert(quat_tf, quat_msg); + laserOdometryROS.pose.pose.orientation = quat_msg; + pubLaserOdometryGlobal->publish(laserOdometryROS); + // Publish TF - static tf::TransformBroadcaster br; - tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]), - tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5])); - tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link"); - br.sendTransform(trans_odom_to_lidar); + quat_tf.setRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); + tf2::Transform t_odom_to_lidar = tf2::Transform(quat_tf, tf2::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5])); + tf2::TimePoint time_point = tf2_ros::fromRclcpp(timeLaserInfoStamp); + tf2::Stamped temp_odom_to_lidar(t_odom_to_lidar, time_point, odometryFrame); + geometry_msgs::msg::TransformStamped trans_odom_to_lidar; + tf2::convert(temp_odom_to_lidar, trans_odom_to_lidar); + trans_odom_to_lidar.child_frame_id = lidarFrame; + br->sendTransform(trans_odom_to_lidar); // Publish odometry for ROS (incremental) static bool lastIncreOdomPubFlag = false; - static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg + static nav_msgs::msg::Odometry laserOdomIncremental; // incremental odometry msg static Eigen::Affine3f increOdomAffine; // incremental odometry in affine if (lastIncreOdomPubFlag == false) { @@ -1654,25 +1608,25 @@ class mapOptimization : public ParamServer increOdomAffine = increOdomAffine * affineIncre; float x, y, z, roll, pitch, yaw; pcl::getTranslationAndEulerAngles (increOdomAffine, x, y, z, roll, pitch, yaw); - if (cloudInfo.imuAvailable == true) + if (cloudInfo.imu_available == true) { - if (std::abs(cloudInfo.imuPitchInit) < 1.4) + if (std::abs(cloudInfo.imu_pitch_init) < 1.4) { double imuWeight = 0.1; - tf::Quaternion imuQuaternion; - tf::Quaternion transformQuaternion; + tf2::Quaternion imuQuaternion; + tf2::Quaternion transformQuaternion; double rollMid, pitchMid, yawMid; // slerp roll transformQuaternion.setRPY(roll, 0, 0); - imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0); - tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); + imuQuaternion.setRPY(cloudInfo.imu_roll_init, 0, 0); + tf2::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); roll = rollMid; // slerp pitch transformQuaternion.setRPY(0, pitch, 0); - imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0); - tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); + imuQuaternion.setRPY(0, cloudInfo.imu_pitch_init, 0); + tf2::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); pitch = pitchMid; } } @@ -1682,13 +1636,17 @@ class mapOptimization : public ParamServer laserOdomIncremental.pose.pose.position.x = x; laserOdomIncremental.pose.pose.position.y = y; laserOdomIncremental.pose.pose.position.z = z; - laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); + tf2::Quaternion quat_tf; + quat_tf.setRPY(roll, pitch, yaw); + geometry_msgs::msg::Quaternion quat_msg; + tf2::convert(quat_tf, quat_msg); + laserOdomIncremental.pose.pose.orientation = quat_msg; if (isDegenerate) laserOdomIncremental.pose.covariance[0] = 1; else laserOdomIncremental.pose.covariance[0] = 0; } - pubLaserOdometryIncremental.publish(laserOdomIncremental); + pubLaserOdometryIncremental->publish(laserOdomIncremental); } void publishFrames() @@ -1696,50 +1654,57 @@ class mapOptimization : public ParamServer if (cloudKeyPoses3D->points.empty()) return; // publish key poses - publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame); + publishCloud(pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame); // Publish surrounding key frames - publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame); + publishCloud(pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame); // publish registered key frame - if (pubRecentKeyFrame.getNumSubscribers() != 0) + if (pubRecentKeyFrame->get_subscription_count() != 0) { pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped); *cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D); *cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D); - publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame); + publishCloud(pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame); } // publish registered high-res raw cloud - if (pubCloudRegisteredRaw.getNumSubscribers() != 0) + if (pubCloudRegisteredRaw->get_subscription_count() != 0) { pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut); PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped); *cloudOut = *transformPointCloud(cloudOut, &thisPose6D); - publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame); + publishCloud(pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame); } // publish path - if (pubPath.getNumSubscribers() != 0) + if (pubPath->get_subscription_count() != 0) { globalPath.header.stamp = timeLaserInfoStamp; globalPath.header.frame_id = odometryFrame; - pubPath.publish(globalPath); + pubPath->publish(globalPath); } } }; int main(int argc, char** argv) -{ - ros::init(argc, argv, "lio_sam"); +{ + rclcpp::init(argc, argv); - mapOptimization MO; + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + rclcpp::executors::SingleThreadedExecutor exec; - ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m"); - - std::thread loopthread(&mapOptimization::loopClosureThread, &MO); - std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO); + auto MO = std::make_shared(options); + exec.add_node(MO); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> Map Optimization Started.\033[0m"); + + std::thread loopthread(&mapOptimization::loopClosureThread, MO); + std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, MO); + + exec.spin(); - ros::spin(); + rclcpp::shutdown(); loopthread.join(); visualizeMapThread.join(); diff --git a/srv/save_map.srv b/srv/save_map.srv deleted file mode 100644 index 6837bc69..00000000 --- a/srv/save_map.srv +++ /dev/null @@ -1,4 +0,0 @@ -float32 resolution -string destination ---- -bool success From 3254ace3b76aa8bb8280f74f53e88efc85e0fb93 Mon Sep 17 00:00:00 2001 From: Fetullah Atas Date: Wed, 18 Aug 2021 13:18:33 +0200 Subject: [PATCH 02/15] give name to nodes --- config/params.yaml | 2 +- launch/run.launch.py | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/config/params.yaml b/config/params.yaml index 567c6215..589ef80f 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -1,4 +1,4 @@ -/lio_sam*: +/**: ros__parameters: # Topics diff --git a/launch/run.launch.py b/launch/run.launch.py index 39c79514..cfde01d2 100644 --- a/launch/run.launch.py +++ b/launch/run.launch.py @@ -42,24 +42,28 @@ def generate_launch_description(): Node( package='lio_sam', executable='lio_sam_imuPreintegration', + name='lio_sam_imuPreintegration', parameters=[parameter_file], output='screen' ), Node( package='lio_sam', executable='lio_sam_imageProjection', + name='lio_sam_imageProjection', parameters=[parameter_file], output='screen' ), Node( package='lio_sam', executable='lio_sam_featureExtraction', + name='lio_sam_featureExtraction', parameters=[parameter_file], output='screen' ), Node( package='lio_sam', executable='lio_sam_mapOptimization', + name='lio_sam_mapOptimization', parameters=[parameter_file], output='screen' ), From 404ab013520e5e8c36303562c5dfb54c867936bb Mon Sep 17 00:00:00 2001 From: Christoph Gruber Date: Thu, 16 Sep 2021 08:26:32 +0200 Subject: [PATCH 03/15] fix installation description in README.md --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 2bc31da6..7db3f72b 100644 --- a/README.md +++ b/README.md @@ -67,7 +67,7 @@ This branch was tested with an Ouster lidar and a Xsens IMU using the following In these tests, the IMU was mounted on the bottom of the lidar such that their x-axes pointed in the same direction. The parameters `extrinsicRot` and `extrinsicRPY` in `params.yaml` correspond to this constellation. -## Dependency +## Dependencies - [ROS2](https://docs.ros.org/en/foxy/Installation.html) (tested with Foxy on Ubuntu 20.04) ``` @@ -90,6 +90,7 @@ Use the following commands to download and compile the package. ``` cd ~/ros2_ws/src git clone https://github.com/TixiaoShan/LIO-SAM.git + cd lio-sam git checkout ros2 cd .. colcon build From 92a25ab8d31819b08dba5e938ae5481b758997dc Mon Sep 17 00:00:00 2001 From: Christoph Gruber Date: Thu, 16 Sep 2021 08:53:58 +0200 Subject: [PATCH 04/15] fix error with frame names introduced in the ROS2 port --- src/mapOptmization.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp index 7c58eb8a..69d7b315 100644 --- a/src/mapOptmization.cpp +++ b/src/mapOptmization.cpp @@ -1591,7 +1591,7 @@ class mapOptimization : public ParamServer tf2::Stamped temp_odom_to_lidar(t_odom_to_lidar, time_point, odometryFrame); geometry_msgs::msg::TransformStamped trans_odom_to_lidar; tf2::convert(temp_odom_to_lidar, trans_odom_to_lidar); - trans_odom_to_lidar.child_frame_id = lidarFrame; + trans_odom_to_lidar.child_frame_id = "lidar_link"; br->sendTransform(trans_odom_to_lidar); // Publish odometry for ROS (incremental) From 74ffb6c62c3414aea7d1c2c1f820d82077c73af1 Mon Sep 17 00:00:00 2001 From: Christoph Gruber Date: Thu, 16 Sep 2021 09:09:19 +0200 Subject: [PATCH 05/15] use rclcpp callback groups to make multithreaded spinning work --- src/imageProjection.cpp | 26 +++++++++++++++++++++++--- src/imuPreintegration.cpp | 38 ++++++++++++++++++++++++++++++++++---- 2 files changed, 57 insertions(+), 7 deletions(-) diff --git a/src/imageProjection.cpp b/src/imageProjection.cpp index 2b3b452d..980a6a52 100644 --- a/src/imageProjection.cpp +++ b/src/imageProjection.cpp @@ -43,15 +43,18 @@ class ImageProjection : public ParamServer std::mutex odoLock; rclcpp::Subscription::SharedPtr subLaserCloud; + rclcpp::CallbackGroup::SharedPtr callbackGroupLidar; rclcpp::Publisher::SharedPtr pubLaserCloud; rclcpp::Publisher::SharedPtr pubExtractedCloud; rclcpp::Publisher::SharedPtr pubLaserCloudInfo; rclcpp::Subscription::SharedPtr subImu; + rclcpp::CallbackGroup::SharedPtr callbackGroupImu; std::deque imuQueue; rclcpp::Subscription::SharedPtr subOdom; + rclcpp::CallbackGroup::SharedPtr callbackGroupOdom; std::deque odomQueue; std::deque cloudQueue; @@ -89,15 +92,32 @@ class ImageProjection : public ParamServer ImageProjection(const rclcpp::NodeOptions & options) : ParamServer("lio_sam_imageProjection", options), deskewFlag(0) { + callbackGroupLidar = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + callbackGroupImu = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + callbackGroupOdom = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + auto lidarOpt = rclcpp::SubscriptionOptions(); + lidarOpt.callback_group = callbackGroupLidar; + auto imuOpt = rclcpp::SubscriptionOptions(); + imuOpt.callback_group = callbackGroupImu; + auto odomOpt = rclcpp::SubscriptionOptions(); + odomOpt.callback_group = callbackGroupOdom; + subImu = create_subscription( imuTopic, qos_imu, - std::bind(&ImageProjection::imuHandler, this, std::placeholders::_1)); + std::bind(&ImageProjection::imuHandler, this, std::placeholders::_1), + imuOpt); subOdom = create_subscription( odomTopic + "_incremental", qos_imu, - std::bind(&ImageProjection::odometryHandler, this, std::placeholders::_1)); + std::bind(&ImageProjection::odometryHandler, this, std::placeholders::_1), + odomOpt); subLaserCloud = create_subscription( pointCloudTopic, qos_lidar, - std::bind(&ImageProjection::cloudHandler, this, std::placeholders::_1)); + std::bind(&ImageProjection::cloudHandler, this, std::placeholders::_1), + lidarOpt); pubExtractedCloud = create_publisher( "lio_sam/deskew/cloud_deskewed", 1); diff --git a/src/imuPreintegration.cpp b/src/imuPreintegration.cpp index 20e66be4..4d0cbeb7 100644 --- a/src/imuPreintegration.cpp +++ b/src/imuPreintegration.cpp @@ -28,6 +28,9 @@ class TransformFusion : public ParamServer rclcpp::Subscription::SharedPtr subImuOdometry; rclcpp::Subscription::SharedPtr subLaserOdometry; + rclcpp::CallbackGroup::SharedPtr callbackGroupImuOdometry; + rclcpp::CallbackGroup::SharedPtr callbackGroupLaserOdometry; + rclcpp::Publisher::SharedPtr pubImuOdometry; rclcpp::Publisher::SharedPtr pubImuPath; @@ -48,12 +51,24 @@ class TransformFusion : public ParamServer tfBuffer = std::make_shared(get_clock()); tfListener = std::make_shared(*tfBuffer); + callbackGroupImuOdometry = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + callbackGroupLaserOdometry = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + auto imuOdomOpt = rclcpp::SubscriptionOptions(); + imuOdomOpt.callback_group = callbackGroupImuOdometry; + auto laserOdomOpt = rclcpp::SubscriptionOptions(); + laserOdomOpt.callback_group = callbackGroupLaserOdometry; + subLaserOdometry = create_subscription( "lio_sam/mapping/odometry", qos, - std::bind(&TransformFusion::lidarOdometryHandler, this, std::placeholders::_1)); + std::bind(&TransformFusion::lidarOdometryHandler, this, std::placeholders::_1), + laserOdomOpt); subImuOdometry = create_subscription( odomTopic+"_incremental", qos_imu, - std::bind(&TransformFusion::imuOdometryHandler, this, std::placeholders::_1)); + std::bind(&TransformFusion::imuOdometryHandler, this, std::placeholders::_1), + imuOdomOpt); pubImuOdometry = create_publisher(odomTopic, qos_imu); pubImuPath = create_publisher("lio_sam/imu/path", qos); @@ -164,6 +179,9 @@ class IMUPreintegration : public ParamServer rclcpp::Subscription::SharedPtr subOdometry; rclcpp::Publisher::SharedPtr pubImuOdometry; + rclcpp::CallbackGroup::SharedPtr callbackGroupImu; + rclcpp::CallbackGroup::SharedPtr callbackGroupOdom; + bool systemInitialized = false; gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; @@ -206,12 +224,24 @@ class IMUPreintegration : public ParamServer IMUPreintegration(const rclcpp::NodeOptions & options) : ParamServer("lio_sam_imu_preintegration", options) { + callbackGroupImu = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + callbackGroupOdom = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + auto imuOpt = rclcpp::SubscriptionOptions(); + imuOpt.callback_group = callbackGroupImu; + auto odomOpt = rclcpp::SubscriptionOptions(); + odomOpt.callback_group = callbackGroupOdom; + subImu = create_subscription( imuTopic, qos_imu, - std::bind(&IMUPreintegration::imuHandler, this, std::placeholders::_1)); + std::bind(&IMUPreintegration::imuHandler, this, std::placeholders::_1) + imuOpt); subOdometry = create_subscription( "lio_sam/mapping/odometry_incremental", qos, - std::bind(&IMUPreintegration::odometryHandler, this, std::placeholders::_1)); + std::bind(&IMUPreintegration::odometryHandler, this, std::placeholders::_1), + odomOpt); pubImuOdometry = create_publisher(odomTopic+"_incremental", qos_imu); From 9d9dfececca7a658402f893e36f6341ae6c884cc Mon Sep 17 00:00:00 2001 From: Frank Bu Date: Tue, 25 Jan 2022 16:13:32 -0500 Subject: [PATCH 06/15] Update imuPreintegration.cpp --- src/imuPreintegration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/imuPreintegration.cpp b/src/imuPreintegration.cpp index 4d0cbeb7..8420c045 100644 --- a/src/imuPreintegration.cpp +++ b/src/imuPreintegration.cpp @@ -236,7 +236,7 @@ class IMUPreintegration : public ParamServer subImu = create_subscription( imuTopic, qos_imu, - std::bind(&IMUPreintegration::imuHandler, this, std::placeholders::_1) + std::bind(&IMUPreintegration::imuHandler, this, std::placeholders::_1), imuOpt); subOdometry = create_subscription( "lio_sam/mapping/odometry_incremental", qos, From 09583e2665fe95c0929ef86d679b7f4748ea1262 Mon Sep 17 00:00:00 2001 From: Frank Bu Date: Tue, 8 Mar 2022 13:04:56 -0500 Subject: [PATCH 07/15] Ros2 add save map service (#317) * add save map service * Update mapOptmization.cpp * Update mapOptmization.cpp * Update README.md * Update mapOptmization.cpp * Update mapOptmization.cpp * Update mapOptmization.cpp * Update mapOptmization.cpp * Update mapOptmization.cpp * Update SaveMap.srv --- CMakeLists.txt | 2 +- README.md | 8 +++++- src/mapOptmization.cpp | 64 ++++++++++++++++++++++++++++++++++++++++-- srv/SaveMap.srv | 4 +++ 4 files changed, 74 insertions(+), 4 deletions(-) create mode 100644 srv/SaveMap.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 04be5d63..6516469d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,7 +30,7 @@ include_directories( include/lio_sam ) -rosidl_generate_interfaces(${PROJECT_NAME} "msg/CloudInfo.msg" DEPENDENCIES std_msgs sensor_msgs) +rosidl_generate_interfaces(${PROJECT_NAME} "msg/CloudInfo.msg" "srv/SaveMap.srv" DEPENDENCIES std_msgs sensor_msgs) add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp) ament_target_dependencies(${PROJECT_NAME}_featureExtraction rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) diff --git a/README.md b/README.md index 7db3f72b..4578ac1d 100644 --- a/README.md +++ b/README.md @@ -56,7 +56,6 @@ We design a system that maintains two graphs and runs up to 10x faster than real ## Notes on ROS2 branch There are some features of the original ROS1 version that are currently missing in this ROS2 version, namely: -- The service for saving maps - Testing with Velodyne lidars and Microstrain IMUs - A launch file for the navsat module/GPS factor - The rviz2 configuration misses many elements @@ -138,6 +137,13 @@ ros2 launch lio_sam run.launch.py ros2 bag play your-bag.bag ``` +## Save map +``` +ros2 service call /lio_sam/save_map lio_sam/srv/SaveMap +``` +``` +ros2 service call /lio_sam/save_map lio_sam/srv/SaveMap "{resolution: 0.2, destination: /Downloads/service_LOAM}" +``` ## Other notes - **Loop closure:** The loop function here gives an example of proof of concept. It is directly adapted from LeGO-LOAM loop closure. For more advanced loop closure implementation, please refer to [ScanContext](https://github.com/irapkaist/SC-LeGO-LOAM). Set the "loopClosureEnableFlag" in "params.yaml" to "true" to test the loop closure function. In Rviz, uncheck "Map (cloud)" and check "Map (global)". This is because the visualized map - "Map (cloud)" - is simply a stack of point clouds in Rviz. Their postion will not be updated after pose correction. The loop closure function here is simply adapted from LeGO-LOAM, which is an ICP-based method. Because ICP runs pretty slow, it is suggested that the playback speed is set to be "-r 1". You can try the Garden dataset for testing. diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp index 69d7b315..deb6147c 100644 --- a/src/mapOptmization.cpp +++ b/src/mapOptmization.cpp @@ -1,6 +1,6 @@ #include "utility.hpp" #include "lio_sam/msg/cloud_info.hpp" - +#include "lio_sam/srv/save_map.hpp" #include #include #include @@ -72,6 +72,7 @@ class mapOptimization : public ParamServer rclcpp::Publisher::SharedPtr pubCloudRegisteredRaw; rclcpp::Publisher::SharedPtr pubLoopConstraintEdge; + rclcpp::Service::SharedPtr srvSaveMap; rclcpp::Subscription::SharedPtr subCloud; rclcpp::Subscription::SharedPtr subGPS; rclcpp::Subscription::SharedPtr subLoop; @@ -175,6 +176,66 @@ class mapOptimization : public ParamServer "lio_loop/loop_closure_detection", qos, std::bind(&mapOptimization::loopInfoHandler, this, std::placeholders::_1)); + auto saveMapService = [this](const std::shared_ptr request_header, const std::shared_ptr req, std::shared_ptr res) -> void { + (void)request_header; + string saveMapDirectory; + cout << "****************************************************" << endl; + cout << "Saving map to pcd files ..." << endl; + if(req->destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory; + else saveMapDirectory = std::getenv("HOME") + req->destination; + cout << "Save destination: " << saveMapDirectory << endl; + // create directory and remove old files; + int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str()); + unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str()); + // save key frame transformations + pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D); + pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D); + // extract global point cloud map + pcl::PointCloud::Ptr globalCornerCloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr globalCornerCloudDS(new pcl::PointCloud()); + pcl::PointCloud::Ptr globalSurfCloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr globalSurfCloudDS(new pcl::PointCloud()); + pcl::PointCloud::Ptr globalMapCloud(new pcl::PointCloud()); + for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) + { + *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); + *globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); + cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ..."; + } + if(req->resolution != 0) + { + cout << "\n\nSave resolution: " << req->resolution << endl; + // down-sample and save corner cloud + downSizeFilterCorner.setInputCloud(globalCornerCloud); + downSizeFilterCorner.setLeafSize(req->resolution, req->resolution, req->resolution); + downSizeFilterCorner.filter(*globalCornerCloudDS); + pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS); + // down-sample and save surf cloud + downSizeFilterSurf.setInputCloud(globalSurfCloud); + downSizeFilterSurf.setLeafSize(req->resolution, req->resolution, req->resolution); + downSizeFilterSurf.filter(*globalSurfCloudDS); + pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS); + } + else + { + // save corner cloud + pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud); + // save surf cloud + pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud); + } + // save global point cloud map + *globalMapCloud += *globalCornerCloud; + *globalMapCloud += *globalSurfCloud; + int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud); + res->success = ret == 0; + downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize); + downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize); + cout << "****************************************************" << endl; + cout << "Saving map to pcd files completed\n" << endl; + return; + }; + + srvSaveMap = create_service("lio_sam/save_map", saveMapService); pubHistoryKeyFrames = create_publisher("lio_sam/mapping/icp_loop_closure_history_cloud", 1); pubIcpKeyFrames = create_publisher("lio_sam/mapping/icp_loop_closure_history_cloud", 1); pubLoopConstraintEdge = create_publisher("/lio_sam/mapping/loop_closure_constraints", 1); @@ -1374,7 +1435,6 @@ class mapOptimization : public ParamServer float noise_z = thisGPS.pose.covariance[14]; if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold) continue; - float gps_x = thisGPS.pose.pose.position.x; float gps_y = thisGPS.pose.pose.position.y; float gps_z = thisGPS.pose.pose.position.z; diff --git a/srv/SaveMap.srv b/srv/SaveMap.srv new file mode 100644 index 00000000..6837bc69 --- /dev/null +++ b/srv/SaveMap.srv @@ -0,0 +1,4 @@ +float32 resolution +string destination +--- +bool success From d6af2908a73899beb3258f965d7cb223f8dfee6f Mon Sep 17 00:00:00 2001 From: Christoph Date: Wed, 18 Jan 2023 10:11:13 +0100 Subject: [PATCH 08/15] support ros2 humble; merge in some changes from master --- CMakeLists.txt | 9 +-- README.md | 42 ++++++------- config/params.yaml | 18 +++--- include/lio_sam/utility.hpp | 116 ++++++++++++++++++++++-------------- package.xml | 4 ++ src/imageProjection.cpp | 23 +++++-- src/mapOptmization.cpp | 16 +++-- 7 files changed, 138 insertions(+), 90 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6516469d..9b7466c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(pcl_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_sensor_msgs REQUIRED) @@ -33,20 +34,20 @@ include_directories( rosidl_generate_interfaces(${PROJECT_NAME} "msg/CloudInfo.msg" "srv/SaveMap.srv" DEPENDENCIES std_msgs sensor_msgs) add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp) -ament_target_dependencies(${PROJECT_NAME}_featureExtraction rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) +ament_target_dependencies(${PROJECT_NAME}_featureExtraction rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) rosidl_target_interfaces(${PROJECT_NAME}_featureExtraction ${PROJECT_NAME} "rosidl_typesupport_cpp") add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp) -ament_target_dependencies(${PROJECT_NAME}_imageProjection rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) +ament_target_dependencies(${PROJECT_NAME}_imageProjection rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs pcl_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) rosidl_target_interfaces(${PROJECT_NAME}_imageProjection ${PROJECT_NAME} "rosidl_typesupport_cpp") add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp) -ament_target_dependencies(${PROJECT_NAME}_imuPreintegration rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL GTSAM Eigen) +ament_target_dependencies(${PROJECT_NAME}_imuPreintegration rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL GTSAM Eigen) target_link_libraries(${PROJECT_NAME}_imuPreintegration gtsam) rosidl_target_interfaces(${PROJECT_NAME}_imuPreintegration ${PROJECT_NAME} "rosidl_typesupport_cpp") add_executable(${PROJECT_NAME}_mapOptimization src/mapOptmization.cpp) -ament_target_dependencies(${PROJECT_NAME}_mapOptimization rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL GTSAM) +ament_target_dependencies(${PROJECT_NAME}_mapOptimization rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL GTSAM) target_link_libraries(${PROJECT_NAME}_mapOptimization gtsam) rosidl_target_interfaces(${PROJECT_NAME}_mapOptimization ${PROJECT_NAME} "rosidl_typesupport_cpp") diff --git a/README.md b/README.md index 4578ac1d..612d31c4 100644 --- a/README.md +++ b/README.md @@ -49,36 +49,38 @@ drawing

-We design a system that maintains two graphs and runs up to 10x faster than real-time. - - The factor graph in "mapOptimization.cpp" optimizes lidar odometry factor and GPS factor. This factor graph is maintained consistently throughout the whole test. +We design a system that maintains two graphs and runs up to 10x faster than real-time. + - The factor graph in "mapOptimization.cpp" optimizes lidar odometry factor and GPS factor. This factor graph is maintained consistently throughout the whole test. - The factor graph in "imuPreintegration.cpp" optimizes IMU and lidar odometry factor and estimates IMU bias. This factor graph is reset periodically and guarantees real-time odometry estimation at IMU frequency. ## Notes on ROS2 branch There are some features of the original ROS1 version that are currently missing in this ROS2 version, namely: -- Testing with Velodyne lidars and Microstrain IMUs +- Testing with Velodyne & Livox lidars and Microstrain IMUs - A launch file for the navsat module/GPS factor - The rviz2 configuration misses many elements -This branch was tested with an Ouster lidar and a Xsens IMU using the following ROS2 drivers: +This branch was tested with Ouster lidars, Xsens IMUs and SBG-Systems IMUs using the following ROS2 drivers: - [ros2_ouster_drivers](https://github.com/ros-drivers/ros2_ouster_drivers) - [bluespace_ai_xsens_ros_mti_driver](https://github.com/bluespace-ai/bluespace_ai_xsens_ros_mti_driver) +- [sbg_ros2_driver](https://github.com/SBG-Systems/sbg_ros2_driver) In these tests, the IMU was mounted on the bottom of the lidar such that their x-axes pointed in the same direction. The parameters `extrinsicRot` and `extrinsicRPY` in `params.yaml` correspond to this constellation. ## Dependencies -- [ROS2](https://docs.ros.org/en/foxy/Installation.html) (tested with Foxy on Ubuntu 20.04) +Tested with ROS2 versions foxy and galactic on Ubuntu 20.04 and humble on Ubuntu 22.04 +- [ROS2](https://docs.ros.org/en/humble/Installation.html) ``` - sudo apt install ros-foxy-perception-pcl \ - ros-foxy-pcl-msgs \ - ros-foxy-vision-opencv \ - ros-foxy-xacro + sudo apt install ros--perception-pcl \ + ros--pcl-msgs \ + ros--vision-opencv \ + ros--xacro ``` -- [gtsam](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library) +- [gtsam](https://gtsam.org/get_started) (Georgia Tech Smoothing and Mapping library) ``` # Add GTSAM-PPA - sudo add-apt-repository ppa:borglab/gtsam-release-4.0 + sudo add-apt-repository ppa:borglab/gtsam-develop sudo apt install libgtsam-dev libgtsam-unstable-dev ``` @@ -99,15 +101,15 @@ Use the following commands to download and compile the package. The user needs to prepare the point cloud data in the correct format for cloud deskewing, which is mainly done in "imageProjection.cpp". The two requirements are: - **Provide point time stamp**. LIO-SAM uses IMU data to perform point cloud deskew. Thus, the relative point time in a scan needs to be known. The up-to-date Velodyne ROS driver should output this information directly. Here, we assume the point time channel is called "time." The definition of the point type is located at the top of the "imageProjection.cpp." "deskewPoint()" function utilizes this relative time to obtain the transformation of this point relative to the beginning of the scan. When the lidar rotates at 10Hz, the timestamp of a point should vary between 0 and 0.1 seconds. If you are using other lidar sensors, you may need to change the name of this time channel and make sure that it is the relative time in a scan. - - **Provide point ring number**. LIO-SAM uses this information to organize the point correctly in a matrix. The ring number indicates which channel of the sensor that this point belongs to. The definition of the point type is located at the top of "imageProjection.cpp." The up-to-date Velodyne ROS driver should output this information directly. Again, if you are using other lidar sensors, you may need to rename this information. Note that only mechanical lidars are supported by the package currently. + - **Provide point ring number**. LIO-SAM uses this information to organize the point correctly in a matrix. The ring number indicates which channel of the sensor that this point belongs to. The definition of the point type is located at the top of "imageProjection.cpp." The up-to-date Velodyne ROS driver should output this information directly. Again, if you are using other lidar sensors, you may need to rename this information. Note that only mechanical lidars are supported by the package currently. ## Prepare IMU data - **IMU requirement**. Like the original LOAM implementation, LIO-SAM only works with a 9-axis IMU, which gives roll, pitch, and yaw estimation. The roll and pitch estimation is mainly used to initialize the system at the correct attitude. The yaw estimation initializes the system at the right heading when using GPS data. Theoretically, an initialization procedure like VINS-Mono will enable LIO-SAM to work with a 6-axis IMU. The performance of the system largely depends on the quality of the IMU measurements. The higher the IMU data rate, the better the system accuracy. We use Microstrain 3DM-GX5-25, which outputs data at 500Hz. We recommend using an IMU that gives at least a 200Hz output rate. Note that the internal IMU of Ouster lidar is an 6-axis IMU. - - **IMU alignment**. LIO-SAM transforms IMU raw data from the IMU frame to the Lidar frame, which follows the ROS REP-105 convention (x - forward, y - left, z - upward). To make the system function properly, the correct extrinsic transformation needs to be provided in "params.yaml" file. **The reason why there are two extrinsics is that my IMU (Microstrain 3DM-GX5-25) acceleration and attitude have different cooridinates. Depend on your IMU manufacturer, the two extrinsics for your IMU may or may not be the same**. Using our setup as an example: - - we need to set the readings of x-z acceleration and gyro negative to transform the IMU data in the lidar frame, which is indicated by "extrinsicRot" in "params.yaml." - - The transformation of attitude readings is slightly different. We rotate the attitude measurements by -90 degrees around "lidar-z" axis and get the corresponding roll, pitch, and yaw readings in the lidar frame. This transformation is indicated by "extrinsicRPY" in "params.yaml." + - **IMU alignment**. LIO-SAM transforms IMU raw data from the IMU frame to the Lidar frame, which follows the ROS REP-105 convention (x - forward, y - left, z - upward). To make the system function properly, the correct extrinsic transformation needs to be provided in "params.yaml" file. **The reason why there are two extrinsics is that my IMU (Microstrain 3DM-GX5-25) acceleration and attitude have different coordinates. Depending on your IMU manufacturer, the two extrinsics for your IMU may or may not be the same**. + - "extrinsicRot" in "params.yaml" is a rotation matrix that transforms IMU gyro and acceleometer measurements to lidar frame. + - "extrinsicRPY" in "params.yaml" is a rotation matrix that transforms IMU orientation to lidar frame. - **IMU debug**. It's strongly recommended that the user uncomment the debug lines in "imuHandler()" of "imageProjection.cpp" and test the output of the transformed IMU data. The user can rotate the sensor suite to check whether the readings correspond to the sensor's movement. A YouTube video that shows the corrected IMU data can be found [here (link to YouTube)](https://youtu.be/BOUK8LYQhHs). @@ -121,9 +123,9 @@ The user needs to prepare the point cloud data in the correct format for cloud d ## Sample datasets -For data protection reasons, no data set can currently be made available for ROS2. +For privacy reasons, no data set can currently be made available for ROS2. -README.md of the master branch contains some links to ROS1 rosbags. Using [ros1_bridge](https://github.com/ros2/ros1_bridge) with these rosbags and this ROS2 branch of LIO-SAM did however not succeeed for timing reasons. +README.md of the master branch contains some links to ROS1 rosbags. It is possible to use [ros1_bridge](https://github.com/ros2/ros1_bridge) with these rosbags, but verify timing behavior (message frequency in ROS2) first. Mind [DDS tuning](https://docs.ros.org/en/humble/How-To-Guides/DDS-tuning.html). ## Run the package @@ -160,7 +162,7 @@ ros2 service call /lio_sam/save_map lio_sam/srv/SaveMap "{resolution: 0.2, desti

- **KITTI:** Since LIO-SAM needs a high-frequency IMU for function properly, we need to use KITTI raw data for testing. One problem remains unsolved is that the intrinsics of the IMU are unknown, which has a big impact on the accuracy of LIO-SAM. Download the provided sample data and make the following changes in "params.yaml": - - extrinsicTrans: [-8.086759e-01, 3.195559e-01, -7.997231e-01] + - extrinsicTrans: [-8.086759e-01, 3.195559e-01, -7.997231e-01] - extrinsicRot: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01] - extrinsicRPY: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01] - N_SCAN: 64 @@ -197,9 +199,9 @@ ros2 service call /lio_sam/save_map lio_sam/srv/SaveMap "{resolution: 0.2, desti - **gps odometry unavailable**: it is generally caused due to unavailable transform between message frame_ids and robot frame_id (for example: transform should be available from "imu_frame_id" and "gps_frame_id" to "base_link" frame. Please read the Robot Localization documentation found [here](http://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html). -## Paper +## Paper -Thank you for citing [LIO-SAM (IROS-2020)](./config/doc/paper.pdf) if you use any of this code. +Thank you for citing [LIO-SAM (IROS-2020)](./config/doc/paper.pdf) if you use any of this code. ``` @inproceedings{liosam2020shan, title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping}, diff --git a/config/params.yaml b/config/params.yaml index 589ef80f..dd334c98 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -2,7 +2,7 @@ ros__parameters: # Topics - pointCloudTopic: "/points" # Point cloud data + pointCloudTopic: "/points" # Point cloud data imuTopic: "/imu/data" # IMU data odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file @@ -24,9 +24,9 @@ savePCDDirectory: "/Downloads/LOAM/" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation # Sensor Settings - sensor: ouster # lidar sensor type, either 'velodyne' or 'ouster' - N_SCAN: 64 # number of lidar channel (i.e., 16, 32, 64, 128) - Horizon_SCAN: 512 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) + sensor: ouster # lidar sensor type, either 'velodyne', 'ouster' or 'livox' + N_SCAN: 64 # number of lidar channels (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6) + Horizon_SCAN: 512 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000) downsampleRate: 1 # default: 1. Downsample your data if too many # points. i.e., 16 = 64 / 4, 16 = 16 / 1 lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used @@ -42,12 +42,12 @@ imuRPYWeight: 0.01 extrinsicTrans: [ 0.0, 0.0, 0.0 ] - extrinsicRot: [ 1.0, 0.0, 0.0, - 0.0, -1.0, 0.0, + extrinsicRot: [-1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, -1.0 ] - extrinsicRPY: [ 1.0, 0.0, 0.0, - 0.0, -1.0, 0.0, - 0.0, 0.0, -1.0 ] + extrinsicRPY: [ 0.0, 1.0, 0.0, + -1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] # LOAM feature threshold edgeThreshold: 1.0 diff --git a/include/lio_sam/utility.hpp b/include/lio_sam/utility.hpp index 6c9e15d6..60e7b886 100644 --- a/include/lio_sam/utility.hpp +++ b/include/lio_sam/utility.hpp @@ -18,26 +18,28 @@ #include +#include // pcl include kdtree_flann throws error if PCL_NO_PRECOMPILE + // is defined before +#define PCL_NO_PRECOMPILE #include #include #include #include -#include #include #include #include #include #include #include -#include +#include #include #include #include #include -#include -#include - +#include +#include + #include #include #include @@ -60,7 +62,7 @@ using namespace std; typedef pcl::PointXYZI PointType; -enum class SensorType { VELODYNE, OUSTER }; +enum class SensorType { VELODYNE, OUSTER, LIVOX }; class ParamServer : public rclcpp::Node { @@ -123,7 +125,7 @@ class ParamServer : public rclcpp::Node float mappingCornerLeafSize; float mappingSurfLeafSize ; - float z_tollerance; + float z_tollerance; float rotation_tollerance; // CPU Params @@ -131,11 +133,11 @@ class ParamServer : public rclcpp::Node double mappingProcessInterval; // Surrounding map - float surroundingkeyframeAddingDistThreshold; - float surroundingkeyframeAddingAngleThreshold; + float surroundingkeyframeAddingDistThreshold; + float surroundingkeyframeAddingAngleThreshold; float surroundingKeyframeDensity; float surroundingKeyframeSearchRadius; - + // Loop closure bool loopClosureEnableFlag; float loopClosureFrequency; @@ -178,13 +180,35 @@ class ParamServer : public rclcpp::Node get_parameter("gpsCovThreshold", gpsCovThreshold); declare_parameter("poseCovThreshold", 25.0); get_parameter("poseCovThreshold", poseCovThreshold); - + declare_parameter("savePCD", false); get_parameter("savePCD", savePCD); - declare_parameter("savePCDDirectory", "/Downloads/LOAM/"); get_parameter("savePCDDirectory", savePCDDirectory); + std::string sensorStr; + declare_parameter("sensor", "ouster"); + get_parameter("sensor", sensorStr); + if (sensorStr == "velodyne") + { + sensor = SensorType::VELODYNE; + } + else if (sensorStr == "ouster") + { + sensor = SensorType::OUSTER; + } + else if (sensorStr == "livox") + { + sensor = SensorType::LIVOX; + } + else + { + RCLCPP_ERROR_STREAM( + get_logger(), + "Invalid sensor type (must be either 'velodyne' or 'ouster' or 'livox'): " << sensorStr); + rclcpp::shutdown(); + } + declare_parameter("N_SCAN", 64); get_parameter("N_SCAN", N_SCAN); declare_parameter("Horizon_SCAN", 512); @@ -247,7 +271,7 @@ class ParamServer : public rclcpp::Node get_parameter("z_tollerance", z_tollerance); declare_parameter("rotation_tollerance", 1000.0); get_parameter("rotation_tollerance", rotation_tollerance); - + declare_parameter("numberOfCores", 4); get_parameter("numberOfCores", numberOfCores); declare_parameter("mappingProcessInterval", 0.15); @@ -383,59 +407,59 @@ float pointDistance(PointType p1, PointType p2) } rmw_qos_profile_t qos_profile{ - RMW_QOS_POLICY_HISTORY_KEEP_LAST, - 1, - RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false }; auto qos = rclcpp::QoS( rclcpp::QoSInitialization( - qos_profile.history, - qos_profile.depth + qos_profile.history, + qos_profile.depth ), qos_profile); rmw_qos_profile_t qos_profile_imu{ - RMW_QOS_POLICY_HISTORY_KEEP_LAST, - 2000, - RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 2000, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false }; auto qos_imu = rclcpp::QoS( rclcpp::QoSInitialization( - qos_profile_imu.history, - qos_profile_imu.depth + qos_profile_imu.history, + qos_profile_imu.depth ), qos_profile_imu); rmw_qos_profile_t qos_profile_lidar{ - RMW_QOS_POLICY_HISTORY_KEEP_LAST, - 5, - RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 5, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false }; auto qos_lidar = rclcpp::QoS( rclcpp::QoSInitialization( - qos_profile_lidar.history, - qos_profile_lidar.depth + qos_profile_lidar.history, + qos_profile_lidar.depth ), qos_profile_lidar); diff --git a/package.xml b/package.xml index c5b33502..4a8d84ed 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,8 @@ geometry_msgs tf2_geometry_msgs tf2_sensor_msgs + visualization_msgs + pcl_msgs tf2_eigen tf2_ros tf2 @@ -37,6 +39,8 @@ sensor_msgs nav_msgs geometry_msgs + visualization_msgs + pcl_msgs tf2_geometry_msgs tf2_sensor_msgs tf2_eigen diff --git a/src/imageProjection.cpp b/src/imageProjection.cpp index 980a6a52..a35ba5cb 100644 --- a/src/imageProjection.cpp +++ b/src/imageProjection.cpp @@ -87,6 +87,8 @@ class ImageProjection : public ParamServer double timeScanEnd; std_msgs::msg::Header cloudHeader; + vector columnIdnCountVec; + public: ImageProjection(const rclcpp::NodeOptions & options) : @@ -228,7 +230,7 @@ class ImageProjection : public ParamServer // convert cloud currentCloudMsg = std::move(cloudQueue.front()); cloudQueue.pop_front(); - if (sensor == SensorType::VELODYNE) + if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX) { pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn); } @@ -566,12 +568,21 @@ class ImageProjection : public ParamServer if (rowIdn % downsampleRate != 0) continue; - float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; + int columnIdn = -1; + if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER) + { + float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; + static float ang_res_x = 360.0/float(Horizon_SCAN); + columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2; + if (columnIdn >= Horizon_SCAN) + columnIdn -= Horizon_SCAN; + } + else if (sensor == SensorType::LIVOX) + { + columnIdn = columnIdnCountVec[rowIdn]; + columnIdnCountVec[rowIdn] += 1; + } - static float ang_res_x = 360.0/float(Horizon_SCAN); - int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2; - if (columnIdn >= Horizon_SCAN) - columnIdn -= Horizon_SCAN; if (columnIdn < 0 || columnIdn >= Horizon_SCAN) continue; diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp index deb6147c..1fa44512 100644 --- a/src/mapOptmization.cpp +++ b/src/mapOptmization.cpp @@ -90,8 +90,8 @@ class mapOptimization : public ParamServer pcl::PointCloud::Ptr laserCloudCornerLast; // corner feature set from odoOptimization pcl::PointCloud::Ptr laserCloudSurfLast; // surf feature set from odoOptimization - pcl::PointCloud::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization - pcl::PointCloud::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization + pcl::PointCloud::Ptr laserCloudCornerLastDS; // downsampled corner feature set from odoOptimization + pcl::PointCloud::Ptr laserCloudSurfLastDS; // downsampled surf feature set from odoOptimization pcl::PointCloud::Ptr laserCloudOri; pcl::PointCloud::Ptr coeffSel; @@ -1118,8 +1118,8 @@ class mapOptimization : public ParamServer if (planeValid) { float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; - float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x - + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); + float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointOri.x * pointOri.x + + pointOri.y * pointOri.y + pointOri.z * pointOri.z)); coeff.x = s * pa; coeff.y = s * pb; @@ -1359,6 +1359,12 @@ class mapOptimization : public ParamServer if (cloudKeyPoses3D->points.empty()) return true; + if (sensor == SensorType::LIVOX) + { + if (timeLaserInfoCur - cloudKeyPoses6D->back().time > 1.0) + return true; + } + Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back()); Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); @@ -1367,7 +1373,7 @@ class mapOptimization : public ParamServer pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw); if (abs(roll) < surroundingkeyframeAddingAngleThreshold && - abs(pitch) < surroundingkeyframeAddingAngleThreshold && + abs(pitch) < surroundingkeyframeAddingAngleThreshold && abs(yaw) < surroundingkeyframeAddingAngleThreshold && sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold) return false; From 06da1cc5d0a3228a1721382bfb67d0ca45ab3b4a Mon Sep 17 00:00:00 2001 From: Christoph Date: Thu, 19 Jan 2023 08:39:26 +0100 Subject: [PATCH 09/15] fix linker warning related to different tbb versions #398 --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b7466c5..dc922dd1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_sensor_msgs REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_ros REQUIRED) +set(OpenCV_STATIC ON) find_package(OpenCV REQUIRED) find_package(PCL REQUIRED) find_package(GTSAM REQUIRED) From 23d5e5d00e8eaff698b31b7b8510da23de040131 Mon Sep 17 00:00:00 2001 From: Christoph Date: Thu, 2 Feb 2023 09:44:09 +0100 Subject: [PATCH 10/15] #400 --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 612d31c4..391dacfd 100644 --- a/README.md +++ b/README.md @@ -80,7 +80,7 @@ Tested with ROS2 versions foxy and galactic on Ubuntu 20.04 and humble on Ubuntu - [gtsam](https://gtsam.org/get_started) (Georgia Tech Smoothing and Mapping library) ``` # Add GTSAM-PPA - sudo add-apt-repository ppa:borglab/gtsam-develop + sudo add-apt-repository ppa:borglab/gtsam-release-4.1 sudo apt install libgtsam-dev libgtsam-unstable-dev ``` From e48b7a0fbd366c367cc22e4b49785a6a19f4864a Mon Sep 17 00:00:00 2001 From: lukasj-imar <126788643+lukasj-imar@users.noreply.github.com> Date: Wed, 23 Aug 2023 07:03:21 +0200 Subject: [PATCH 11/15] Fixed CMake Warnings for humble and added compiler flag for openMP (#443) * Fixed CMake Warnings for humble and added compiler flag for openMP * Update CMakeLists.txt, added openMP in a clean way --- CMakeLists.txt | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dc922dd1..bd481e9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,7 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) set(CMAKE_BUILD_TYPE Release) endif() + # find dependencies find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -27,6 +28,7 @@ find_package(OpenCV REQUIRED) find_package(PCL REQUIRED) find_package(GTSAM REQUIRED) find_package(Eigen REQUIRED) +find_package(OpenMP REQUIRED) include_directories( include/lio_sam @@ -34,23 +36,29 @@ include_directories( rosidl_generate_interfaces(${PROJECT_NAME} "msg/CloudInfo.msg" "srv/SaveMap.srv" DEPENDENCIES std_msgs sensor_msgs) + + add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp) ament_target_dependencies(${PROJECT_NAME}_featureExtraction rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) -rosidl_target_interfaces(${PROJECT_NAME}_featureExtraction ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${PROJECT_NAME}_featureExtraction "${cpp_typesupport_target}") add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp) ament_target_dependencies(${PROJECT_NAME}_imageProjection rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs pcl_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL) -rosidl_target_interfaces(${PROJECT_NAME}_imageProjection ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${PROJECT_NAME}_imageProjection "${cpp_typesupport_target}") add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp) ament_target_dependencies(${PROJECT_NAME}_imuPreintegration rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL GTSAM Eigen) -target_link_libraries(${PROJECT_NAME}_imuPreintegration gtsam) -rosidl_target_interfaces(${PROJECT_NAME}_imuPreintegration ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${PROJECT_NAME}_imuPreintegration gtsam "${cpp_typesupport_target}") add_executable(${PROJECT_NAME}_mapOptimization src/mapOptmization.cpp) ament_target_dependencies(${PROJECT_NAME}_mapOptimization rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL GTSAM) -target_link_libraries(${PROJECT_NAME}_mapOptimization gtsam) -rosidl_target_interfaces(${PROJECT_NAME}_mapOptimization ${PROJECT_NAME} "rosidl_typesupport_cpp") +if (OpenMP_CXX_FOUND) + target_link_libraries(${PROJECT_NAME}_mapOptimization gtsam "${cpp_typesupport_target}" OpenMP::OpenMP_CXX) +else() + target_link_libraries(${PROJECT_NAME}_mapOptimization gtsam "${cpp_typesupport_target}") +endif() + install( DIRECTORY launch From 4938d3bb4423b76bf5aa22556dd755526b03a253 Mon Sep 17 00:00:00 2001 From: Vineet <52542471+VineetTambe@users.noreply.github.com> Date: Wed, 23 Aug 2023 16:02:16 -0400 Subject: [PATCH 12/15] ROS2 dockerfile and docker compose (#446) * Added ros2 huble dockerfile * Added docker compose yaml file * Updated README with instructions to run the docker container * Updated directory name from colcon_ws to ros2_ws --- Dockerfile | 36 ++++++++++++++++++++++++++++++++++++ README.md | 42 ++++++++++++++++++++++++++++++++++++++++++ docker-compose.yaml | 32 ++++++++++++++++++++++++++++++++ 3 files changed, 110 insertions(+) create mode 100644 Dockerfile create mode 100644 docker-compose.yaml diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 00000000..72aa4d71 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,36 @@ +FROM osrf/ros:humble-desktop-full-jammy + +RUN apt-get update \ + && apt-get install -y curl \ + && curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - \ + && apt-get update \ + && apt install -y python3-colcon-common-extensions \ + && apt-get install -y ros-humble-navigation2 \ + && apt-get install -y ros-humble-robot-localization \ + && apt-get install -y ros-humble-robot-state-publisher \ + && apt install -y ros-humble-perception-pcl \ + && apt install -y ros-humble-pcl-msgs \ + && apt install -y ros-humble-vision-opencv \ + && apt install -y ros-humble-xacro \ + && rm -rf /var/lib/apt/lists/* + +RUN apt-get update \ + && apt install -y software-properties-common \ + && add-apt-repository -y ppa:borglab/gtsam-release-4.1 \ + && apt-get update \ + && apt install -y libgtsam-dev libgtsam-unstable-dev \ + && rm -rf /var/lib/apt/lists/* + +SHELL ["/bin/bash", "-c"] + +RUN mkdir -p ~/ros2_ws/src \ + && cd ~/ros2_ws/src \ + && git clone --branch ros2 https://github.com/TixiaoShan/LIO-SAM.git \ + && cd .. \ + && source /opt/ros/humble/setup.bash \ + && colcon build + +RUN echo "source /opt/ros/humble/setup.bash" >> /root/.bashrc \ + && echo "source /root/ros2_ws/install/setup.bash" >> /root/.bashrc + +WORKDIR /root/ros2_ws diff --git a/README.md b/README.md index 391dacfd..27604afb 100644 --- a/README.md +++ b/README.md @@ -97,6 +97,48 @@ Use the following commands to download and compile the package. colcon build ``` +## Using Docker + +Build image (based on ROS2 Humble): + +``` +docker build -t liosam-humble-jammy . +``` + +Once you have the image, you can start a container by using one of the following methods: + +1. `docker run` + +``` +docker run --init -it -d \ + --name liosam-humble-jammy-container \ + -v /etc/localtime:/etc/localtime:ro \ + -v /etc/timezone:/etc/timezone:ro \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -e DISPLAY=$DISPLAY \ + --runtime=nvidia --gpus all \ + liosam-humble-jammy \ + bash +``` + +2. `docker compose` + +Start a docker compose container: + +``` +docker compose up -d +``` + +Stopping a docker compose container: +``` +docker compose down +``` + +To enter into the running container use: + +``` +docker exec -it liosam-humble-jammy-container bash +``` ## Prepare lidar data The user needs to prepare the point cloud data in the correct format for cloud deskewing, which is mainly done in "imageProjection.cpp". The two requirements are: diff --git a/docker-compose.yaml b/docker-compose.yaml new file mode 100644 index 00000000..dd06afc8 --- /dev/null +++ b/docker-compose.yaml @@ -0,0 +1,32 @@ +version: '3' +services: + ddg-humble-container: + image: liosam-humble-jammy + container_name: liosam-humble-jammy-container + privileged: true + hostname: liosam-humble-jammy-container + restart: always + stdin_open: true + tty: true + network_mode: "host" + dns: + - 1.1.1.1 + - 1.0.0.1 + - 8.8.8.8 + environment: + - "DISPLAY=$DISPLAY" + - "NVIDIA_VISIBLE_DEVICES=all" + - "NVIDIA_DRIVER_CAPABILITIES=all" + - "FASTRTPS_DEFAULT_PROFILES_FILE=/usr/local/share/middleware_profiles/rtps_udp_profile.xml" + volumes: + - "/tmp/.X11-unix:/tmp/.X11-unix" + - "/etc/localtime:/etc/localtime:ro" + - "/etc/timezone:/etc/timezone:ro" + - "/dev/*:/dev/*" + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: 1 + capabilities: [ gpu ] \ No newline at end of file From 9d039fd2949676071fcdc19818600e210cad6733 Mon Sep 17 00:00:00 2001 From: Vineet <52542471+VineetTambe@users.noreply.github.com> Date: Mon, 2 Oct 2023 06:58:21 -0400 Subject: [PATCH 13/15] Added logic to handle "ring" field for velodyne lidars (#452) * Added ros2 huble dockerfile * Added docker compose yaml file * Updated README with instructions to run the docker container * Updated directory name from colcon_ws to ros2_ws * Updated ring calculation logic to be cocmpatible with tartanair dataset * updated container name * Added code to calculate ring in the case of velodyne sensor * updated ringFlag to make it more general for future ease of use * reverted config to default * removed extra whitespace * fixed extra whitespace * fixed whitespace in config --- docker-compose.yaml | 4 ++-- src/imageProjection.cpp | 26 ++++++++++++++++++++++---- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/docker-compose.yaml b/docker-compose.yaml index dd06afc8..defacee9 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -1,6 +1,6 @@ version: '3' services: - ddg-humble-container: + liosam-humble-jammy-container: image: liosam-humble-jammy container_name: liosam-humble-jammy-container privileged: true @@ -29,4 +29,4 @@ services: devices: - driver: nvidia count: 1 - capabilities: [ gpu ] \ No newline at end of file + capabilities: [ gpu ] diff --git a/src/imageProjection.cpp b/src/imageProjection.cpp index a35ba5cb..a7d7c470 100644 --- a/src/imageProjection.cpp +++ b/src/imageProjection.cpp @@ -74,6 +74,7 @@ class ImageProjection : public ParamServer pcl::PointCloud::Ptr fullCloud; pcl::PointCloud::Ptr extractedCloud; + int ringFlag = 0; int deskewFlag; cv::Mat rangeMat; @@ -232,7 +233,7 @@ class ImageProjection : public ParamServer cloudQueue.pop_front(); if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX) { - pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn); + pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn); } else if (sensor == SensorType::OUSTER) { @@ -262,6 +263,10 @@ class ImageProjection : public ParamServer cloudHeader = currentCloudMsg.header; timeScanCur = stamp2Sec(cloudHeader.stamp); timeScanEnd = timeScanCur + laserCloudIn->points.back().time; + + // remove Nan + vector indices; + pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices); // check dense flag if (laserCloudIn->is_dense == false) @@ -271,7 +276,7 @@ class ImageProjection : public ParamServer } // check ring channel - static int ringFlag = 0; + // we will skip the ring check in case of velodyne - as we calculate the ring value downstream (line 572) if (ringFlag == 0) { ringFlag = -1; @@ -285,8 +290,12 @@ class ImageProjection : public ParamServer } if (ringFlag == -1) { - RCLCPP_ERROR(get_logger(), "Point cloud ring channel not available, please configure your point cloud data!"); - rclcpp::shutdown(); + if (sensor == SensorType::VELODYNE) { + ringFlag = 2; + } else { + RCLCPP_ERROR(get_logger(), "Point cloud ring channel not available, please configure your point cloud data!"); + rclcpp::shutdown(); + } } } @@ -562,6 +571,15 @@ class ImageProjection : public ParamServer continue; int rowIdn = laserCloudIn->points[i].ring; + // if sensor is a velodyne (ringFlag = 2) calculate rowIdn based on number of scans + if (ringFlag == 2) { + float verticalAngle = + atan2(thisPoint.z, + sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * + 180 / M_PI; + rowIdn = (verticalAngle + (N_SCAN - 1)) / 2.0; + } + if (rowIdn < 0 || rowIdn >= N_SCAN) continue; From 102c07dd2adcb821f7517d140b443741872155bd Mon Sep 17 00:00:00 2001 From: AylonPintoQT <102977396+AylonPinto95@users.noreply.github.com> Date: Tue, 2 Apr 2024 14:52:30 +0200 Subject: [PATCH 14/15] Fix debug logs imu for ros 2 (#486) --- src/imageProjection.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/imageProjection.cpp b/src/imageProjection.cpp index a7d7c470..fbd1b90d 100644 --- a/src/imageProjection.cpp +++ b/src/imageProjection.cpp @@ -191,9 +191,9 @@ class ImageProjection : public ParamServer // ", y: " << thisImu.angular_velocity.y << // ", z: " << thisImu.angular_velocity.z << endl; // double imuRoll, imuPitch, imuYaw; - // tf::Quaternion orientation; - // tf::quaternionMsgToTF(thisImu.orientation, orientation); - // tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw); + // tf2::Quaternion orientation; + // tf2::fromMsg(thisImu.orientation, orientation); + // tf2::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw); // cout << "IMU roll pitch yaw: " << endl; // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl; } From 3526cc91a148a332269c0b2c919bfd7873bd5a35 Mon Sep 17 00:00:00 2001 From: shenxulei1225 Date: Tue, 10 Sep 2024 19:06:44 +0800 Subject: [PATCH 15/15] add note --- src/featureExtraction.cpp | 66 ++++++--- src/imageProjection.cpp | 278 ++++++++++++++++++++++++++++++++------ src/imuPreintegration.cpp | 22 ++- src/mapOptmization.cpp | 49 +++++-- 4 files changed, 338 insertions(+), 77 deletions(-) diff --git a/src/featureExtraction.cpp b/src/featureExtraction.cpp index 0a6af036..ce9c9493 100644 --- a/src/featureExtraction.cpp +++ b/src/featureExtraction.cpp @@ -16,7 +16,7 @@ class FeatureExtraction : public ParamServer { public: - + // 订阅 imageProjection 发布的 点云畸变校正后的 点云信息 rclcpp::Subscription::SharedPtr subLaserCloudInfo; rclcpp::Publisher::SharedPtr pubLaserCloudInfo; @@ -75,32 +75,35 @@ class FeatureExtraction : public ParamServer cloudHeader = msgIn->header; // new cloud header pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction - calculateSmoothness(); + calculateSmoothness(); // 计算每个点的曲率 - markOccludedPoints(); + markOccludedPoints(); // 标记遮挡和平行点 - extractFeatures(); + extractFeatures(); // 提取surface和corner特征 - publishFeatureCloud(); + publishFeatureCloud(); // 发布特征点云 } + //计算的方法和ALOAM是一致的 void calculateSmoothness() { int cloudSize = extractedCloud->points.size(); + //前5个点不要，因为遍历每一个点，要周围10点的距离 for (int i = 5; i < cloudSize - 5; i++) { + // 计算当前点和周围10个点的距离差 float diffRange = cloudInfo.point_range[i-5] + cloudInfo.point_range[i-4] + cloudInfo.point_range[i-3] + cloudInfo.point_range[i-2] + cloudInfo.point_range[i-1] - cloudInfo.point_range[i] * 10 + cloudInfo.point_range[i+1] + cloudInfo.point_range[i+2] + cloudInfo.point_range[i+3] + cloudInfo.point_range[i+4] + cloudInfo.point_range[i+5]; - - cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ; - + // 曲率值就是距离差的平方 + cloudCurvature[i] = diffRange*diffRange; //diffX * diffX + diffY * diffY + diffZ * diffZ; + // 这两个值附成初始值 ，一个是这个点被选择了，另一个是点的标签 cloudNeighborPicked[i] = 0; cloudLabel[i] = 0; - // cloudSmoothness for sorting + // cloudSmoothness for sorting 将曲率与点的索引保存到点云曲率的队列中，用来进行曲率排序 cloudSmoothness[i].value = cloudCurvature[i]; cloudSmoothness[i].ind = i; } @@ -112,12 +115,15 @@ class FeatureExtraction : public ParamServer // mark occluded points and parallel beam points for (int i = 5; i < cloudSize - 6; ++i) { - // occluded points + // occluded points 取出相邻两个点距离信息，计算两个有效点之间的列id差 + // lidar一条sacn上,相邻的两个或几个点的距离偏差很大,被视为遮挡点 float depth1 = cloudInfo.point_range[i]; float depth2 = cloudInfo.point_range[i+1]; int columnDiff = std::abs(int(cloudInfo.point_col_ind[i+1] - cloudInfo.point_col_ind[i])); if (columnDiff < 10){ - // 10 pixel diff in range image + // 10 pixel diff in range image 只有靠近才比较有意义 + + //这样的depth1 容易被遮挡，因此其之前的5个点都设置为无效点 if (depth1 - depth2 > 0.3){ cloudNeighborPicked[i - 5] = 1; cloudNeighborPicked[i - 4] = 1; @@ -125,6 +131,8 @@ class FeatureExtraction : public ParamServer cloudNeighborPicked[i - 2] = 1; cloudNeighborPicked[i - 1] = 1; cloudNeighborPicked[i] = 1; + + //这样的depth2 容易被遮挡，因此其之后的5个点都设置为无效点 }else if (depth2 - depth1 > 0.3){ cloudNeighborPicked[i + 1] = 1; cloudNeighborPicked[i + 2] = 1; @@ -138,6 +146,13 @@ class FeatureExtraction : public ParamServer float diff1 = std::abs(float(cloudInfo.point_range[i-1] - cloudInfo.point_range[i])); float diff2 = std::abs(float(cloudInfo.point_range[i+1] - cloudInfo.point_range[i])); + /* 如果两点距离比较大，就很可能是平行的点，也很可能失去观测 + 激光的射线几乎和物体的平面平行了 + 剔除这种点的原因有两个: + + 激光的数据会不准,射线被拉长 + 这种点被视为特征点后会非常不稳定,下一帧可能就没了,无法做配对了 + */ if (diff1 > 0.02 * cloudInfo.point_range[i] && diff2 > 0.02 * cloudInfo.point_range[i]) cloudNeighborPicked[i] = 1; } @@ -157,29 +172,33 @@ class FeatureExtraction : public ParamServer for (int j = 0; j < 6; j++) { - + // 把每一根 scan 等分成6份，每份分别提取特征点 安曲率排序 int sp = (cloudInfo.start_ring_index[i] * (6 - j) + cloudInfo.end_ring_index[i] * j) / 6; int ep = (cloudInfo.start_ring_index[i] * (5 - j) + cloudInfo.end_ring_index[i] * (j + 1)) / 6 - 1; if (sp >= ep) continue; - + // 安曲率排序 std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value()); + //开始提取角点 int largestPickedNum = 0; for (int k = ep; k >= sp; k--) { int ind = cloudSmoothness[k].ind; + // 如果没有被认为是遮挡点和平行点 并且曲率大于阈值 if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold) - { + { + // 每一段最多只取20个角点标签置 largestPickedNum++; if (largestPickedNum <= 20){ - cloudLabel[ind] = 1; - cornerCloud->push_back(extractedCloud->points[ind]); + cloudLabel[ind] = 1; // 1 表示是角点 + cornerCloud->push_back(extractedCloud->points[ind]); // 这个点收集进存储角点的点云中 } else { break; } - + // 将这个点周围的几个点设置为遮挡点，避免选取太集中 + // 列idx距离太远就算了，空间上也不会太集中 cloudNeighborPicked[ind] = 1; for (int l = 1; l <= 5; l++) { @@ -198,15 +217,16 @@ class FeatureExtraction : public ParamServer } } + // 提取面点 for (int k = sp; k <= ep; k++) { int ind = cloudSmoothness[k].ind; if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold) { - - cloudLabel[ind] = -1; + cloudLabel[ind] = -1; //-1 表示是面点 cloudNeighborPicked[ind] = 1; + // 把周围点设置为遮挡点 for (int l = 1; l <= 5; l++) { int columnDiff = std::abs(int(cloudInfo.point_col_ind[ind + l] - cloudInfo.point_col_ind[ind + l - 1])); if (columnDiff > 10) @@ -223,19 +243,21 @@ class FeatureExtraction : public ParamServer } } } - + for (int k = sp; k <= ep; k++) - { + { + //注意这里是小于等于0 也就是说 不是角点的都认为是面点了 if (cloudLabel[k] <= 0){ surfaceCloudScan->push_back(extractedCloud->points[k]); } } } + // 因为面点太多了，所以做个下采样 surfaceCloudScanDS->clear(); downSizeFilter.setInputCloud(surfaceCloudScan); downSizeFilter.filter(*surfaceCloudScanDS); - + *surfaceCloud += *surfaceCloudScanDS; } } diff --git a/src/imageProjection.cpp b/src/imageProjection.cpp index fbd1b90d..3f39d808 100644 --- a/src/imageProjection.cpp +++ b/src/imageProjection.cpp @@ -1,19 +1,46 @@ + +/************************************************* +-------------------------------------------------- +功能简介: + 1、利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正）； + 2、同时用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。 + +订阅： + 1、订阅原始IMU数据； + 2、订阅IMU里程计数据，来自ImuPreintegration，表示每一时刻对应的位姿； + 3、订阅原始激光点云数据。 + +发布： + 1、发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示； + 2、发布当前帧激光运动畸变校正之后的点云信息，包括点云数据、初始位姿、姿态角、有效点云数据等，发布给FeatureExtraction进行特征提取。 +*************************************************/ + + #include "utility.hpp" #include "lio_sam/msg/cloud_info.hpp" +/** + * Velodyne点云结构，变量名XYZIRT是每个变量的首字母 +*/ struct VelodynePointXYZIRT { - PCL_ADD_POINT4D - PCL_ADD_INTENSITY; - uint16_t ring; - float time; + PCL_ADD_POINT4D // 位置 + PCL_ADD_INTENSITY; // 激光点反射强度，也可以存点的索引 + uint16_t ring; // 扫描线 + float time; // 时间戳，记录相对于当前帧第一个激光点的时差，第一个点time=0 EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; +} EIGEN_ALIGN16; // 内存16字节对齐，EIGEN SSE优化要求 + +// 注册为PCL点云格式 POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (uint16_t, ring, ring) (float, time, time) ) + +/** + * Ouster点云结构 +*/ struct OusterPointXYZIRT { PCL_ADD_POINT4D; float intensity; @@ -31,35 +58,48 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT, ) // Use the Velodyne point format as a common representation +// 本程序使用Velodyne点云结构 + using PointXYZIRT = VelodynePointXYZIRT; +// imu数据队列长度 const int queueLength = 2000; class ImageProjection : public ParamServer { private: + // imu队列、odom队列的互斥锁 std::mutex imuLock; std::mutex odoLock; + // 订阅原始激光点云 rclcpp::Subscription::SharedPtr subLaserCloud; rclcpp::CallbackGroup::SharedPtr callbackGroupLidar; rclcpp::Publisher::SharedPtr pubLaserCloud; + // 发布当前帧校正后点云，有效点 rclcpp::Publisher::SharedPtr pubExtractedCloud; rclcpp::Publisher::SharedPtr pubLaserCloudInfo; + // imu数据队列（原始数据，转lidar系下） rclcpp::Subscription::SharedPtr subImu; rclcpp::CallbackGroup::SharedPtr callbackGroupImu; std::deque imuQueue; + // 里程计队列 rclcpp::Subscription::SharedPtr subOdom; rclcpp::CallbackGroup::SharedPtr callbackGroupOdom; std::deque odomQueue; + // 激光点云数据队列 std::deque cloudQueue; + + // 队列front帧，作为当前处理帧点云 sensor_msgs::msg::PointCloud2 currentCloudMsg; + // 当前激光帧起止时刻间对应的imu数据，计算相对于起始时刻的旋转增量，以及时时间戳； + // 用于插值计算当前激光帧起止时间范围内，每一时刻的旋转姿态 double *imuTime = new double[queueLength]; double *imuRotX = new double[queueLength]; double *imuRotY = new double[queueLength]; @@ -69,9 +109,12 @@ class ImageProjection : public ParamServer bool firstPointFlag; Eigen::Affine3f transStartInverse; + // 当前帧原始激光点云 pcl::PointCloud::Ptr laserCloudIn; pcl::PointCloud::Ptr tmpOusterCloudIn; + // 当期帧运动畸变校正之后的激光点云 pcl::PointCloud::Ptr fullCloud; + // 从fullCloud中提取有效点 pcl::PointCloud::Ptr extractedCloud; int ringFlag = 0; @@ -79,19 +122,29 @@ class ImageProjection : public ParamServer cv::Mat rangeMat; bool odomDeskewFlag; + // 当前激光帧起止时刻间对应的imu数据，计算相对于起始时刻的旋转增量，以及时时间戳； + // 用于插值计算当前激光帧起止时间范围内，每一时刻的旋转姿态 float odomIncreX; float odomIncreY; float odomIncreZ; + // 当前帧激光点云运动畸变校正之后的数据，包括点云数据、初始位姿、姿态角等， + // 发布给featureExtraction进行特征提取 lio_sam::msg::CloudInfo cloudInfo; + // 当前帧起始时刻 double timeScanCur; + // 当前帧结束时刻 double timeScanEnd; + // 当前帧header，包含时间戳信息 std_msgs::msg::Header cloudHeader; vector columnIdnCountVec; public: + /** + * 构造函数 + */ ImageProjection(const rclcpp::NodeOptions & options) : ParamServer("lio_sam_imageProjection", options), deskewFlag(0) { @@ -109,27 +162,35 @@ class ImageProjection : public ParamServer auto odomOpt = rclcpp::SubscriptionOptions(); odomOpt.callback_group = callbackGroupOdom; + // 订阅原始imu数据 subImu = create_subscription( imuTopic, qos_imu, std::bind(&ImageProjection::imuHandler, this, std::placeholders::_1), imuOpt); + // 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿 subOdom = create_subscription( odomTopic + "_incremental", qos_imu, std::bind(&ImageProjection::odometryHandler, this, std::placeholders::_1), odomOpt); + // 订阅原始lidar数据 subLaserCloud = create_subscription( pointCloudTopic, qos_lidar, std::bind(&ImageProjection::cloudHandler, this, std::placeholders::_1), lidarOpt); + + // 发布当前激光帧运动畸变校正后的点云，有效点 pubExtractedCloud = create_publisher( "lio_sam/deskew/cloud_deskewed", 1); + // 发布当前激光帧运动畸变校正后的点云信息 pubLaserCloudInfo = create_publisher( "lio_sam/deskew/cloud_info", qos); - + + // 初始化 allocateMemory(); + // 重置参数 resetParameters(); - + // pcl日志级别，只打ERROR日志 pcl::console::setVerbosityLevel(pcl::console::L_ERROR); } @@ -150,7 +211,10 @@ class ImageProjection : public ParamServer resetParameters(); } - + + /** + * 重置参数，接收每帧lidar数据都要重置这些参数 + */ void resetParameters() { laserCloudIn->clear(); @@ -170,13 +234,15 @@ class ImageProjection : public ParamServer imuRotZ[i] = 0; } } - + //~ImageProjection()是一个空的析构函数，它表示当ImageProjection对象被销毁时，不需要执行任何操作。 ~ImageProjection(){} void imuHandler(const sensor_msgs::msg::Imu::SharedPtr imuMsg) { + // imu原始测量数据转换到lidar系，加速度、角速度、RPY sensor_msgs::msg::Imu thisImu = imuConverter(*imuMsg); + // 上锁，添加数据的时候队列不可用 std::lock_guard lock1(imuLock); imuQueue.push_back(thisImu); @@ -198,26 +264,55 @@ class ImageProjection : public ParamServer // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl; } + + /** + * 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿 + */ void odometryHandler(const nav_msgs::msg::Odometry::SharedPtr odometryMsg) { std::lock_guard lock2(odoLock); odomQueue.push_back(*odometryMsg); } + /** + * 订阅原始lidar数据 + * 1、添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性 + * 2、当前帧起止时刻对应的imu数据、imu里程计数据处理 + * imu数据： + * 1) 遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角 + * 2) 用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0 + * imu里程计数据： + * 1) 遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿 + * 2) 用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量 + * 3、当前帧激光点云运动畸变校正 + * 1) 检查激光点距离、扫描线是否合规 + * 2) 激光运动畸变校正，保存激光点 + * 4、提取有效激光点，存extractedCloud + * 5、发布当前帧校正后点云，有效点 + * 6、重置参数，接收每帧lidar数据都要重置这些参数 + */ void cloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudMsg) { + // 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性 if (!cachePointCloud(laserCloudMsg)) return; + // 检查当前帧起止时刻对应的imu数据，如果没有IMU数据就不进行点云处理 if (!deskewInfo()) return; + + // 当前帧激光点云运动畸变校正 + // 1、检查激光点距离、扫描线是否合规 + // 2、激光运动畸变校正，保存激光点 projectPointCloud(); + // 提取有效激光点，存extractedCloud cloudExtraction(); - + + // 发布当前帧校正后点云，有效点 publishClouds(); - + // 重置参数，接收每帧lidar数据都要重置这些参数 resetParameters(); } @@ -229,15 +324,18 @@ class ImageProjection : public ParamServer return false; // convert cloud - currentCloudMsg = std::move(cloudQueue.front()); + + // 取出激光点云队列中最早的一帧 + currentCloudMsg = std::move(cloudQueue.front()); cloudQueue.pop_front(); if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX) { + // 转换成pcl点云格式 pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn); } else if (sensor == SensorType::OUSTER) { - // Convert to Velodyne format + // 转换成Velodyne格式 pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn); laserCloudIn->points.resize(tmpOusterCloudIn->size()); laserCloudIn->is_dense = tmpOusterCloudIn->is_dense; @@ -261,10 +359,12 @@ class ImageProjection : public ParamServer // get timestamp cloudHeader = currentCloudMsg.header; + // 当前帧起始时刻 timeScanCur = stamp2Sec(cloudHeader.stamp); + // 当前帧结束时刻，注：点云中激光点的time记录相对于当前帧第一个激光点的时差，第一个点time=0 timeScanEnd = timeScanCur + laserCloudIn->points.back().time; - // remove Nan + // remove Nan 存在无效点Nan需消除 vector indices; pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices); @@ -275,7 +375,7 @@ class ImageProjection : public ParamServer rclcpp::shutdown(); } - // check ring channel + // check ring channel 检查是否存在ring通道，注意static只检查一次 // we will skip the ring check in case of velodyne - as we calculate the ring value downstream (line 572) if (ringFlag == 0) { @@ -299,12 +399,13 @@ class ImageProjection : public ParamServer } } - // check point time + // check point time if (deskewFlag == 0) { deskewFlag = -1; for (auto &field : currentCloudMsg.fields) { + //检查是否存在time通道 field.name可能是t或time if (field.name == "time" || field.name == "t") { deskewFlag = 1; @@ -318,12 +419,15 @@ class ImageProjection : public ParamServer return true; } + /** + * imu数据、imu里程计数据处理结果是否成功 + */ bool deskewInfo() { std::lock_guard lock1(imuLock); std::lock_guard lock2(odoLock); - // make sure IMU data available for the scan + // 要求Imu队列有imu数据，无数据不进行其他处理 if (imuQueue.empty() || stamp2Sec(imuQueue.front().header.stamp) > timeScanCur || stamp2Sec(imuQueue.back().header.stamp) < timeScanEnd) @@ -331,18 +435,24 @@ class ImageProjection : public ParamServer RCLCPP_INFO(get_logger(), "Waiting for IMU data ..."); return false; } - + // 当前帧对应imu数据处理 imuDeskewInfo(); - + // 当前帧对应imu里程计处理 odomDeskewInfo(); return true; } + /** + * 当前帧对应imu数据处理 + * 1、遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角 + * 2、用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0 + * 注：imu数据都已经转换到lidar系下了 + */ void imuDeskewInfo() { cloudInfo.imu_available = false; - + //取lidar帧起止时刻之间的imu数据 while (!imuQueue.empty()) { if (stamp2Sec(imuQueue.front().header.stamp) < timeScanCur - 0.01) @@ -361,7 +471,7 @@ class ImageProjection : public ParamServer sensor_msgs::msg::Imu thisImuMsg = imuQueue[i]; double currentImuTime = stamp2Sec(thisImuMsg.header.stamp); - // get roll, pitch, and yaw estimation for this scan + // 提取imu姿态角RPY，作为当前lidar帧初始姿态角 if (currentImuTime <= timeScanCur) imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imu_roll_init, &cloudInfo.imu_pitch_init, &cloudInfo.imu_yaw_init); if (currentImuTime > timeScanEnd + 0.01) @@ -376,11 +486,11 @@ class ImageProjection : public ParamServer continue; } - // get angular velocity + // get angular velocity 获取角速度 double angular_x, angular_y, angular_z; imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z); - // integrate rotation + // integrate rotation 旋转角度积分 double timeDiff = currentImuTime - imuTime[imuPointerCur-1]; imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff; imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff; @@ -397,10 +507,16 @@ class ImageProjection : public ParamServer cloudInfo.imu_available = true; } + + /** 当前帧对应imu里程计处理 + * 1、遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿 + * 2、用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量 + * 注：imu数据都已经转换到lidar系下了 + **/ void odomDeskewInfo() { cloudInfo.odom_available = false; - + //取lidar帧起止时刻之间的odom数据 while (!odomQueue.empty()) { if (stamp2Sec(odomQueue.front().header.stamp) < timeScanCur - 0.01) @@ -415,7 +531,7 @@ class ImageProjection : public ParamServer if (stamp2Sec(odomQueue.front().header.stamp) > timeScanCur) return; - // get start odometry at the beinning of the scan + // 获取lidar帧初始时刻的odom nav_msgs::msg::Odometry startOdomMsg; for (int i = 0; i < (int)odomQueue.size(); ++i) @@ -434,7 +550,7 @@ class ImageProjection : public ParamServer double roll, pitch, yaw; tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw); - // Initial guess used in mapOptimization + // Initial guess used in mapOptimization 使用了初始假设 cloudInfo.initial_guess_x = startOdomMsg.pose.pose.position.x; cloudInfo.initial_guess_y = startOdomMsg.pose.pose.position.y; cloudInfo.initial_guess_z = startOdomMsg.pose.pose.position.z; @@ -461,7 +577,7 @@ class ImageProjection : public ParamServer else break; } - + //起始和结束Odom数据匹配,协方差不同时，认为数据不匹配，直接返回， odomDeskewFlag = false if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0]))) return; @@ -479,10 +595,14 @@ class ImageProjection : public ParamServer odomDeskewFlag = true; } + + /* imuTime队列中每个点都有这个点的时间。 + * 按照给定点的时间，找到对应的imu旋转数据，赋值给当前点的旋转变量 + */ void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur) { *rotXCur = 0; *rotYCur = 0; *rotZCur = 0; - + // 取到imu时间队列中，符合给定的pointtime的第一个imuPointFront点 int imuPointerFront = 0; while (imuPointerFront < imuPointerCur) { @@ -490,13 +610,16 @@ class ImageProjection : public ParamServer break; ++imuPointerFront; } - + //直接将imuPointerFront对应的数据作为给定pointTime时间点的初始姿态 if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0) { *rotXCur = imuRotX[imuPointerFront]; *rotYCur = imuRotY[imuPointerFront]; *rotZCur = imuRotZ[imuPointerFront]; } else { + /* 给定的pointTime时间点的imu数据在imuTime队列中间的某一个点，imuPointerFront ！= 0 + * 需要按照点的时间进行插值计算，找到对应的imu旋转数据，赋值给当前点的旋转变量 + */ int imuPointerBack = imuPointerFront - 1; double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); @@ -509,7 +632,7 @@ class ImageProjection : public ParamServer void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur) { *posXCur = 0; *posYCur = 0; *posZCur = 0; - + //参照照相机原理 ，原点移动对最终成像影响很小，因此非常小的时间差内移动直接忽略 // If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented. // if (cloudInfo.odomAvailable == false || odomDeskewFlag == false) @@ -524,27 +647,60 @@ class ImageProjection : public ParamServer PointType deskewPoint(PointType *point, double relTime) { + /* 如果畸形矫正标识为-1，或者缺失imu数据，说明不需要进行畸形矫正，直接返回原始数据 + * 因为IMU数据（例如姿态和加速度）通常用于精确补偿运动，故不进行去畸变。 + * 但没有IMU进行矫正，会产生严重的漂移 + */ if (deskewFlag == -1 || cloudInfo.imu_available == false) return *point; + /* Lidar数据中有这个reltime值, laserCloudIn->points[i].time + * relTime： 表示当前激光点相对于整个激光帧开始时间的相对时间。但findPosition实际实现中忽略的这个reltime + * pointTime： 是当前点的绝对时间戳，通过帧起始时间 timeScanCur 加上该点的相对时间 relTime 得到。 + */ double pointTime = timeScanCur + relTime; + /* 旋转增量： + * 这一步根据点的时间戳 pointTime 计算该时刻的旋转增量，rotXCur、rotYCur 和 rotZCur 是相对于激光帧起始时刻的旋转增量（绕X、Y、Z轴的旋转角度）。 + */ float rotXCur, rotYCur, rotZCur; findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur); - + /* 平移增量： + * 这一步根据相对时间 relTime 计算该时刻的平移增量，posXCur、posYCur 和 posZCur 是相对于起始时刻的平移增量。 + */ float posXCur, posYCur, posZCur; findPosition(relTime, &posXCur, &posYCur, &posZCur); + /* 记录第一点的逆变换： + * 如果是处理的第一个点，计算它的变换矩阵（包括旋转和平移）， + * 并且求取其逆矩阵 transStartInverse，用于将后续点相对于第一个点的运动进行去畸变。 + * firstPointFlag 标志在处理完第一个点后被置为 false，确保这个过程只在第一个点时发生。 + */ if (firstPointFlag == true) { + /* transStart是 Z(yaw)Y(pitch)X(roll)外旋，就是先转x，再转y，最后转z的变换矩阵 ，生成一个从世界坐标系到当前坐标系的变换矩阵。 + * 通过.inverse() 方法获得这个变换的逆矩阵 transStartInverse。 + * transStartInverse是逆矩阵 ，也就是说，这个矩阵将当前坐标系下的点转换回世界坐标系。 + */ transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse(); firstPointFlag = false; } - // transform points to start + /* 计算当前点的变换（transform points to start） + * rotXCur、rotYCur 和 rotZCur 是相对于激光帧起始时刻的旋转增量 + * posXCur、posYCur 和 posZCur 是相对于起始时刻的平移增量。 + * transFinal 是根据当前点的旋转和平移增量，计算出来的当前时刻相对于激光帧起始时刻的变换矩阵。 + */ Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur); - Eigen::Affine3f transBt = transStartInverse * transFinal; + /* 当前时刻激光点与第一个激光点的位姿变换: + * 通过 transStartInverse 和 transFinal 的矩阵相乘 transBt，得到从第一个点开始到当前点的相对变换。这个变换会用于去畸变。 + * transBt = transStartInverse * transFinal矩阵乘法表示的是： + * 先将当前坐标系的点通过 transStartInverse 转换回世界坐标系，再通过 transFinal 转换回当前的坐标系。 + */ + Eigen::Affine3f transBt = transStartInverse * transFinal; + + // 当前激光点在第一个激光点坐标系下的坐标 PointType newPoint; newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3); newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3); @@ -553,11 +709,17 @@ class ImageProjection : public ParamServer return newPoint; } + + /** + * 当前帧激光点云运动畸变校正 + * 1、检查激光点距离、扫描线是否合规 + * 2、激光运动畸变校正，保存激光点 + */ void projectPointCloud() { int cloudSize = laserCloudIn->points.size(); - // range image projection + // range image projection for (int i = 0; i < cloudSize; ++i) { PointType thisPoint; @@ -565,13 +727,20 @@ class ImageProjection : public ParamServer thisPoint.y = laserCloudIn->points[i].y; thisPoint.z = laserCloudIn->points[i].z; thisPoint.intensity = laserCloudIn->points[i].intensity; - + + //这里用半径作为过滤点云的条件，如果需要用其他条件过滤，在这里修改。 float range = pointDistance(thisPoint); if (range < lidarMinRange || range > lidarMaxRange) continue; int rowIdn = laserCloudIn->points[i].ring; // if sensor is a velodyne (ringFlag = 2) calculate rowIdn based on number of scans + + /* + * 这里计算以-90 ~ 90为扫描线rowIdn的计算，只适合与激光雷达线数小于90的情况， + * 中心点为激光线数/2，计算公式为：rowIdn = (verticalAngle + (N_SCAN - 1)) / 2， + * 如果激光线分布不均匀 或者激光线数大于90，均要调整算法 + */ if (ringFlag == 2) { float verticalAngle = atan2(thisPoint.z, @@ -586,6 +755,10 @@ class ImageProjection : public ParamServer if (rowIdn % downsampleRate != 0) continue; + + /* + 注意atan2(thisPoint.x, thisPoint.y)，X 是前方、y是右方，代表正前方的角度为0度 + */ int columnIdn = -1; if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER) { @@ -595,8 +768,27 @@ class ImageProjection : public ParamServer if (columnIdn >= Horizon_SCAN) columnIdn -= Horizon_SCAN; } - else if (sensor == SensorType::LIVOX) - { + + /* Livox 雷达的点云数据结构与 Velodyne 或 Ouster 等传统机械式旋转激光雷达不同，它采用了非旋转的扫描机制，使用了非重复采样模式， + * 这意味着 Livox 雷达采集的点云并不会均匀地覆盖空间，而是随机分布在整个视野中。 + * 因此，传统的基于水平角度计算列索引的方式（如 Velodyne 使用 atan2 来计算水平角度）并不适用于 Livox 雷达。 + + * 非重复采样模式： + Livox 雷达的扫描模式不是固定的旋转式，而是基于其特有的非重复采样技术。它在每次扫描中，采样的点在空间中的分布是随机的、非均匀的。这样做的好处是，随着时间的推移，雷达可以更均匀地覆盖整个视野，但在单次扫描中，点的分布并不规则。 + 因此，在每个扫描线（rowIdn）上，点的水平分布是动态变化的，不能像 Velodyne 那样通过固定的角度关系来计算每个点在范围图中的列索引。 + + * 随机采样导致无法直接通过角度计算列索引： + 对于 Velodyne 或 Ouster 这样的旋转雷达，点云中的每个点都有明确的水平角度，可以通过水平角度直接计算出在二维范围图中的列索引。 + 而Livox 的点没有固定的水平角度顺序，采集的点没有均匀的排列，因此无法通过 atan2 来计算每个点的列索引。 + + * 在这种情况下，使用 columnIdnCountVec 来累加列索引： + 由于 Livox 雷达的点分布是随机的，代码中使用了 columnIdnCountVec 来对每个扫描线（rowIdn）的列索引进行累加。 + 这意味着它不依赖于点的水平角度，而是按照点在该扫描线上的出现顺序分配列索引。 + 每次在该扫描线上处理一个点时，就将该点分配到下一个可用的列索引，并将对应的 columnIdnCountVec[rowIdn] 累加1。 + 这种方式能够确保所有点都能被有效地投影到二维范围图中，尽管它们的水平分布是不规则的。 + */ + else if (sensor == SensorType::LIVOX) + { columnIdn = columnIdnCountVec[rowIdn]; columnIdnCountVec[rowIdn] += 1; } @@ -609,7 +801,10 @@ class ImageProjection : public ParamServer continue; thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); - + /* rangeMat 是一个 OpenCV 的 cv::Mat 类型的二维矩阵，通常用于存储点云数据的距离信息，形成所谓的“范围图（range image）” + rangeMat.at(rowIdn, columnIdn) 表示访问 rangeMat 矩阵中位于 (rowIdn, columnIdn) 的元素， + 该元素是一个 float 类型的浮点数，通常表示激光雷达点云的距离值（range）。 + */ rangeMat.at(rowIdn, columnIdn) = range; int index = columnIdn + rowIdn * Horizon_SCAN; @@ -622,7 +817,11 @@ class ImageProjection : public ParamServer int count = 0; // extract segmented cloud for lidar odometry for (int i = 0; i < N_SCAN; ++i) - { + { + /* 表示跳过前 5 个点。这种处理通常用于避免扫描线两端的噪声点， + * 因为激光雷达的每条扫描线开头和结尾的点容易受到不稳定因素的影响 + *（例如，边缘点由于视角原因可能不准确）。 + */ cloudInfo.start_ring_index[i] = count - 1 + 5; for (int j = 0; j < Horizon_SCAN; ++j) { @@ -638,6 +837,7 @@ class ImageProjection : public ParamServer ++count; } } + //表示跳过扫描线末尾的 5 个点，原因与上面类似，是为了避免使用靠近扫描线末端的噪声点。 cloudInfo.end_ring_index[i] = count -1 - 5; } } diff --git a/src/imuPreintegration.cpp b/src/imuPreintegration.cpp index 8420c045..418729a3 100644 --- a/src/imuPreintegration.cpp +++ b/src/imuPreintegration.cpp @@ -61,6 +61,7 @@ class TransformFusion : public ParamServer auto laserOdomOpt = rclcpp::SubscriptionOptions(); laserOdomOpt.callback_group = callbackGroupLaserOdometry; + // 订阅地图优化的节点的全局位姿 和预积分节点的 增量位姿 subLaserOdometry = create_subscription( "lio_sam/mapping/odometry", qos, std::bind(&TransformFusion::lidarOdometryHandler, this, std::placeholders::_1), @@ -86,9 +87,10 @@ class TransformFusion : public ParamServer void lidarOdometryHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg) { std::lock_guard lock(mtx); - + // 将全局位姿保存下来 + //将ros的odom格式转换成 Eigen::Affine3f 的形式 lidarOdomAffine = odom2affine(*odomMsg); - + // 将最新帧的时间保存下来 lidarOdomTime = stamp2Sec(odomMsg->header.stamp); } @@ -98,9 +100,10 @@ class TransformFusion : public ParamServer imuOdomQueue.push_back(*odomMsg); - // get latest odometry (at current IMU stamp) + // get latest odometry (at current IMU stamp) 如果没有收到lidar位姿 就returen if (lidarOdomTime == -1) return; + //弹出时间戳小于最新lidar位姿时刻之前的imu里程计数据 while (!imuOdomQueue.empty()) { if (stamp2Sec(imuOdomQueue.front().header.stamp) <= lidarOdomTime) @@ -108,15 +111,19 @@ class TransformFusion : public ParamServer else break; } + // 计算最新队列里imu里程计的增量 Eigen::Isometry3d imuOdomAffineFront = odom2affine(imuOdomQueue.front()); Eigen::Isometry3d imuOdomAffineBack = odom2affine(imuOdomQueue.back()); Eigen::Isometry3d imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack; + // 增量补偿到lidar的位姿上去，就得到了最新的预测的位姿 Eigen::Isometry3d imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre; auto t = tf2::eigenToTransform(imuOdomAffineLast); + + // 更新tf ?? tf2::Stamped tCur; tf2::convert(t, tCur); - // publish latest odometry + // publish latest odometry 发送全局一致位姿的最新位姿 nav_msgs::msg::Odometry laserOdometry = imuOdomQueue.back(); laserOdometry.pose.pose.position.x = t.transform.translation.x; laserOdometry.pose.pose.position.y = t.transform.translation.y; @@ -145,10 +152,11 @@ class TransformFusion : public ParamServer ts.child_frame_id = baselinkFrame; tfBroadcaster->sendTransform(ts); - // publish IMU path + // publish IMU path 发布imu里程计的轨迹 static nav_msgs::msg::Path imuPath; static double last_path_time = -1; double imuTime = stamp2Sec(imuOdomQueue.back().header.stamp); + // 控制一下更新频率，不超过10hz if (imuTime - last_path_time > 0.1) { last_path_time = imuTime; @@ -156,9 +164,13 @@ class TransformFusion : public ParamServer pose_stamped.header.stamp = imuOdomQueue.back().header.stamp; pose_stamped.header.frame_id = odometryFrame; pose_stamped.pose = laserOdometry.pose.pose; + // 将最新的位姿送入轨迹中 imuPath.poses.push_back(pose_stamped); + // 把lidar时间戳之前的轨迹全部擦除 while(!imuPath.poses.empty() && stamp2Sec(imuPath.poses.front().header.stamp) < lidarOdomTime - 1.0) imuPath.poses.erase(imuPath.poses.begin()); + + // 发布轨迹，这个轨迹实际上是可视化imu预积分节点输出的预测值 if (pubImuPath->get_subscription_count() != 0) { imuPath.header.stamp = imuOdomQueue.back().header.stamp; diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp index 1fa44512..2e270b06 100644 --- a/src/mapOptmization.cpp +++ b/src/mapOptmization.cpp @@ -496,11 +496,6 @@ class mapOptimization : public ParamServer - - - - - void loopClosureThread() { if (loopClosureEnableFlag == false) @@ -533,12 +528,16 @@ class mapOptimization : public ParamServer return; mtx.lock(); + //关键帧的位置 *copy_cloudKeyPoses3D = *cloudKeyPoses3D; + //关键帧的位姿 *copy_cloudKeyPoses6D = *cloudKeyPoses6D; mtx.unlock(); // find keys + // 当前关键帧 int loopKeyCur; + // 历史关键帧 int loopKeyPre; if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false) if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false) @@ -550,6 +549,9 @@ class mapOptimization : public ParamServer { loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0); loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum); + + // 回环帧把自己周围一些点云取出来，也就是构成一个帧局部地图的一个匹配问题 + // historyKeyframeSearchNum 25帧 if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000) return; if (pubHistoryKeyFrames->get_subscription_count() != 0) @@ -558,10 +560,15 @@ class mapOptimization : public ParamServer // ICP Settings static pcl::IterativeClosestPoint icp; + // 设置最大相关距离 icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2); + // 最大优化次数 icp.setMaximumIterations(100); + //TransformationEpsilon 表示ICP算法中变换矩阵的收敛阈值 icp.setTransformationEpsilon(1e-6); + // EuclideanFitnessEpsilon 表示ICP算法中目标点云和源点云之间的欧式距离的收敛阈值 icp.setEuclideanFitnessEpsilon(1e-6); + // RANSAC 迭代次数 icp.setRANSACIterations(0); // Align clouds @@ -569,7 +576,8 @@ class mapOptimization : public ParamServer icp.setInputTarget(prevKeyframeCloud); pcl::PointCloud::Ptr unused_result(new pcl::PointCloud()); icp.align(*unused_result); - + + // 检测icp是否收敛 或 得分满足要求 if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) return; @@ -584,27 +592,40 @@ class mapOptimization : public ParamServer // Get pose transformation float x, y, z, roll, pitch, yaw; Eigen::Affine3f correctionLidarFrame; + + // 把修正后的当前点云发布供可视化使用 correctionLidarFrame = icp.getFinalTransformation(); - // transform from world origin to wrong pose + + // transform from world origin to wrong pose 取出当前帧的位姿 Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]); - // transform from world origin to corrected pose + + // transform from world origin to corrected pose 将icp结果补偿过去，就是当前帧的更为准确的位姿结果 Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame + + // 将当前帧补偿后的位姿 转换成 平移和旋转 pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw); + + /* 将当前帧补偿后的位姿 转换成 gtsam的形式 + * From 和 To相当于帧间约束的因子 + * To是历史回环帧的位姿 + */ gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]); gtsam::Vector Vector6(6); + + // 使用icp的得分作为他们的约束噪声项 float noiseScore = icp.getFitnessScore(); Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore; noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6); - // Add pose constraint + // Add pose constraint 使用icp的得分作为他们的约束噪声项 mtx.lock(); loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre)); loopPoseQueue.push_back(poseFrom.between(poseTo)); loopNoiseQueue.push_back(constraintNoise); mtx.unlock(); - // add loop constriant + // add loop constriant 将两帧索引，两帧相对位姿和噪声作为回环约束 送入对列 loopIndexContainer[loopKeyCur] = loopKeyPre; } @@ -622,10 +643,14 @@ class mapOptimization : public ParamServer std::vector pointSearchIndLoop; std::vector pointSearchSqDisLoop; kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D); + // 只包含关键帧位移信息的点云填充kdtree kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0); for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i) { + /*历史帧，必须比当前帧间隔30s以上,必须满足时间上超过一定阈值，才认为是一个有效的回环 + * historyKeyframeSearchTimeDiff 时间阈值 30s + */ int id = pointSearchIndLoop[i]; if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff) { @@ -633,7 +658,7 @@ class mapOptimization : public ParamServer break; } } - + // 如果没有找到回环或者回环找到自己身上去了，就认为是本次回环寻找失败 if (loopKeyPre == -1 || loopKeyCur == loopKeyPre) return false; @@ -702,11 +727,13 @@ class mapOptimization : public ParamServer // extract near keyframes nearKeyframes->clear(); int cloudSize = copy_cloudKeyPoses6D->size(); + // searchNum 是搜索范围 ，遍历帧的范围 for (int i = -searchNum; i <= searchNum; ++i) { int keyNear = key + i; if (keyNear < 0 || keyNear >= cloudSize ) continue; + // 把对应角点和面点的点云转到世界坐标系下去 *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]); *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]); }