YALIO — Yet Another Lidar ICP Odometry for ROS 2

2D laser odometry for low-cost LiDARs (e.g. LDRobot LD19 / LD06) based on a Point-to-Line ICP (PL-ICP) scan matcher, packaged as a managed (lifecycle) ROS 2 component that loads into a shared container.

Packages

Package	Role
yalio_lib	Pure C++ PL-ICP library (no ROS dependency). Unit-tested with gtest. Vendors nanoflann.
yalio_component	nav2_util::LifecycleNode component wrapping the library; registered as a composable component.
yalio	Launch, YAML config and RViz files (container + composition, optional Nav2 lifecycle_manager).

Build

cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install --cmake-args=-DCMAKE_BUILD_TYPE=Release
source install/local_setup.bash

Targets ROS 2 Humble / Jazzy (depends on nav2_util and nav2_lifecycle_manager).

All dependencies are resolved by rosdep. nanoflann is vendored inside yalio_lib (include/yalio_lib/nanoflann.hpp, v1.10.0) — it does not need to be installed separately.

Run — manual lifecycle

Start the container with the component (it comes up UNCONFIGURED):

ros2 launch yalio yalio_bringup.launch.py scan_topic:=/scan

Drive the transitions:

ros2 lifecycle set /yalio configure   # -> INACTIVE
ros2 lifecycle set /yalio activate    # -> ACTIVE, publishes /odom_icp

Run — managed (auto-configure + activate)

ros2 launch yalio yalio_with_mgr.launch.py

The Nav2 lifecycle_manager (config config/lifecycle_mgr.yaml, autostart: true) configures and activates the component automatically and keeps a bond.

With RViz2:

ros2 launch yalio yalio_rviz2.launch.py

Lifecycle state machine

UNCONFIGURED --configure--> INACTIVE --activate--> ACTIVE
   ^                            |                     |
   |                            |<----deactivate------|
   +---------cleanup------------+

• on_configure: read parameters, create publishers (inactive), TF broadcaster, reset_odometry service and set_pose subscription.
• on_activate: activate the odometry publisher, subscribe to scan, open the lifecycle-manager bond. Scans are processed only here.
• on_deactivate: drop the scan subscription, deactivate the publisher.
• on_cleanup / on_shutdown: tear down interfaces and free state.

For an in-depth explanation of managed nodes, see the tutorial ROS 2 Lifecycle Nodes.

Composition

The component is loaded into a component_container_mt (multi-threaded) so the scan callback and the lifecycle/service callbacks run concurrently; shared state is guarded by a mutex. Intra-process comms are enabled. The component can also run standalone via ros2 run yalio_component yalio_component_node.

For an in-depth explanation of node composition, see the tutorial ROS 2 Node Composition Explained.

Resetting the odometry (drift cancellation)

Available once the node is configured:

# reset to origin
ros2 service call /yalio/reset_odometry std_srvs/srv/Trigger

# reset to a known pose (AMCL initialpose convention)
ros2 topic pub --once /yalio/set_pose \
  geometry_msgs/msg/PoseWithCovarianceStamped \
  '{header: {frame_id: "odom"}, pose: {pose: {position: {x: 3.0, y: 1.0},
    orientation: {z: 0.3827, w: 0.9239}}}}'

Both drop the reference cloud so the next scan re-seeds the matcher cleanly.

Output

• /odom_icp (nav_msgs/Odometry) — pose with Hessian-derived covariance (σ²·H⁻¹, rotated into the base frame; per-step scan-matching uncertainty). The twist (velocity in the base frame, REP-105) is obtained by differentiating consecutive poses over the scan interval; its covariance is propagated from the two scan matches. The first scans after start-up or a reset carry no velocity estimate: zero twist with a large covariance.
• TF odom -> base_link (toggle with frames.publish_tf).

Reading the pose covariance

The pose covariance is the usual row-major 6×6 matrix over (x, y, z, roll, pitch, yaw). Only part of it is actually measured:

• x, y, yaw — the 3×3 block estimated from the ICP Hessian. Typical well-constrained values are ~1e-6 m² (≈ 1 mm std. dev.) and ~1e-7 rad². These grow when the scene under-constrains the match (see Notes).
• z, roll, pitch — reported as 1e6. A planar 2D scan matcher cannot observe these axes, and a huge variance is the standard ROS way to tell fusion consumers (e.g. a robot_localization EKF) to ignore them. Reporting 0 instead would claim infinite certainty that z = 0 and corrupt any 3D state estimate fusing this topic.

Notes

• Scan-to-keyframe odometry: drift accumulates; pair with a SLAM back-end for absolute pose.
• Planar laser mount assumed; set frames.laser_x/y/yaw.
• A degenerate environment (single corridor) under-constrains the along-corridor translation — surfaced as a large covariance entry.

Tests

colcon test --packages-select yalio_lib
colcon test-result --verbose