├── .gitmodules ├── package.xml ├── LICENSE ├── README.md ├── launch ├── lio_mid360.launch.py └── lio_avia.launch.py ├── CMakeLists.txt ├── include └── SensorEventQueue.h ├── rviz └── lio_rviz.rviz └── src └── lio_node.cpp /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lidar_inertial_odometry"] 2 | path = lidar_inertial_odometry 3 | url = https://github.com/93won/lidar_inertial_odometry.git 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lio_ros_wrapper 5 | 0.1.0 6 | ROS2 wrapper for LiDAR-Inertial Odometry 7 | Seungwon Choi 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | sensor_msgs 14 | geometry_msgs 15 | nav_msgs 16 | tf2 17 | tf2_ros 18 | tf2_geometry_msgs 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Seungwon Choi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LIO ROS Wrapper 2 | 3 | ROS2 wrapper for [lidar_inertial_odometry](https://github.com/93won/lidar_inertial_odometry). 4 | 5 | ### MIT License 6 | 7 | ## Demo Video 8 | 9 | [![LIO ROS Wrapper Demo](https://img.youtube.com/vi/AlfKASD0Rrc/0.jpg)](https://youtu.be/AlfKASD0Rrc) 10 | 11 | **Watch on YouTube**: https://youtu.be/AlfKASD0Rrc 12 | 13 | ## Installation 14 | 15 | ```bash 16 | # Clone with submodules 17 | cd ~/ros2_ws/src 18 | git clone --recursive https://github.com/93won/lio_ros_wrapper.git 19 | cd lio_ros_wrapper 20 | 21 | # If already cloned without --recursive 22 | git submodule update --init --recursive 23 | 24 | # Install dependencies 25 | sudo apt install -y libeigen3-dev libyaml-cpp-dev libspdlog-dev 26 | 27 | # Build 28 | cd ~/ros2_ws 29 | colcon build --packages-select lio_ros_wrapper --cmake-args -DCMAKE_BUILD_TYPE=Release 30 | source install/setup.bash 31 | ``` 32 | 33 | ## Usage 34 | 35 | ```bash 36 | # Launch with RViz (Livox AVIA) 37 | ros2 launch lio_ros_wrapper lio_avia.launch.py 38 | 39 | # Launch with RViz (Livox Mid360) 40 | ros2 launch lio_ros_wrapper lio_mid360.launch.py 41 | ``` 42 | 43 | ## Example Dataset 44 | 45 | Download ROS2 bag files from: 46 | **https://drive.google.com/drive/folders/1uqa_LDlOTtQo1PIBcXQ22JObDuHHwBAs?usp=sharing** 47 | 48 | ```bash 49 | # Play bag file 50 | ros2 bag play your_dataset.db3 -r 1.0 51 | 52 | # Play at 10x speed 53 | ros2 bag play your_dataset.db3 -r 10 54 | ``` 55 | 56 | ## License 57 | 58 | MIT License 59 | -------------------------------------------------------------------------------- /launch/lio_mid360.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | LIO ROS2 Launch File (Mid360 LiDAR) 3 | 4 | Author: Seungwon Choi 5 | Email: csw3575@snu.ac.kr 6 | Date: 2025-11-22 7 | """ 8 | 9 | from launch import LaunchDescription 10 | from launch_ros.actions import Node 11 | from launch.actions import DeclareLaunchArgument 12 | from launch.substitutions import LaunchConfiguration 13 | from ament_index_python.packages import get_package_share_directory 14 | import os 15 | 16 | 17 | def generate_launch_description(): 18 | # Get package share directory 19 | pkg_share = get_package_share_directory('lio_ros_wrapper') 20 | # Use config from lidar_inertial_odometry subdirectory 21 | default_config = os.path.join(pkg_share, 'config', 'lidar_inertial_odometry', 'mid360.yaml') 22 | 23 | # RViz config path (relative to package) 24 | default_rviz_config = os.path.join(pkg_share, 'rviz', 'lio_rviz.rviz') 25 | 26 | return LaunchDescription([ 27 | # Declare launch arguments 28 | DeclareLaunchArgument( 29 | 'imu_topic', 30 | default_value='/livox/imu', 31 | description='IMU data topic' 32 | ), 33 | DeclareLaunchArgument( 34 | 'lidar_topic', 35 | default_value='/livox/lidar', 36 | description='LiDAR point cloud topic' 37 | ), 38 | DeclareLaunchArgument( 39 | 'config_file', 40 | default_value=default_config, 41 | description='LIO configuration file path' 42 | ), 43 | DeclareLaunchArgument( 44 | 'init_imu_samples', 45 | default_value='100', 46 | description='Number of IMU samples for gravity initialization' 47 | ), 48 | 49 | # LIO Node 50 | Node( 51 | package='lio_ros_wrapper', 52 | executable='lio_node', 53 | name='lio_node', 54 | output='screen', 55 | parameters=[{ 56 | 'imu_topic': LaunchConfiguration('imu_topic'), 57 | 'lidar_topic': LaunchConfiguration('lidar_topic'), 58 | 'config_file': LaunchConfiguration('config_file'), 59 | 'init_imu_samples': LaunchConfiguration('init_imu_samples'), 60 | }] 61 | ), 62 | 63 | # RViz2 64 | Node( 65 | package='rviz2', 66 | executable='rviz2', 67 | name='rviz2', 68 | arguments=['-d', default_rviz_config], 69 | output='screen' 70 | ), 71 | ]) 72 | -------------------------------------------------------------------------------- /launch/lio_avia.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | LIO ROS2 Launch File (Avia LiDAR) 3 | 4 | Author: Seungwon Choi 5 | Email: csw3575@snu.ac.kr 6 | Date: 2025-11-22 7 | """ 8 | 9 | from launch import LaunchDescription 10 | from launch_ros.actions import Node 11 | from launch.actions import DeclareLaunchArgument 12 | from launch.substitutions import LaunchConfiguration 13 | from ament_index_python.packages import get_package_share_directory 14 | import os 15 | 16 | 17 | def generate_launch_description(): 18 | # Get package share directory 19 | pkg_share = get_package_share_directory('lio_ros_wrapper') 20 | # Use config from lidar_inertial_odometry subdirectory 21 | default_config = os.path.join(pkg_share, 'config', 'lidar_inertial_odometry', 'avia.yaml') 22 | 23 | # RViz config path (relative to package) 24 | default_rviz_config = os.path.join(pkg_share, 'rviz', 'lio_rviz.rviz') 25 | 26 | return LaunchDescription([ 27 | # Declare launch arguments 28 | DeclareLaunchArgument( 29 | 'imu_topic', 30 | default_value='/livox/avia/imu', 31 | description='IMU data topic' 32 | ), 33 | DeclareLaunchArgument( 34 | 'lidar_topic', 35 | default_value='/livox/avia/lidar', 36 | description='LiDAR point cloud topic' 37 | ), 38 | DeclareLaunchArgument( 39 | 'config_file', 40 | default_value=default_config, 41 | description='LIO configuration file path' 42 | ), 43 | DeclareLaunchArgument( 44 | 'init_imu_samples', 45 | default_value='100', 46 | description='Number of IMU samples for gravity initialization' 47 | ), 48 | 49 | # LIO Node 50 | Node( 51 | package='lio_ros_wrapper', 52 | executable='lio_node', 53 | name='lio_node', 54 | output='screen', 55 | parameters=[{ 56 | 'imu_topic': LaunchConfiguration('imu_topic'), 57 | 'lidar_topic': LaunchConfiguration('lidar_topic'), 58 | 'config_file': LaunchConfiguration('config_file'), 59 | 'init_imu_samples': LaunchConfiguration('init_imu_samples'), 60 | }] 61 | ), 62 | 63 | # RViz2 64 | Node( 65 | package='rviz2', 66 | executable='rviz2', 67 | name='rviz2', 68 | arguments=['-d', default_rviz_config], 69 | output='screen' 70 | ), 71 | ]) 72 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(lio_ros_wrapper) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # Set C++17 standard 9 | set(CMAKE_CXX_STANDARD 17) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | 12 | # Find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(nav_msgs REQUIRED) 18 | find_package(tf2 REQUIRED) 19 | find_package(tf2_ros REQUIRED) 20 | find_package(tf2_geometry_msgs REQUIRED) 21 | 22 | find_package(Eigen3 REQUIRED) 23 | find_package(yaml-cpp REQUIRED) 24 | 25 | # Force system spdlog to avoid conflicts 26 | find_package(PkgConfig REQUIRED) 27 | pkg_check_modules(SPDLOG REQUIRED spdlog) 28 | 29 | # LIO directory (no Pangolin needed for ROS2 - use RViz instead) 30 | set(LIO_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lidar_inertial_odometry) 31 | 32 | # Include directories 33 | include_directories( 34 | include 35 | ${EIGEN3_INCLUDE_DIR} 36 | ${LIO_DIR}/src/core 37 | ${LIO_DIR}/src/util 38 | ${LIO_DIR}/thirdparty/unordered_dense 39 | ) 40 | 41 | # Collect LIO source files (no viewer - use RViz for visualization) 42 | set(LIO_SOURCES 43 | ${LIO_DIR}/src/core/Estimator.cpp 44 | ${LIO_DIR}/src/core/State.cpp 45 | ${LIO_DIR}/src/core/VoxelMap.cpp 46 | ${LIO_DIR}/src/core/ProbabilisticKernelOptimizer.cpp 47 | ${LIO_DIR}/src/util/ConfigUtils.cpp 48 | ${LIO_DIR}/src/util/PointCloudUtils.cpp 49 | ${LIO_DIR}/src/util/LieUtils.cpp 50 | ) 51 | 52 | # LIO ROS2 Node 53 | add_executable(lio_node 54 | src/lio_node.cpp 55 | ${LIO_SOURCES} 56 | ) 57 | 58 | ament_target_dependencies(lio_node 59 | rclcpp 60 | sensor_msgs 61 | geometry_msgs 62 | nav_msgs 63 | tf2 64 | tf2_ros 65 | tf2_geometry_msgs 66 | ) 67 | 68 | target_link_libraries(lio_node 69 | yaml-cpp 70 | ${SPDLOG_LIBRARIES} 71 | ) 72 | 73 | target_include_directories(lio_node PRIVATE 74 | ${SPDLOG_INCLUDE_DIRS} 75 | ) 76 | 77 | if(TARGET Eigen3::Eigen) 78 | target_link_libraries(lio_node Eigen3::Eigen) 79 | else() 80 | target_include_directories(lio_node PRIVATE ${EIGEN3_INCLUDE_DIR}) 81 | endif() 82 | 83 | # Install targets 84 | install(TARGETS 85 | lio_node 86 | DESTINATION lib/${PROJECT_NAME} 87 | ) 88 | 89 | # Install launch files 90 | install(DIRECTORY 91 | launch 92 | DESTINATION share/${PROJECT_NAME} 93 | ) 94 | 95 | # Install rviz config files 96 | install(DIRECTORY 97 | rviz 98 | DESTINATION share/${PROJECT_NAME} 99 | ) 100 | 101 | # Install LIO config files (from lidar_inertial_odometry) 102 | install(DIRECTORY 103 | lidar_inertial_odometry/config/ 104 | DESTINATION share/${PROJECT_NAME}/config/lidar_inertial_odometry 105 | FILES_MATCHING PATTERN "*.yaml" 106 | ) 107 | 108 | if(BUILD_TESTING) 109 | find_package(ament_lint_auto REQUIRED) 110 | ament_lint_auto_find_test_dependencies() 111 | endif() 112 | 113 | ament_package() 114 | -------------------------------------------------------------------------------- /include/SensorEventQueue.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SensorEventQueue.h 3 | * @brief Thread-safe priority queue for sensor events with timestamp-based ordering 4 | * @author Seungwon Choi 5 | * @email csw3575@snu.ac.kr 6 | * @date 2025-11-22 7 | * @copyright Copyright (c) 2025 Seungwon Choi. All rights reserved. 8 | * 9 | * @par License 10 | * This project is released under the MIT License. 11 | */ 12 | 13 | #pragma once 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | // Forward declaration for LIO types 23 | namespace lio { 24 | struct IMUData; 25 | struct LidarData; 26 | } 27 | 28 | /** 29 | * @brief Sensor type enum 30 | */ 31 | enum class SensorType { 32 | IMU, 33 | LIDAR 34 | }; 35 | 36 | /** 37 | * @brief Sensor event with timestamp-based ordering 38 | * lio_player.cpp의 SensorEvent 패턴을 ROS2 realtime에 적용 39 | */ 40 | struct SensorEvent { 41 | SensorType type; 42 | double timestamp; 43 | 44 | // Actual sensor data (using shared_ptr for efficiency) 45 | std::shared_ptr imu_data; 46 | std::shared_ptr lidar_data; 47 | 48 | SensorEvent() : type(SensorType::IMU), timestamp(0.0) {} 49 | 50 | SensorEvent(double ts, std::shared_ptr imu) 51 | : type(SensorType::IMU), timestamp(ts), imu_data(imu), lidar_data(nullptr) {} 52 | 53 | SensorEvent(double ts, std::shared_ptr lidar) 54 | : type(SensorType::LIDAR), timestamp(ts), imu_data(nullptr), lidar_data(lidar) {} 55 | 56 | // Comparison operator for priority queue (MIN heap - oldest timestamp first) 57 | bool operator<(const SensorEvent& other) const { 58 | // Reverse comparison for MIN heap (priority_queue is MAX heap by default) 59 | return timestamp > other.timestamp; 60 | } 61 | }; 62 | 63 | /** 64 | * @brief Thread-safe priority queue for sensor events with timestamp-based ordering 65 | * 66 | * lio_player.cpp의 CreateEventSequence() 패턴을 실시간으로 구현: 67 | * - IMU callback → push to queue 68 | * - LiDAR callback → push to queue 69 | * - Processing thread → pop in timestamp order (oldest first) 70 | * 71 | * Key difference from lio_player: 72 | * - lio_player: Load all data, sort once, play back 73 | * - ROS2: Receive data realtime, maintain sorted order dynamically 74 | */ 75 | class SensorEventQueue { 76 | public: 77 | SensorEventQueue() : shutdown_(false) {} 78 | 79 | ~SensorEventQueue() { 80 | shutdown(); 81 | } 82 | 83 | /** 84 | * @brief Push IMU event to queue (automatically sorted by timestamp) 85 | */ 86 | void pushIMU(double timestamp, std::shared_ptr imu_data) { 87 | { 88 | std::lock_guard lock(mutex_); 89 | if (shutdown_) return; 90 | 91 | SensorEvent event(timestamp, imu_data); 92 | queue_.push(event); 93 | } 94 | cv_.notify_one(); 95 | } 96 | 97 | /** 98 | * @brief Push LiDAR event to queue (automatically sorted by timestamp) 99 | */ 100 | void pushLiDAR(double timestamp, std::shared_ptr lidar_data) { 101 | { 102 | std::lock_guard lock(mutex_); 103 | if (shutdown_) return; 104 | 105 | SensorEvent event(timestamp, lidar_data); 106 | queue_.push(event); 107 | } 108 | cv_.notify_one(); 109 | } 110 | 111 | /** 112 | * @brief Pop oldest event from queue (blocking) 113 | * @return std::optional The event, or std::nullopt if shutdown 114 | * 115 | * ⭐ Key: Priority queue automatically gives us the OLDEST timestamp first! 116 | */ 117 | std::optional pop() { 118 | std::unique_lock lock(mutex_); 119 | 120 | // Wait until queue has events or shutdown 121 | cv_.wait(lock, [this]() { 122 | return !queue_.empty() || shutdown_; 123 | }); 124 | 125 | if (shutdown_ && queue_.empty()) { 126 | return std::nullopt; 127 | } 128 | 129 | // Get event with smallest timestamp (oldest) 130 | SensorEvent event = queue_.top(); 131 | queue_.pop(); 132 | return event; 133 | } 134 | 135 | /** 136 | * @brief Get current queue size 137 | */ 138 | size_t size() const { 139 | std::lock_guard lock(mutex_); 140 | return queue_.size(); 141 | } 142 | 143 | /** 144 | * @brief Check if queue is empty 145 | */ 146 | bool empty() const { 147 | std::lock_guard lock(mutex_); 148 | return queue_.empty(); 149 | } 150 | 151 | /** 152 | * @brief Shutdown the queue (unblock all waiting threads) 153 | */ 154 | void shutdown() { 155 | { 156 | std::lock_guard lock(mutex_); 157 | shutdown_ = true; 158 | } 159 | cv_.notify_all(); 160 | } 161 | 162 | /** 163 | * @brief Clear all events in the queue 164 | */ 165 | void clear() { 166 | std::lock_guard lock(mutex_); 167 | // Clear priority_queue (no clear() method, so recreate) 168 | std::priority_queue empty_queue; 169 | queue_.swap(empty_queue); 170 | } 171 | 172 | private: 173 | // ⭐ Priority queue: automatically maintains timestamp ordering! 174 | std::priority_queue queue_; 175 | mutable std::mutex mutex_; 176 | std::condition_variable cv_; 177 | bool shutdown_; 178 | }; 179 | -------------------------------------------------------------------------------- /rviz/lio_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 632 10 | - Class: rviz_common/Selection 11 | Name: Selection 12 | - Class: rviz_common/Tool Properties 13 | Expanded: 14 | - /2D Goal Pose1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz_common/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz_common/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: PointCloud2 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.10000000149011612 32 | Autocompute Intensity Bounds: true 33 | Autocompute Value Bounds: 34 | Max Value: 2.680649518966675 35 | Min Value: -0.14708352088928223 36 | Value: true 37 | Axis: Z 38 | Channel Name: intensity 39 | Class: rviz_default_plugins/PointCloud2 40 | Color: 255; 255; 255 41 | Color Transformer: AxisColor 42 | Decay Time: 10000 43 | Enabled: true 44 | Invert Rainbow: false 45 | Max Color: 255; 255; 255 46 | Max Intensity: 4096 47 | Min Color: 0; 0; 0 48 | Min Intensity: 0 49 | Name: PointCloud2 50 | Position Transformer: XYZ 51 | Selectable: true 52 | Size (Pixels): 1 53 | Size (m): 0.009999999776482582 54 | Style: Points 55 | Topic: 56 | Depth: 5 57 | Durability Policy: Volatile 58 | Filter size: 10 59 | History Policy: Keep Last 60 | Reliability Policy: Reliable 61 | Value: /lio/map 62 | Use Fixed Frame: true 63 | Use rainbow: true 64 | Value: true 65 | - Alpha: 1 66 | Axes Length: 1 67 | Axes Radius: 0.10000000149011612 68 | Class: rviz_default_plugins/Pose 69 | Color: 255; 25; 0 70 | Enabled: true 71 | Head Length: 0.30000001192092896 72 | Head Radius: 0.10000000149011612 73 | Name: Pose 74 | Shaft Length: 1 75 | Shaft Radius: 0.05000000074505806 76 | Shape: Axes 77 | Topic: 78 | Depth: 5 79 | Durability Policy: Volatile 80 | Filter size: 10 81 | History Policy: Keep Last 82 | Reliability Policy: Reliable 83 | Value: /lio/pose 84 | Value: true 85 | Enabled: true 86 | Global Options: 87 | Background Color: 48; 48; 48 88 | Fixed Frame: map 89 | Frame Rate: 30 90 | Name: root 91 | Tools: 92 | - Class: rviz_default_plugins/Interact 93 | Hide Inactive Objects: true 94 | - Class: rviz_default_plugins/MoveCamera 95 | - Class: rviz_default_plugins/Select 96 | - Class: rviz_default_plugins/FocusCamera 97 | - Class: rviz_default_plugins/Measure 98 | Line color: 128; 128; 0 99 | - Class: rviz_default_plugins/SetInitialPose 100 | Covariance x: 0.25 101 | Covariance y: 0.25 102 | Covariance yaw: 0.06853891909122467 103 | Topic: 104 | Depth: 5 105 | Durability Policy: Volatile 106 | History Policy: Keep Last 107 | Reliability Policy: Reliable 108 | Value: /initialpose 109 | - Class: rviz_default_plugins/SetGoal 110 | Topic: 111 | Depth: 5 112 | Durability Policy: Volatile 113 | History Policy: Keep Last 114 | Reliability Policy: Reliable 115 | Value: /goal_pose 116 | - Class: rviz_default_plugins/PublishPoint 117 | Single click: true 118 | Topic: 119 | Depth: 5 120 | Durability Policy: Volatile 121 | History Policy: Keep Last 122 | Reliability Policy: Reliable 123 | Value: /clicked_point 124 | Transformation: 125 | Current: 126 | Class: rviz_default_plugins/TF 127 | Value: true 128 | Views: 129 | Current: 130 | Class: rviz_default_plugins/Orbit 131 | Distance: 3.7983803749084473 132 | Enable Stereo Rendering: 133 | Stereo Eye Separation: 0.05999999865889549 134 | Stereo Focal Distance: 1 135 | Swap Stereo Eyes: false 136 | Value: false 137 | Focal Point: 138 | X: 1.070921540260315 139 | Y: -2.4436278343200684 140 | Z: 7.933935642242432 141 | Focal Shape Fixed Size: false 142 | Focal Shape Size: 0.05000000074505806 143 | Invert Z Axis: false 144 | Name: Current View 145 | Near Clip Distance: 0.009999999776482582 146 | Pitch: 1.1447961330413818 147 | Target Frame: base_link 148 | Value: Orbit (rviz) 149 | Yaw: 4.115396499633789 150 | Saved: ~ 151 | Window Geometry: 152 | Displays: 153 | collapsed: false 154 | Height: 1011 155 | Hide Left Dock: false 156 | Hide Right Dock: false 157 | QMainWindow State: 000000ff00000000fd0000000400000000000001f70000033dfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000033d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000003c1000001c00000000000000000fb0000000a0049006d006100670065000000058d000000e90000000000000000fb0000000c00430061006d006500720061000000058e000000e80000000000000000fb0000000a0049006d00610067006501000005a5000002a60000000000000000000000010000038e0000033dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000033d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e0000005afc0100000002fb0000000800540069006d006501000000000000073e0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005410000033d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 158 | Selection: 159 | collapsed: false 160 | Time: 161 | collapsed: false 162 | Tool Properties: 163 | collapsed: false 164 | Views: 165 | collapsed: false 166 | Width: 1854 167 | X: 66 168 | Y: 32 169 | -------------------------------------------------------------------------------- /src/lio_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lio_node.cpp 3 | * @brief ROS2 wrapper node for LiDAR-Inertial Odometry 4 | * @author Seungwon Choi 5 | * @email csw3575@snu.ac.kr 6 | * @date 2025-11-22 7 | * @copyright Copyright (c) 2025 Seungwon Choi. All rights reserved. 8 | * 9 | * @par License 10 | * This project is released under the MIT License. 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // LIO includes 29 | #include "Estimator.h" 30 | #include "ConfigUtils.h" 31 | #include "SensorEventQueue.h" 32 | 33 | using namespace lio; 34 | 35 | /** 36 | * @brief LIO processing result for publisher thread 37 | */ 38 | struct LIOProcessingResult { 39 | bool success; 40 | double timestamp; 41 | State state; // LIO state (position, rotation, velocity, biases) 42 | PointCloudPtr processed_cloud; // Current scan (downsampled) 43 | PointCloudPtr raw_cloud; // Raw scan (for visualization) 44 | 45 | LIOProcessingResult() : success(false), timestamp(0.0) {} 46 | }; 47 | 48 | class LIONode : public rclcpp::Node 49 | { 50 | public: 51 | LIONode() : Node("lio_node"), 52 | gravity_initialized_(false), 53 | running_(true), 54 | imu_count_(0), 55 | lidar_count_(0), 56 | prev_rotation_(Eigen::Matrix3f::Identity()), 57 | prev_timestamp_(0.0), 58 | first_odom_frame_(true) 59 | { 60 | // Declare parameters 61 | this->declare_parameter("imu_topic", "/livox/imu"); 62 | this->declare_parameter("lidar_topic", "/livox/lidar"); 63 | this->declare_parameter("config_file", ""); 64 | this->declare_parameter("init_imu_samples", 200); 65 | 66 | // Get parameters 67 | std::string imu_topic = this->get_parameter("imu_topic").as_string(); 68 | std::string lidar_topic = this->get_parameter("lidar_topic").as_string(); 69 | std::string config_path = this->get_parameter("config_file").as_string(); 70 | init_imu_samples_ = this->get_parameter("init_imu_samples").as_int(); 71 | 72 | RCLCPP_INFO(this->get_logger(), "Starting LIO Node"); 73 | RCLCPP_INFO(this->get_logger(), "IMU topic: %s", imu_topic.c_str()); 74 | RCLCPP_INFO(this->get_logger(), "LiDAR topic: %s", lidar_topic.c_str()); 75 | RCLCPP_INFO(this->get_logger(), "Config file: %s", config_path.c_str()); 76 | RCLCPP_INFO(this->get_logger(), "Init IMU samples: %d", init_imu_samples_); 77 | 78 | // Load LIO configuration 79 | try { 80 | if (config_path.empty()) { 81 | throw std::runtime_error("Config file path not specified! Please set 'config_file' parameter."); 82 | } 83 | 84 | if (!LoadConfig(config_path, config_)) { 85 | throw std::runtime_error("Failed to load config file: " + config_path); 86 | } 87 | 88 | PrintConfig(config_); 89 | 90 | } catch (const std::exception& e) { 91 | RCLCPP_ERROR(this->get_logger(), "Failed to load config: %s", e.what()); 92 | throw; 93 | } 94 | 95 | // Create subscribers 96 | imu_sub_ = this->create_subscription( 97 | imu_topic, 1000, 98 | [this](const sensor_msgs::msg::Imu::SharedPtr msg) { 99 | this->imuCallback(msg); 100 | }); 101 | 102 | lidar_sub_ = this->create_subscription( 103 | lidar_topic, 10, 104 | [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { 105 | this->lidarCallback(msg); 106 | }); 107 | 108 | // Create publishers 109 | odom_pub_ = this->create_publisher("/lio/odometry", 10); 110 | pose_pub_ = this->create_publisher("/lio/pose", 100); // 100Hz for IMU rate 111 | current_scan_pub_ = this->create_publisher("/lio/current_scan", 10); 112 | raw_scan_pub_ = this->create_publisher("/lio/raw_scan", 10); 113 | map_cloud_pub_ = this->create_publisher("/lio/map", 10); 114 | trajectory_pub_ = this->create_publisher("/lio/trajectory", 10); 115 | 116 | // TF broadcaster 117 | tf_broadcaster_ = std::make_shared(this); 118 | 119 | // Initialize estimator (will be done after gravity initialization) 120 | estimator_ = std::make_shared(); 121 | 122 | // Configure estimator parameters from config 123 | estimator_->m_params.voxel_size = config_.estimator.voxel_size; 124 | estimator_->m_params.max_correspondences = config_.estimator.max_correspondences; 125 | estimator_->m_params.max_correspondence_distance = config_.estimator.max_correspondence_distance; 126 | estimator_->m_params.max_iterations = config_.estimator.max_iterations; 127 | estimator_->m_params.convergence_threshold = config_.estimator.convergence_threshold; 128 | estimator_->m_params.enable_undistortion = config_.estimator.enable_undistortion; 129 | estimator_->m_params.max_map_distance = config_.estimator.max_distance; 130 | estimator_->m_params.voxel_hierarchy_factor = config_.estimator.voxel_hierarchy_factor; 131 | estimator_->m_params.frustum_fov_horizontal = config_.estimator.frustum_fov_horizontal; 132 | estimator_->m_params.frustum_fov_vertical = config_.estimator.frustum_fov_vertical; 133 | estimator_->m_params.frustum_max_range = config_.estimator.frustum_max_range; 134 | estimator_->m_params.keyframe_translation_threshold = config_.estimator.keyframe_translation_threshold; 135 | estimator_->m_params.keyframe_rotation_threshold = config_.estimator.keyframe_rotation_threshold; 136 | estimator_->m_params.scan_planarity_threshold = config_.estimator.scan_planarity_threshold; 137 | estimator_->m_params.map_planarity_threshold = config_.estimator.map_planarity_threshold; 138 | estimator_->m_params.stride = config_.estimator.stride; 139 | estimator_->m_params.stride_then_voxel = config_.estimator.stride_then_voxel; 140 | 141 | // Configure IMU noise parameters (convert covariance to standard deviation) 142 | estimator_->m_params.gyr_noise_std = std::sqrt(config_.imu.gyr_cov); 143 | estimator_->m_params.acc_noise_std = std::sqrt(config_.imu.acc_cov); 144 | estimator_->m_params.gyr_bias_noise_std = std::sqrt(config_.imu.b_gyr_cov); 145 | estimator_->m_params.acc_bias_noise_std = std::sqrt(config_.imu.b_acc_cov); 146 | 147 | // Configure extrinsics 148 | estimator_->m_params.R_il = config_.extrinsics.R_il.cast(); 149 | estimator_->m_params.t_il = config_.extrinsics.t_il.cast(); 150 | 151 | // Configure gravity 152 | estimator_->m_params.gravity = config_.imu.gravity.cast(); 153 | 154 | // Update process noise matrix 155 | estimator_->UpdateProcessNoise(); 156 | 157 | RCLCPP_INFO(this->get_logger(), "[Estimator] Configured from YAML"); 158 | RCLCPP_INFO(this->get_logger(), " Voxel size: %.2f m", estimator_->m_params.voxel_size); 159 | RCLCPP_INFO(this->get_logger(), " IMU noise: gyr=%.4f rad/s, acc=%.4f m/s²", 160 | estimator_->m_params.gyr_noise_std, 161 | estimator_->m_params.acc_noise_std); 162 | 163 | // Start worker threads 164 | processing_thread_ = std::thread(&LIONode::processingThreadLoop, this); 165 | publisher_thread_ = std::thread(&LIONode::publisherThreadLoop, this); 166 | 167 | RCLCPP_INFO(this->get_logger(), "LIO Node initialized successfully"); 168 | RCLCPP_INFO(this->get_logger(), "Processing & Publisher threads started"); 169 | RCLCPP_INFO(this->get_logger(), "Waiting for %d IMU samples for gravity initialization...", 170 | init_imu_samples_); 171 | } 172 | 173 | ~LIONode() { 174 | RCLCPP_INFO(this->get_logger(), "Shutting down LIO Node..."); 175 | 176 | // Stop threads 177 | running_ = false; 178 | event_queue_.shutdown(); 179 | result_queue_.shutdown(); 180 | 181 | // Wait for threads to finish 182 | if (processing_thread_.joinable()) { 183 | processing_thread_.join(); 184 | RCLCPP_INFO(this->get_logger(), "Processing thread stopped"); 185 | } 186 | 187 | if (publisher_thread_.joinable()) { 188 | publisher_thread_.join(); 189 | RCLCPP_INFO(this->get_logger(), "Publisher thread stopped"); 190 | } 191 | 192 | RCLCPP_INFO(this->get_logger(), "LIO Node shutdown complete"); 193 | } 194 | 195 | private: 196 | // IMU Callback: Lightweight - just push to queue 197 | void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) 198 | { 199 | imu_count_++; 200 | 201 | double timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; 202 | 203 | // Create LIO IMUData 204 | auto imu_data = std::make_shared( 205 | timestamp, 206 | Eigen::Vector3f(msg->linear_acceleration.x, 207 | msg->linear_acceleration.y, 208 | msg->linear_acceleration.z), 209 | Eigen::Vector3f(msg->angular_velocity.x, 210 | msg->angular_velocity.y, 211 | msg->angular_velocity.z) 212 | ); 213 | 214 | // Gravity initialization phase: collect IMU samples 215 | if (!gravity_initialized_) { 216 | std::lock_guard lock(init_mutex_); 217 | init_imu_buffer_.push_back(*imu_data); 218 | 219 | if (init_imu_buffer_.size() >= static_cast(init_imu_samples_)) { 220 | // Perform gravity initialization 221 | RCLCPP_INFO(this->get_logger(), 222 | "🔧 Starting gravity initialization with %zu IMU samples...", 223 | init_imu_buffer_.size()); 224 | 225 | if (estimator_->GravityInitialization(init_imu_buffer_)) { 226 | gravity_initialized_ = true; 227 | RCLCPP_INFO(this->get_logger(), "Gravity initialization successful!"); 228 | RCLCPP_INFO(this->get_logger(), "LIO estimator is now ready to process data"); 229 | } else { 230 | RCLCPP_ERROR(this->get_logger(), "Gravity initialization failed!"); 231 | init_imu_buffer_.clear(); // Reset and try again 232 | } 233 | } else { 234 | // Progress update every 50 samples 235 | if (init_imu_buffer_.size() % 50 == 0) { 236 | RCLCPP_INFO(this->get_logger(), "Collecting IMU samples: %zu/%d", 237 | init_imu_buffer_.size(), init_imu_samples_); 238 | } 239 | } 240 | return; // Don't push to queue until initialized 241 | } 242 | 243 | // Push to event queue (automatically sorted by timestamp) 244 | event_queue_.pushIMU(timestamp, imu_data); 245 | 246 | RCLCPP_DEBUG(this->get_logger(), "IMU #%ld @ %.6f - Queue size: %zu", 247 | imu_count_, timestamp, event_queue_.size()); 248 | } 249 | 250 | // LiDAR Callback: Lightweight - just convert and push to queue 251 | void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 252 | { 253 | if (!gravity_initialized_) { 254 | RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, 255 | "Waiting for gravity initialization before processing LiDAR data..."); 256 | return; // Skip LiDAR until initialized 257 | } 258 | 259 | lidar_count_++; 260 | 261 | double timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; 262 | 263 | // Convert ROS PointCloud2 to LIO PointCloud 264 | auto cloud = std::make_shared(); 265 | cloud->reserve(msg->width * msg->height); 266 | 267 | // Create iterators for x, y, z, intensity, offset_time 268 | sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); 269 | sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); 270 | sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); 271 | 272 | // Check if intensity field exists 273 | bool has_intensity = false; 274 | for (const auto& field : msg->fields) { 275 | if (field.name == "intensity") { 276 | has_intensity = true; 277 | break; 278 | } 279 | } 280 | 281 | // Check if offset_time field exists 282 | bool has_offset_time = false; 283 | for (const auto& field : msg->fields) { 284 | if (field.name == "offset_time") { 285 | has_offset_time = true; 286 | break; 287 | } 288 | } 289 | 290 | if (has_intensity && has_offset_time) { 291 | sensor_msgs::PointCloud2ConstIterator iter_intensity(*msg, "intensity"); 292 | sensor_msgs::PointCloud2ConstIterator iter_offset_time(*msg, "offset_time"); 293 | 294 | for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_offset_time) { 295 | Point3D point; 296 | point.x = *iter_x; 297 | point.y = *iter_y; 298 | point.z = *iter_z; 299 | point.intensity = *iter_intensity; 300 | point.offset_time = static_cast(static_cast(*iter_offset_time) / 1e9); // uint32 (ns) -> double -> float (sec) 301 | cloud->push_back(point); 302 | } 303 | } else if (has_intensity) { 304 | sensor_msgs::PointCloud2ConstIterator iter_intensity(*msg, "intensity"); 305 | 306 | for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_intensity) { 307 | Point3D point; 308 | point.x = *iter_x; 309 | point.y = *iter_y; 310 | point.z = *iter_z; 311 | point.intensity = *iter_intensity; 312 | point.offset_time = 0.0f; // No offset time 313 | cloud->push_back(point); 314 | } 315 | } else { 316 | // No intensity, no offset_time 317 | for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { 318 | Point3D point; 319 | point.x = *iter_x; 320 | point.y = *iter_y; 321 | point.z = *iter_z; 322 | point.intensity = 0.0f; 323 | point.offset_time = 0.0f; 324 | cloud->push_back(point); 325 | } 326 | } 327 | 328 | // Create LidarData 329 | auto lidar_data = std::make_shared(timestamp, cloud); 330 | 331 | // Push to event queue (automatically sorted by timestamp) 332 | event_queue_.pushLiDAR(timestamp, lidar_data); 333 | 334 | RCLCPP_DEBUG(this->get_logger(), "LiDAR #%ld @ %.6f - Points: %zu, Queue size: %zu", 335 | lidar_count_, timestamp, cloud->size(), event_queue_.size()); 336 | 337 | size_t queue_size = event_queue_.size(); 338 | if (queue_size > 100) { 339 | RCLCPP_WARN(this->get_logger(), 340 | "Event queue size: %zu (processing may be slower than incoming data)", 341 | queue_size); 342 | } 343 | } 344 | 345 | // Processing Thread: Pop events in timestamp order and process 346 | void processingThreadLoop() { 347 | RCLCPP_INFO(this->get_logger(), "Processing thread started"); 348 | 349 | size_t processed_count = 0; 350 | size_t lidar_processed = 0; 351 | size_t imu_processed = 0; 352 | 353 | double last_timestamp = -1.0; // For timestamp ordering check 354 | 355 | while (running_) { 356 | // Pop oldest event (blocking) 357 | auto event_opt = event_queue_.pop(); 358 | 359 | if (!event_opt.has_value()) { 360 | break; // Queue shutdown 361 | } 362 | 363 | SensorEvent event = event_opt.value(); 364 | processed_count++; 365 | 366 | // Check timestamp ordering (only warn if violation > 10ms) 367 | if (last_timestamp >= 0.0 && event.timestamp < last_timestamp) { 368 | double diff = event.timestamp - last_timestamp; 369 | if (diff < -0.01) { // -10ms threshold 370 | RCLCPP_ERROR(this->get_logger(), 371 | "TIMESTAMP ORDER VIOLATION! last=%.9f, current=%.9f (diff=%.9f)", 372 | last_timestamp, event.timestamp, diff); 373 | } else { 374 | RCLCPP_DEBUG(this->get_logger(), 375 | "Minor timestamp reordering: last=%.9f, current=%.9f (diff=%.9f)", 376 | last_timestamp, event.timestamp, diff); 377 | } 378 | } 379 | last_timestamp = event.timestamp; 380 | 381 | try { 382 | if (event.type == SensorType::IMU) { 383 | // Process IMU (propagate state) 384 | estimator_->ProcessIMU(*event.imu_data); 385 | imu_processed++; 386 | 387 | RCLCPP_DEBUG(this->get_logger(), "[%zu] IMU @ %.9f | acc=[%.3f, %.3f, %.3f] gyr=[%.3f, %.3f, %.3f]", 388 | processed_count, event.timestamp, 389 | event.imu_data->acc.x(), 390 | event.imu_data->acc.y(), 391 | event.imu_data->acc.z(), 392 | event.imu_data->gyr.x(), 393 | event.imu_data->gyr.y(), 394 | event.imu_data->gyr.z()); 395 | 396 | } else { 397 | // Process LiDAR (scan matching + update) 398 | estimator_->ProcessLidar(*event.lidar_data); 399 | lidar_processed++; 400 | 401 | RCLCPP_DEBUG(this->get_logger(), 402 | "[%zu] LiDAR @ %.9f | Points: %zu | Queue: %zu", 403 | processed_count, event.timestamp, event.lidar_data->cloud->size(), 404 | event_queue_.size()); 405 | 406 | // Get current state 407 | State current_state = estimator_->GetCurrentState(); 408 | 409 | // Publish pose and odometry immediately after LiDAR update (synchronized) 410 | publishPoseOnly(current_state, event.timestamp); 411 | publishOdometry(current_state, event.timestamp); 412 | 413 | // Publish raw scan immediately (if there are subscribers) 414 | if (raw_scan_pub_->get_subscription_count() > 0) { 415 | rclcpp::Time ros_time(static_cast(event.timestamp * 1e9)); 416 | publishRawScan(event.lidar_data->cloud, current_state, ros_time); 417 | } 418 | 419 | // Prepare result for publisher thread (other visualizations) 420 | LIOProcessingResult result; 421 | result.success = true; 422 | result.timestamp = event.timestamp; 423 | result.state = current_state; 424 | result.processed_cloud = estimator_->GetProcessedCloud(); 425 | result.raw_cloud = nullptr; // Already published in processing thread 426 | result_queue_.push(result); 427 | } 428 | 429 | } catch (const std::exception& e) { 430 | RCLCPP_ERROR(this->get_logger(), 431 | "Exception in LIO processing: %s", e.what()); 432 | } 433 | } 434 | 435 | RCLCPP_INFO(this->get_logger(), 436 | "Processing thread stopped (processed %zu events: %zu IMU, %zu LiDAR)", 437 | processed_count, imu_processed, lidar_processed); 438 | } 439 | 440 | // Publisher Thread: Publish ROS topics 441 | void publisherThreadLoop() { 442 | RCLCPP_INFO(this->get_logger(), "Publisher thread started"); 443 | 444 | size_t published_count = 0; 445 | 446 | while (running_) { 447 | auto result_opt = result_queue_.pop(); 448 | 449 | if (!result_opt.has_value()) { 450 | break; // Queue shutdown 451 | } 452 | 453 | LIOProcessingResult result = result_opt.value(); 454 | 455 | if (result.success) { 456 | // Odometry is now published in processing thread (synchronized with pose) 457 | // Only publish visualization here 458 | publishVisualization(result); 459 | 460 | published_count++; 461 | 462 | RCLCPP_DEBUG(this->get_logger(), "Published frame #%zu", published_count); 463 | } 464 | } 465 | 466 | RCLCPP_INFO(this->get_logger(), 467 | "Publisher thread stopped (published %zu frames)", 468 | published_count); 469 | } 470 | 471 | // Publish pose only (called at IMU rate ~100Hz) 472 | void publishPoseOnly(const State& state, double timestamp) 473 | { 474 | rclcpp::Time ros_time(static_cast(timestamp * 1e9)); 475 | 476 | Eigen::Quaternionf q(state.m_rotation); 477 | q.normalize(); 478 | 479 | auto pose_msg = geometry_msgs::msg::PoseStamped(); 480 | pose_msg.header.stamp = ros_time; 481 | pose_msg.header.frame_id = "map"; 482 | 483 | pose_msg.pose.position.x = state.m_position.x(); 484 | pose_msg.pose.position.y = state.m_position.y(); 485 | pose_msg.pose.position.z = state.m_position.z(); 486 | 487 | pose_msg.pose.orientation.x = q.x(); 488 | pose_msg.pose.orientation.y = q.y(); 489 | pose_msg.pose.orientation.z = q.z(); 490 | pose_msg.pose.orientation.w = q.w(); 491 | 492 | pose_pub_->publish(pose_msg); 493 | } 494 | 495 | void publishOdometry(const State& state, double timestamp) 496 | { 497 | // Create ROS timestamp 498 | rclcpp::Time ros_time(static_cast(timestamp * 1e9)); 499 | 500 | // Convert rotation matrix to quaternion 501 | Eigen::Quaternionf q(state.m_rotation); 502 | q.normalize(); 503 | 504 | // Publish Odometry 505 | auto odom_msg = nav_msgs::msg::Odometry(); 506 | odom_msg.header.stamp = ros_time; 507 | odom_msg.header.frame_id = "map"; 508 | odom_msg.child_frame_id = "base_link"; 509 | 510 | odom_msg.pose.pose.position.x = state.m_position.x(); 511 | odom_msg.pose.pose.position.y = state.m_position.y(); 512 | odom_msg.pose.pose.position.z = state.m_position.z(); 513 | 514 | odom_msg.pose.pose.orientation.x = q.x(); 515 | odom_msg.pose.pose.orientation.y = q.y(); 516 | odom_msg.pose.pose.orientation.z = q.z(); 517 | odom_msg.pose.pose.orientation.w = q.w(); 518 | 519 | // Linear velocity (world frame) 520 | odom_msg.twist.twist.linear.x = state.m_velocity.x(); 521 | odom_msg.twist.twist.linear.y = state.m_velocity.y(); 522 | odom_msg.twist.twist.linear.z = state.m_velocity.z(); 523 | 524 | // Angular velocity (world frame) 525 | // Calculate from rotation difference: omega = log(R_prev^T * R_curr) / dt 526 | if (!first_odom_frame_ && (timestamp - prev_timestamp_) > 1e-6) { 527 | double dt = timestamp - prev_timestamp_; 528 | 529 | // Compute relative rotation: dR = R_prev^T * R_curr 530 | Eigen::Matrix3f dR = prev_rotation_.transpose() * state.m_rotation; 531 | 532 | // Convert rotation matrix to axis-angle representation 533 | Eigen::AngleAxisf angle_axis(dR); 534 | float angle = angle_axis.angle(); 535 | Eigen::Vector3f axis = angle_axis.axis(); 536 | 537 | // Angular velocity in world frame: omega = (R_prev * axis) * (angle / dt) 538 | Eigen::Vector3f omega_world = (prev_rotation_ * axis) * (angle / dt); 539 | 540 | odom_msg.twist.twist.angular.x = omega_world.x(); 541 | odom_msg.twist.twist.angular.y = omega_world.y(); 542 | odom_msg.twist.twist.angular.z = omega_world.z(); 543 | } else { 544 | // First frame or dt too small, set angular velocity to zero 545 | odom_msg.twist.twist.angular.x = 0.0; 546 | odom_msg.twist.twist.angular.y = 0.0; 547 | odom_msg.twist.twist.angular.z = 0.0; 548 | first_odom_frame_ = false; 549 | } 550 | 551 | // Update previous state for next iteration 552 | prev_rotation_ = state.m_rotation; 553 | prev_timestamp_ = timestamp; 554 | 555 | odom_pub_->publish(odom_msg); 556 | 557 | // Broadcast TF: map -> base_link 558 | geometry_msgs::msg::TransformStamped transform; 559 | transform.header.stamp = ros_time; 560 | transform.header.frame_id = "map"; 561 | transform.child_frame_id = "base_link"; 562 | 563 | transform.transform.translation.x = state.m_position.x(); 564 | transform.transform.translation.y = state.m_position.y(); 565 | transform.transform.translation.z = state.m_position.z(); 566 | 567 | transform.transform.rotation.x = q.x(); 568 | transform.transform.rotation.y = q.y(); 569 | transform.transform.rotation.z = q.z(); 570 | transform.transform.rotation.w = q.w(); 571 | 572 | tf_broadcaster_->sendTransform(transform); 573 | 574 | // Add to trajectory 575 | geometry_msgs::msg::PoseStamped pose_msg; 576 | pose_msg.header.stamp = ros_time; 577 | pose_msg.header.frame_id = "map"; 578 | pose_msg.pose.position.x = state.m_position.x(); 579 | pose_msg.pose.position.y = state.m_position.y(); 580 | pose_msg.pose.position.z = state.m_position.z(); 581 | pose_msg.pose.orientation.x = q.x(); 582 | pose_msg.pose.orientation.y = q.y(); 583 | pose_msg.pose.orientation.z = q.z(); 584 | pose_msg.pose.orientation.w = q.w(); 585 | 586 | trajectory_.poses.push_back(pose_msg); 587 | trajectory_.header.stamp = ros_time; 588 | trajectory_.header.frame_id = "map"; 589 | } 590 | 591 | void publishVisualization(const LIOProcessingResult& result) 592 | { 593 | rclcpp::Time ros_time(static_cast(result.timestamp * 1e9)); 594 | 595 | // 1. Publish processed scan (GREEN - downsampled) 596 | if (result.processed_cloud && !result.processed_cloud->empty()) { 597 | publishCurrentScan(result.processed_cloud, result.state, ros_time); 598 | } 599 | 600 | // 2. Raw scan already published in processing thread 601 | 602 | // 3. Publish map (RED) 603 | auto map_cloud = estimator_->GetMapPointCloud(); 604 | if (map_cloud && !map_cloud->empty()) { 605 | publishMapCloud(map_cloud, ros_time); 606 | } 607 | 608 | // 4. Publish trajectory 609 | trajectory_pub_->publish(trajectory_); 610 | } 611 | 612 | void publishCurrentScan(const PointCloudPtr& cloud, const State& state, const rclcpp::Time& timestamp) 613 | { 614 | // Transform cloud to world frame 615 | Eigen::Matrix4f T_wb = Eigen::Matrix4f::Identity(); 616 | T_wb.block<3, 3>(0, 0) = state.m_rotation; 617 | T_wb.block<3, 1>(0, 3) = state.m_position; 618 | 619 | sensor_msgs::msg::PointCloud2 cloud_msg; 620 | cloud_msg.header.stamp = timestamp; 621 | cloud_msg.header.frame_id = "map"; 622 | cloud_msg.height = 1; 623 | cloud_msg.width = cloud->size(); 624 | cloud_msg.is_dense = false; 625 | cloud_msg.is_bigendian = false; 626 | 627 | sensor_msgs::PointCloud2Modifier modifier(cloud_msg); 628 | modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); 629 | modifier.resize(cloud->size()); 630 | 631 | sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); 632 | sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); 633 | sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); 634 | sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); 635 | sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); 636 | sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); 637 | 638 | for (const auto& point : *cloud) { 639 | // Transform to world frame 640 | Eigen::Vector3f p_w = T_wb.block<3, 3>(0, 0) * Eigen::Vector3f(point.x, point.y, point.z) + T_wb.block<3, 1>(0, 3); 641 | 642 | *iter_x = p_w.x(); 643 | *iter_y = p_w.y(); 644 | *iter_z = p_w.z(); 645 | *iter_r = 0; 646 | *iter_g = 255; // Green 647 | *iter_b = 0; 648 | 649 | ++iter_x; ++iter_y; ++iter_z; 650 | ++iter_r; ++iter_g; ++iter_b; 651 | } 652 | 653 | current_scan_pub_->publish(cloud_msg); 654 | } 655 | 656 | void publishRawScan(const PointCloudPtr& cloud, const State& state, const rclcpp::Time& timestamp) 657 | { 658 | // Transform cloud to world frame 659 | Eigen::Matrix4f T_wb = Eigen::Matrix4f::Identity(); 660 | T_wb.block<3, 3>(0, 0) = state.m_rotation; 661 | T_wb.block<3, 1>(0, 3) = state.m_position; 662 | 663 | sensor_msgs::msg::PointCloud2 cloud_msg; 664 | cloud_msg.header.stamp = timestamp; 665 | cloud_msg.header.frame_id = "map"; 666 | cloud_msg.height = 1; 667 | cloud_msg.width = cloud->size(); 668 | cloud_msg.is_dense = false; 669 | cloud_msg.is_bigendian = false; 670 | 671 | sensor_msgs::PointCloud2Modifier modifier(cloud_msg); 672 | modifier.setPointCloud2FieldsByString(2, "xyz", "rgba"); 673 | modifier.resize(cloud->size()); 674 | 675 | sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); 676 | sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); 677 | sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); 678 | sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); 679 | sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); 680 | sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); 681 | sensor_msgs::PointCloud2Iterator iter_a(cloud_msg, "a"); 682 | 683 | for (const auto& point : *cloud) { 684 | // Transform to world frame 685 | Eigen::Vector3f p_w = T_wb.block<3, 3>(0, 0) * Eigen::Vector3f(point.x, point.y, point.z) + T_wb.block<3, 1>(0, 3); 686 | 687 | *iter_x = p_w.x(); 688 | *iter_y = p_w.y(); 689 | *iter_z = p_w.z(); 690 | *iter_r = 0; 691 | *iter_g = 0; 692 | *iter_b = 255; // Blue (raw scan) 693 | *iter_a = 26; // Alpha = 26/255 ≈ 0.1 694 | 695 | ++iter_x; ++iter_y; ++iter_z; 696 | ++iter_r; ++iter_g; ++iter_b; ++iter_a; 697 | } 698 | 699 | raw_scan_pub_->publish(cloud_msg); 700 | } 701 | 702 | void publishMapCloud(const PointCloudPtr& cloud, const rclcpp::Time& timestamp) 703 | { 704 | sensor_msgs::msg::PointCloud2 cloud_msg; 705 | cloud_msg.header.stamp = timestamp; 706 | cloud_msg.header.frame_id = "map"; 707 | cloud_msg.height = 1; 708 | cloud_msg.width = cloud->size(); 709 | cloud_msg.is_dense = false; 710 | cloud_msg.is_bigendian = false; 711 | 712 | sensor_msgs::PointCloud2Modifier modifier(cloud_msg); 713 | modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); 714 | modifier.resize(cloud->size()); 715 | 716 | sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); 717 | sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); 718 | sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); 719 | sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); 720 | sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); 721 | sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); 722 | 723 | for (const auto& point : *cloud) { 724 | *iter_x = point.x; 725 | *iter_y = point.y; 726 | *iter_z = point.z; 727 | *iter_r = 255; // Red (map) 728 | *iter_g = 0; 729 | *iter_b = 0; 730 | 731 | ++iter_x; ++iter_y; ++iter_z; 732 | ++iter_r; ++iter_g; ++iter_b; 733 | } 734 | 735 | map_cloud_pub_->publish(cloud_msg); 736 | } 737 | 738 | // Thread-safe queue for results 739 | class ResultQueue { 740 | public: 741 | void push(const LIOProcessingResult& result) { 742 | { 743 | std::lock_guard lock(mutex_); 744 | queue_.push(result); 745 | } 746 | cv_.notify_one(); 747 | } 748 | 749 | std::optional pop() { 750 | std::unique_lock lock(mutex_); 751 | cv_.wait(lock, [this]() { return !queue_.empty() || shutdown_; }); 752 | 753 | if (shutdown_ && queue_.empty()) { 754 | return std::nullopt; 755 | } 756 | 757 | LIOProcessingResult result = queue_.front(); 758 | queue_.pop(); 759 | return result; 760 | } 761 | 762 | size_t size() const { 763 | std::lock_guard lock(mutex_); 764 | return queue_.size(); 765 | } 766 | 767 | void shutdown() { 768 | { 769 | std::lock_guard lock(mutex_); 770 | shutdown_ = true; 771 | } 772 | cv_.notify_all(); 773 | } 774 | 775 | private: 776 | std::queue queue_; 777 | mutable std::mutex mutex_; 778 | std::condition_variable cv_; 779 | bool shutdown_ = false; 780 | }; 781 | 782 | // Subscribers 783 | rclcpp::Subscription::SharedPtr imu_sub_; 784 | rclcpp::Subscription::SharedPtr lidar_sub_; 785 | 786 | // Publishers 787 | rclcpp::Publisher::SharedPtr odom_pub_; 788 | rclcpp::Publisher::SharedPtr pose_pub_; 789 | rclcpp::Publisher::SharedPtr current_scan_pub_; 790 | rclcpp::Publisher::SharedPtr raw_scan_pub_; 791 | rclcpp::Publisher::SharedPtr map_cloud_pub_; 792 | rclcpp::Publisher::SharedPtr trajectory_pub_; 793 | 794 | // TF broadcaster 795 | std::shared_ptr tf_broadcaster_; 796 | 797 | // Trajectory storage 798 | nav_msgs::msg::Path trajectory_; 799 | 800 | // LIO Estimator 801 | std::shared_ptr estimator_; 802 | 803 | // Configuration 804 | LIOConfig config_; 805 | int init_imu_samples_; 806 | 807 | // Gravity initialization 808 | bool gravity_initialized_; 809 | std::vector init_imu_buffer_; 810 | std::mutex init_mutex_; 811 | 812 | // Event queue (timestamp-sorted) 813 | SensorEventQueue event_queue_; 814 | ResultQueue result_queue_; 815 | 816 | // Worker threads 817 | std::thread processing_thread_; 818 | std::thread publisher_thread_; 819 | std::atomic running_; 820 | 821 | // Counters 822 | size_t imu_count_; 823 | size_t lidar_count_; 824 | 825 | // Previous state for angular velocity calculation 826 | Eigen::Matrix3f prev_rotation_; 827 | double prev_timestamp_; 828 | bool first_odom_frame_; 829 | }; 830 | 831 | int main(int argc, char** argv) 832 | { 833 | rclcpp::init(argc, argv); 834 | auto node = std::make_shared(); 835 | rclcpp::spin(node); 836 | rclcpp::shutdown(); 837 | return 0; 838 | } 839 | --------------------------------------------------------------------------------