Livox_mapping is a ROS 2 package for 3D SLAM using Livox LiDARs, based on the LOAM algorithm. It provides real-time odometry and mapping capabilities. This version has been adapted for ROS 2 and includes initial support for various Livox models including the Mid360.
This package aims to provide robust LOAM-based SLAM for Livox Lidars.
- Based on the well-known LOAM algorithm.
- Supports various Livox LiDAR models, with specific configurations for Mid-series and Horizon, and initial support for Mid360.
- Provides options for different feature extraction strategies.
- Includes
livox_repubnode for converting LivoxCustomMsgtopcl::PointCloud<PointXYZINormal>with timestamp information crucial for motion compensation.
Original goals from the ROS 1 version included:
- Support for multiple Livox LiDAR models (ongoing with ROS 2).
- Specialized feature extraction for Livox LiDAR patterns.
- Odometry removal for small FOV situations (this may refer to specific algorithm tweaks within LOAM).
- Ubuntu 22.04 (or compatible)
- ROS 2 (e.g., Humble, Jazzy - please adapt to your ROS 2 version)
- C++17 compiler (as specified in
CMakeLists.txt)
- PCL (Point Cloud Library): Version 1.10 or higher. Installation instructions: PCL Downloads.
sudo apt-get update sudo apt-get install libpcl-dev
- Eigen3: Version 3.3 or higher. Usually installed with PCL or ROS. If not:
sudo apt-get install libeigen3-dev
- OpenCV: Version 4.x. Usually installed with ROS Desktop Full. If not:
sudo apt-get install libopencv-dev python3-opencv
- OpenMP: (Optional, for parallelization) - Usually available with GCC.
- livox_ros_driver2: Ensure you have installed
livox_ros_driver2for your specific Livox LiDAR model and ROS 2 version. Follow instructions from livox_ros_driver2 GitHub. - Other ROS 2 packages (will be installed via
rosdep):rclcpp,rclpy,std_msgs,sensor_msgs,geometry_msgs,nav_msgs,tf2,tf2_ros,tf2_geometry_msgs,pcl_conversions,ament_cmake.
-
Create/Navigate to your ROS 2 Workspace:
mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src
-
Clone the Repository:
git clone https://github.com/Livox-SDK/livox_mapping.git # Or your specific fork/branch -
Install Dependencies:
cd ~/ros2_ws sudo apt-get update rosdep install --from-paths src --ignore-src -r -y
This command installs ROS dependencies listed in
package.xml. Ensurerosdepis initialized (sudo rosdep initandrosdep update) if you haven't used it before. -
Build the Package:
colcon build --symlink-install
-
Source the Workspace:
source ~/ros2_ws/install/setup.bash # Or add this to your .bashrc for convenience
Remarks:
- If you want to save the PCD map file, ensure the
map_file_pathparameter in the launch file or C++ node points to a valid directory with write permissions. The default inmapping_mid.launch.pyis " " (an empty space string), which likely means thelaserMappingnode will use an internal default or not save unless specified.
The livox_mapping package consists of several key nodes that work together in a LOAM pipeline:
livox_repub(Executable:livox_repub)- Purpose: Subscribes to raw Livox custom messages (
livox_ros_driver2::msg::CustomMsg), aggregates them, and republishes as a standard ROS 2 point cloud (sensor_msgs::msg::PointCloud2). - Point Type: Converts Livox points to
pcl::PointXYZI. It populates:intensity: Encodes Livox line number (integer part) and reflectivity (decimal part).- The
curvaturefield is no longer populated with a timestamp. Timestamps are primarily handled by the message header.
- Purpose: Subscribes to raw Livox custom messages (
scanRegistration(Executable:loam_scanRegistration)- Purpose: Processes point clouds from
livox_repub(or directly from a driver if configured) to extract geometric features: sharp edges (corners) and planar surfaces. This version is generally applicable to various Livox scan patterns. - Output: Publishes feature clouds (
/laser_cloud_sharp,/laser_cloud_flat). - Point Type: Uses
pcl::PointXYZI. It preservesintensity(line number + reflectivity) from the input cloud.
- Purpose: Processes point clouds from
scanRegistration_horizon(Executable:loam_scanRegistration_horizon)- Purpose: An alternative feature extraction node, potentially optimized for Livox Horizon or other multi-line Lidars where
N_SCANSis a meaningful parameter. - Point Type: Uses
pcl::PointXYZIand preservesintensityfrom the input cloud.
- Purpose: An alternative feature extraction node, potentially optimized for Livox Horizon or other multi-line Lidars where
laserMapping(Executable:loam_laserMapping)- Purpose: Performs scan-to-map matching using the extracted features. It estimates the LiDAR's pose, and builds/updates a 3D map.
- Point Type: Expects input feature clouds with
pcl::PointXYZIpoints. Motion distortion compensation that previously relied on acurvaturefield inpointAssociateToMap_allis currently not active (timestamps are per-message).
Recommended Data Flow for Accurate Mapping:
livox_ros_driver2 -> livox_repub (outputs /livox_pcl0 with pcl::PointXYZI) -> loam_scanRegistration OR loam_scanRegistration_horizon (consumes /livox_pcl0, outputs feature clouds with pcl::PointXYZI) -> loam_laserMapping.
The choice between loam_scanRegistration and loam_scanRegistration_horizon depends on the LiDAR model and scan pattern. loam_scanRegistration is more generic.
Parameters can be set via launch files or command-line arguments.
to_merge_count(int, default:1in C++ code): Number of inputlivox_ros_driver2::msg::CustomMsgmessages to accumulate before publishing an aggregated point cloud. Can be set in its launch file if exposed.
n_scans(int, default:6in C++ code): Number of scan lines/rings assumed for feature extraction. This should be set according to your LiDAR characteristics or howlivox_repubencodes line information into the intensity field.
map_file_path(string, default:" "): Path to save the generated PCD map files. If left as an empty space or not specified, saving might be disabled or use an internal default.filter_parameter_corner(double, default:0.1inmapping_mid.launch.py): Voxel grid leaf size for downsampling corner point clouds. (Node default is 0.2)filter_parameter_surf(double, default:0.2inmapping_mid.launch.py): Voxel grid leaf size for downsampling surface point clouds. (Node default is 0.4)
(Note: Default values for laserMapping parameters in C++ source may differ from launch files. Launch file values take precedence if set.)
This section describes common topics. Actual topic names for feature clouds might vary if using _horizon versions.
- Subscribes:
/livox/lidar(livox_ros_driver2::msg::CustomMsg): Raw Livox point data.
- Publishes:
/livox_pcl0(sensor_msgs::msg::PointCloud2): Processed cloud withpcl::PointXYZINormalpoints.
- Subscribes:
/livox_pcl0(sensor_msgs::msg::PointCloud2): Input point cloud fromlivox_repub.
- Publishes:
/livox_cloud(or/livox_cloud_horizon) (sensor_msgs::msg::PointCloud2): Input cloud after some processing./laser_cloud_sharp(or/laser_cloud_sharp_horizon) (sensor_msgs::msg::PointCloud2): Corner feature points./laser_cloud_flat(or/laser_cloud_flat_horizon) (sensor_msgs::msg::PointCloud2): Surface feature points.
- Subscribes: (Topic names depend on which scanRegistration is used)
/laser_cloud_sharp(or/laser_cloud_sharp_horizon)/laser_cloud_flat(or/laser_cloud_flat_horizon)/livox_cloud(or/livox_cloud_horizon)
- Publishes:
/laser_cloud_surround(sensor_msgs::msg::PointCloud2): Local map cloud./laser_cloud_surround_corner(sensor_msgs::msg::PointCloud2): Corner points from local map./velodyne_cloud_registered(sensor_msgs::msg::PointCloud2): Registered (deskewed) current scan./aft_mapped_to_init(nav_msgs::msg::Odometry): LiDAR odometry in map frame.
No ROS 2 services are explicitly defined by these nodes in the reviewed C++ code.
This feature is designed to help diagnose potential issues within the SLAM pipeline by providing warnings when key operational metrics fall outside expected ranges. It works by logging RCLCPP_WARN messages when these metrics cross pre-configured thresholds.
A master switch, health.enable_health_warnings (boolean, default: true), is available for both the feature extraction (scanRegistration/scanRegistration_horizon) and laserMapping nodes. Setting this to false will suppress all health-related warnings from that specific node.
These parameters control warnings related to the feature extraction process. They are applicable to both loam_scanRegistration and loam_scanRegistration_horizon executables.
health.min_raw_points_for_feature_extraction(int, default: 10)- Purpose: Minimum number of points required in the input cloud (after basic filtering like NaN removal) to proceed with feature extraction.
- Warning Implication: A warning indicates that the input cloud is too sparse. This could be due to issues with the LiDAR, driver, or
livox_repubnode, or an extremely sparse environment.
health.min_sharp_features(int, default: 15)- Purpose: Minimum number of sharp (corner) features to be extracted.
- Warning Implication: Low sharp feature count might suggest a geometrically und-diverse environment (e.g., long corridors, open fields), or that the LiDAR data is not conducive to strong corner detection (e.g., noisy data, insufficient point density on edges).
health.min_flat_features(int, default: 40)- Purpose: Minimum number of flat (surface) features to be extracted.
- Warning Implication: Similar to sharp features, a low count for flat features can indicate a lack of planar surfaces in the environment or issues with point cloud quality for surface fitting.
These parameters control warnings related to the scan-to-map matching and map update process in the loam_laserMapping node.
health.min_downsampled_corner_features(int, default: 12)- Purpose: Minimum number of corner features from the current scan after voxel grid downsampling, before attempting ICP.
- Warning Implication: Very few corner features post-downsampling might lead to poor constraints for ICP, especially rotation.
health.min_downsampled_surf_features(int, default: 30)- Purpose: Minimum number of surface features from the current scan after voxel grid downsampling.
- Warning Implication: Insufficient surface features can weaken the ICP solution, particularly for translation.
health.min_map_corner_points_for_icp(int, default: 30)- Purpose: Minimum number of corner points retrieved from the local map to be used as target points for ICP.
- Warning Implication: If the local map doesn't have enough corner points in the vicinity of the current scan, ICP might be unreliable or skipped. This could indicate poor localization or an unexplored area.
health.min_map_surf_points_for_icp(int, default: 100)- Purpose: Minimum number of surface points retrieved from the local map for ICP.
- Warning Implication: Similar to map corner points, a low count here suggests an insufficient local map for robust surface feature matching.
health.min_icp_correspondences(int, default: 40)- Purpose: Minimum number of selected point correspondences (both corner and surface) used in the ICP optimization step.
- Warning Implication: Fewer correspondences than this threshold (but above the critical minimum of 50 which skips optimization) suggest a weak geometric link between the current scan and the map, potentially leading to less accurate pose updates.
health.max_icp_delta_rotation_deg(double, default: 5.0)- Purpose: Maximum rotational correction (in degrees) applied by a single ICP iteration.
- Warning Implication: A very large rotational correction can indicate unstable tracking, a jump in localization, or issues with initial pose prediction.
health.max_icp_delta_translation_cm(double, default: 20.0)- Purpose: Maximum translational correction (in centimeters) applied by a single ICP iteration.
- Warning Implication: Similar to large rotational corrections, significant translational jumps suggest instability or poor prior pose estimates.
health.warn_on_icp_degeneracy(bool, default: true)- Purpose: Whether to log a warning if the ICP optimization encounters a degenerate geometry (e.g., trying to solve for translation along a corridor where only rotation is well-constrained).
- Warning Implication: Indicates that the current scan and map geometry do not provide enough constraints for a full 6-DOF pose update, potentially leading to drift in certain directions.
You can adjust these health monitoring thresholds via launch arguments in the Python launch files. For example, in mapping_mid.launch.py (or mapping_horizon_launch.py, mapping_mid360_launch.py, mapping_outdoor_launch.py):
-
Declare the launch argument:
# In the launch file, e.g., mapping_mid.launch.py declare_sr_health_min_sharp_features_arg = DeclareLaunchArgument( 'sr_health_min_sharp_features', default_value='20', # Default is 20 description='Min sharp features in scanRegistration' ) declare_lm_health_min_icp_correspondences_arg = DeclareLaunchArgument( 'lm_health_min_icp_correspondences', default_value='75', # Default is 75 description='Min ICP correspondences in laserMapping' )
-
Pass it to the node:
# For scan_registration_node parameters=[ {'health.min_sharp_features': LaunchConfiguration('sr_health_min_sharp_features')}, # ... other sr params ] # For laser_mapping_node parameters=[ # ... other lm params like markers_icp_corr {'health.min_icp_correspondences': LaunchConfiguration('lm_health_min_icp_correspondences')}, # ... other lm health params ]
-
Add the declared argument to the
LaunchDescriptionlist:ld.add_action(declare_sr_health_min_sharp_features_arg) ld.add_action(declare_lm_health_min_icp_correspondences_arg)
-
Override from the command line when launching:
ros2 launch livox_mapping mapping_mid.launch.py sr_health_min_sharp_features:=15 lm_health_min_icp_correspondences:=60
Feedback from users, such as experiences in underground carparks, has highlighted scenarios where default feature detection thresholds might be too high. In such geometrically challenging environments (e.g., feature-poor, repetitive structures), if you observe frequent warnings about low feature counts (e.g., "Number of sharp features (X) is below threshold (Y)"), you might consider:
- Reducing
sr_health_min_sharp_featuresorsr_health_min_flat_features. - Subsequently, you might also need to adjust
lm_health_min_downsampled_corner_features,lm_health_min_downsampled_surf_features, andlm_health_min_icp_correspondencesif the input tolaserMappingis consistently lower in features. Lowering these thresholds can help the system continue tracking in sparse environments, but be mindful that it might also make the system more susceptible to incorrect matches if set too low. Always monitor the output odometry and map quality after tuning.
- Start
livox_ros_driver2: Use the appropriate launch file for your LiDAR model (e.g., Mid-40, Mid360, Horizon, Avia). This publishes the rawlivox_ros_driver2::msg::CustomMsg.# Terminal 1: Source workspace, launch Livox driver source ~/ros2_ws/install/setup.bash ros2 launch livox_ros_driver2 <your_lidar_driver_launch.py>
- Start
livox_mapping: Use the corresponding launch file for your setup.# Terminal 2: Source workspace, launch livox_mapping source ~/ros2_ws/install/setup.bash ros2 launch livox_mapping <your_chosen_mapping_launch.launch.py> rviz:=true
This launch file uses loam_scanRegistration (generic feature extraction).
# Terminal 1 (Livox Driver - example for Mid-40):
# source ~/ros2_ws/install/setup.bash
# ros2 launch livox_ros_driver2 livox_lidar_launch.py # Adjust to your specific driver launch for Mid-40/70
# Terminal 2 (Livox Mapping):
source ~/ros2_ws/install/setup.bash
ros2 launch livox_mapping mapping_mid.launch.py rviz:=trueThis launch file starts livox_repub, then loam_scanRegistration (generic feature extraction), then loam_laserMapping.
# Terminal 1 (Livox Driver - example for Mid360):
# source ~/ros2_ws/install/setup.bash
# ros2 launch livox_ros_driver2 msg_MID360_launch.py # Or your specific Mid360 driver launch
# Terminal 2 (Livox Mapping):
source ~/ros2_ws/install/setup.bash
ros2 launch livox_mapping mapping_mid360.launch.py rviz:=trueNote for Mid360: The loam_scanRegistration node is used for its generality. Performance with Mid360's unique scan pattern should be verified. Parameter tuning in laserMapping (filter sizes) might be necessary.
The original mapping_horizon.launch was XML. A mapping_horizon.launch.py would need to be created, likely launching livox_repub, loam_scanRegistration_horizon (setting the n_scans parameter appropriately for Horizon), and loam_laserMapping.
Important Notes on Launching (General):
- Ensure the feature extraction pipeline consistently uses
pcl::PointXYZI. - The
*_launch.pyfiles provided are examples. You may need to adapt them (e.g., to useloam_scanRegistration_horizoninstead ofloam_scanRegistrationinmapping_mid.launch.pyif preferred) or create new ones for different LiDARs or configurations.
The launch files typically start RViz with rviz_cfg/loam_livox.rviz. You should see:
- TF frames (
camera_init,aft_mapped). - Published point clouds (map, features, registered scan).
- Odometry path.
The original README provided links to ROS 1 bag files. These can be converted to ROS 2 format or you can record new ROS 2 bags.
Example with a ROS 2 bag:
- Launch the mapping system:
source ~/ros2_ws/install/setup.bash # Choose the appropriate launch file for your Lidar/bag data ros2 launch livox_mapping mapping_mid360.launch.py rviz:=true
- Play the ROS 2 Bag:
(Links to original ROS 1 bags for reference: mid40_hall_example, mid40_outdoor, mid100_example, horizon_parking, horizon_outdoor)
# In another terminal: source ~/ros2_ws/install/setup.bash ros2 bag play YOUR_ROS2_BAG_DIRECTORY # Add --remap if topic names in the bag differ from those expected by the nodes. # e.g., ros2 bag play YOUR_ROS2_BAG_DIRECTORY --remap /livox_lidar_msg_from_bag:=/livox/lidar # (The /livox/lidar topic is consumed by livox_repub_node)
- PointType Consistency: All nodes in the processing pipeline (
livox_repub,scanRegistrationorscanRegistration_horizon,laserMapping) now consistently usepcl::PointXYZI. Thecurvaturefield is no longer used for propagating normalized timestamp information. Timestamps are handled at the message level. - Motion Distortion Compensation in
laserMapping: ThelaserMappingnode'spointAssociateToMap_allfunction previously used thecurvaturefield for motion deskewing within a scan. As this field is no longer available for that purpose, this specific per-point motion compensation is not currently active. Overall mapping relies on per-message timestamps. - TF Frames: The transform
camera_init->aft_mappedpublished bylaserMappingmay need adjustments in its RPY mapping depending on your specific robot configuration and desired ROS coordinate frame conventions (e.g., REP-103/REP-105). Visual verification in RViz is crucial. - Livox Mid360: The provided
mapping_mid360.launch.pyuses the genericloam_scanRegistrationnode. The performance and feature extraction quality with Mid360's unique scan pattern should be carefully evaluated. Parameters withinlaserMapping(e.g., filter sizes) or the choice of feature extractor might require tuning for optimal results with Mid360.
To enhance the robustness and performance of the LOAM pipeline under varying conditions, an online data-driven method for automatically adjusting pipeline parameters is proposed. This method is encapsulated by a conceptual AdaptiveParameterManager node.
The AdaptiveParameterManager aims to:
- Maintain healthy LOAM operation by dynamically tuning key parameters of the
laserMappingnode. - Adapt to changing input data quality (e.g., feature-rich vs. feature-poor environments).
- Balance odometry performance and computational resource usage.
The AdaptiveParameterManager would operate as follows:
-
Health Monitoring (Conceptual):
- It would subscribe to health status messages published by the
scanRegistrationandlaserMappingnodes. These messages would indicate states such as low feature counts, ICP instability (large corrections, degeneracy), or healthy operation. - (Currently,
scanRegistrationandlaserMappingonly log warnings. They would need modification to publish structured health messages on dedicated ROS 2 topics).
- It would subscribe to health status messages published by the
-
Parameter Adjustment:
- The primary parameters managed are
filter_parameter_cornerandfilter_parameter_surfin thelaserMappingnode. These control the leaf sizes of voxel grid filters for downsampling corner and surface points. - If
laserMappingreports too few features for ICP: The manager reduces filter sizes to provide more points to the ICP algorithm. - If
laserMappingreports ICP instability (e.g., large corrections, degeneracy):- If
scanRegistrationalso reports few raw features (poor input), filter sizes inlaserMappingare cautiously reduced. - If
scanRegistrationis healthy (rich input), filter sizes are increased, as instability might be due to excessive/noisy points in dense environments.
- If
- If the system is healthy: Filter sizes are slowly increased to probe for potential resource savings, as long as health remains good.
- Health State Smoothing: To prevent reactions to transient fluctuations, the manager now considers a health status from
scanRegistrationorlaserMappingas "stabilized" only if it's reported consecutively for a defined number of times (currently hardcoded asHEALTH_REPORT_STABILITY_THRESHOLD_ = 2). Decisions are based on these stabilized states. - Cautious Healthy Probing: When the overall system health becomes
HEALTHY, the manager waits for a defined number of cycles (PROBING_AFTER_N_HEALTHY_CYCLES_ = 2) during which health remains stable before attempting to increase filter sizes (probing for resource savings). This ensures stability before making parameters more aggressive.
- The primary parameters managed are
-
Simulated Resource Usage Feedback / Overload Detection:
- To prevent the system from becoming unstable due to processing too many points (e.g., after reducing filter sizes too much), a cooldown mechanism is implemented:
- If parameter adjustments lead to a configurable number of consecutive ICP instability warnings, the system is considered "overloaded."
- During overload cooldown, filter sizes are temporarily increased to stabilize the pipeline, and further reductions are paused.
- The cooldown period is reset after a sustained period of healthy operation.
- To prevent the system from becoming unstable due to processing too many points (e.g., after reducing filter sizes too much), a cooldown mechanism is implemented:
- Health Publication:
scanRegistrationandlaserMappingnodes would need to be enhanced to publish their health status (defined inadaptive_parameter_manager_types.h) on new ROS 2 topics. - Parameter Updates:
AdaptiveParameterManagerwould use a ROS 2 parameter client to dynamically setfilter_parameter_cornerandfilter_parameter_surfon thelaserMappingnode. ThelaserMappingnode would need to be modified to support dynamic parameter updates (e.g., using parameter callbacks) and apply these changes to its voxel grid filters.
- The
AdaptiveParameterManagerclass, along with its decision logic and health type definitions, has been designed and implemented. - The new files are:
src/adaptive_parameter_manager.hsrc/adaptive_parameter_manager.cppsrc/adaptive_parameter_manager_types.h
- Full integration (modifying existing LOAM nodes for health publication and dynamic parameter handling) is a separate, subsequent task. This proposal focuses on the methodology and the standalone manager's logic.
- The
AdaptiveParameterManagerinternally uses a ROS 2 asynchronous parameter client for setting parameters on thelaserMappingnode. This ensures non-blocking behavior within the manager's own processing loop and resolves potential executor conflicts. - For more detailed information on the internal logic, specific thresholds, assumptions, and implementation notes, please refer to the comprehensive documentation comments within the
src/adaptive_parameter_manager.handsrc/adaptive_parameter_manager.cppfiles.
The PointCloudAccumulatorNode is a ROS 2 node that subscribes to registered point cloud messages (typically from a SLAM algorithm like LOAM) and progressively saves them into a single, timestamped PLY (Polygon File Format) map file. This allows for the generation of a persistent point cloud map of the environment explored by the LiDAR. The node ensures that the PLY file is correctly finalized with an updated point count when the system is shut down (e.g., via Ctrl+C).
- Topic Name:
/velodyne_cloud_registered - Message Type:
sensor_msgs::msg::PointCloud2
- File Type: Polygon File Format (
.ply) - Filename Format:
map_YYYYMMDD_HHMMSS.ply(e.g.,map_20231026_143000.ply) - Location: The directory specified by the
output_directoryparameter.
The PointCloudAccumulatorNode is integrated into the existing launch system and can be started using launch files such as mapping_horizon_launch.py. A launch argument is provided to specify the output directory for the saved PLY maps.
Example:
ros2 launch livox_mapping mapping_horizon_launch.py accumulator_output_dir:=/path/to/my/ply_mapsThis command launches the LOAM mapping pipeline along with the PointCloudAccumulatorNode. The accumulated point cloud map will be saved in the /path/to/my/ply_maps directory.
output_directory(string, default:./ply_maps_from_launchas set inmapping_horizon_launch.py)- Description: Specifies the directory where the output PLY map files will be saved. If a relative path is given, it's typically relative to where the ROS 2 launch command was executed or the node's working directory if not set otherwise. It's recommended to use absolute paths for clarity. The node will attempt to create this directory if it doesn't exist.
This package is based on the original LOAM algorithm by J. Zhang and S. Singh. We also acknowledge inspiration and reference from LOAM_NOTED.
- LOAM: J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
- LOAM_NOTED







