demo(turtlebot3): FusionCore + icp_odometry feedback loop (issue #1418 Option A) (#1419)

* demo(turtlebot3): add FusionCore + icp_odometry feedback loop demo Adds a TurtleBot3 Gazebo Harmonic demo (ref issue #1418 Option A) where FusionCore (wheel + IMU UKF) and icp_odometry tighten each other in a feedback loop: FusionCore's stable odom frame serves as the scan-match initial guess via guess_frame_id, and the ICP result feeds back into FusionCore as a second velocity source. rtabmap SLAM consumes the ICP output for mapping/loop-closure. Nav2 uses /fusion/odom. Files added: launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py launch/turtlebot3/fusioncore/README.md params/fusioncore_tb3.yaml params/fusioncore_tb3_bridge.yaml (TF-conflict-free bridge) params/turtlebot3_fusioncore_icp_nav2_params.yaml * demo(turtlebot3): add FusionCore demo entry to rtabmap_demos README * fix(fusioncore-demo): remove Odom/ResetCountdown from rtabmap, strip GPS params * docs(demos): add FusionCore icp_odometry demo GIF to README * fix(fusioncore-demo): add imu.frame_id override and docking_server params * fix(fusioncore-demo): remove docking_server from nav2 params * Fixing turtlebot3 demos on Jazzy * Updated humble * Working turtlebot3 demos on humble and jazzy * Fixed Twist->TwistStamped. Fixed rviz2 crashing because missing docking server * demo(turtlebot3): add FusionCore + icp_odometry feedback loop demo Adds a TurtleBot3 Gazebo Harmonic demo (ref issue #1418 Option A) where FusionCore (wheel + IMU UKF) and icp_odometry tighten each other in a feedback loop: FusionCore's stable odom frame serves as the scan-match initial guess via guess_frame_id, and the ICP result feeds back into FusionCore as a second velocity source. rtabmap SLAM consumes the ICP output for mapping/loop-closure. Nav2 uses /fusion/odom. Files added: launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py launch/turtlebot3/fusioncore/README.md params/fusioncore_tb3.yaml params/fusioncore_tb3_bridge.yaml (TF-conflict-free bridge) params/turtlebot3_fusioncore_icp_nav2_params.yaml * demo(turtlebot3): add FusionCore demo entry to rtabmap_demos README * fix(fusioncore-demo): remove Odom/ResetCountdown from rtabmap, strip GPS params * docs(demos): add FusionCore icp_odometry demo GIF to README * fix(fusioncore-demo): add imu.frame_id override and docking_server params * fix(fusioncore-demo): remove docking_server from nav2 params * Fixing turtlebot3 demos on Jazzy * Updated humble * Fixed Twist->TwistStamped. Fixed rviz2 crashing because missing docking server * fix(fusioncore-demo): tune IMU noise for Gazebo sim, add real-hardware comments * docs(fusioncore-demo): add sim vs real hardware note to README --------- Co-authored-by: manankharwar <[email protected]> Co-authored-by: matlabbe <[email protected]>
2026-10-04 16:57:46 +08:00 · 2026-05-13 22:26:25 -07:00
parent d9cd332c53
commit aec4f91f61
7 changed files with 834 additions and 0 deletions
@@ -0,0 +1,63 @@
+# FusionCore config for TurtleBot3 Waffle (Gazebo Harmonic)
+#
+# Platform:  TurtleBot3 Waffle (simulated)
+# IMU:       MPU9250 (9-axis, magnetometer unreliable in sim)
+# GPS:       None (indoor / simulation)
+# LiDAR:     HLS-LFCD2 via /scan (used by icp_odometry, not fused here directly)
+# encoder2:  rtabmap icp_odometry output on /rtabmap/icp_odometry
+#
+# Architecture: FusionCore (wheels + IMU) provides the odom frame.
+# icp_odometry uses that odom frame as its initial scan-matching guess
+# (guess_frame_id: odom). The ICP output feeds back into FusionCore
+# as a second velocity source (encoder2). Each tightens the other.
+
+fusioncore:
+  ros__parameters:
+    base_frame: base_footprint
+    odom_frame: odom
+    publish_rate: 100.0
+    publish.force_2d: true
+
+    # Gazebo Harmonic prefixes sensor frames with the model name (waffle/imu_link/tb3_imu).
+    # Override to the TF frame that robot_state_publisher actually publishes.
+    imu.frame_id: "imu_link"
+
+    # MPU9250: magnetometer disabled (unreliable in sim / near motors)
+    imu.has_magnetometer: false
+    # Simulation-tuned noise values for Gazebo Harmonic MPU9250 plugin.
+    # Real MPU9250 hardware: gyro ~0.005 rad/s, accel ~0.1 m/s2.
+    # Gazebo injects lower noise than the real sensor, so tighter values
+    # reduce unnecessary filter uncertainty in sim without affecting real-hardware users
+    # (who should revert to the hardware spec values above).
+    imu.gyro_noise: 0.002             # rad/s  (Gazebo sim-tuned; real MPU9250: 0.005)
+    imu.accel_noise: 0.02             # m/s2   (Gazebo sim-tuned; real MPU9250: 0.1)
+    imu.remove_gravitational_acceleration: false
+
+    # Wheel odometry noise (TB3 differential drive, simulated)
+    encoder.vel_noise: 0.05           # m/s
+    encoder.yaw_noise: 0.02           # rad/s
+
+    # ICP odometry as second velocity source
+    encoder2.topic: "/rtabmap/icp_odometry"
+
+    outlier_rejection: true
+    outlier_threshold_imu: 15.09
+    outlier_threshold_enc: 11.34
+
+    adaptive.imu: true
+    adaptive.encoder: true
+    adaptive.window: 50
+    adaptive.alpha: 0.01
+
+    zupt.enabled: true
+    zupt.velocity_threshold: 0.08     # m/s: slightly loose for ICP jitter
+    zupt.angular_threshold: 0.05      # rad/s
+    zupt.noise_sigma: 0.01
+
+    ukf.q_position: 0.01
+    ukf.q_orientation: 1.0e-9
+    ukf.q_velocity: 0.1
+    ukf.q_angular_vel: 0.1
+    ukf.q_acceleration: 1.0
+    ukf.q_gyro_bias: 1.0e-5
+    ukf.q_accel_bias: 1.0e-5