FusionCore-ICP feedback loop merged into rtabmap_ros stabilizes SLAM

A significant upgrade to the rtabmap_ros package was merged last week in PR #1419, co-authored by Mathieu Labbe and manankharwar. The patch introduces a TurtleBot3 Gazebo Harmonic demo that implements a feedback loop between FusionCore (a UKF sensor fusion node replacing robot_localization) and icp_odometry (scan matching). The architecture is non-obvious: FusionCore processes /imu and /odom (wheel) at 100 Hz, publishing /fusion/odom which becomes the odom->base_footprint TF. icp_odometry uses that odom frame as its initial guess via guess_frame_id: odom, enabling more consistent scan matching with lower residual error. The ICP output is then fed back into FusionCore as a second velocity source (encoder2), further tightening the UKF state estimate. rtabmap runs SLAM and loop closure on top, publishing map->odom TF. Each node tightens the other – neither is strictly downstream.

To run the demo on ROS 2 Jazzy, users need to install the relevant packages and set TURTLEBOT3_MODEL=waffle, then launch the fusioncore_icp_demo file. Full architecture notes and a topic/TF table are available in the demo README. The feedback loop design is particularly valuable for outdoor applications where GPS can also be integrated via FusionCore. This merged PR represents a practical, tested solution for improving localization robustness in mobile robots, leveraging the strengths of both sensor fusion and scan matching in a tightly coupled manner. It is expected to reduce drift and improve reliability in cluttered or feature-poor environments.