mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 01:07:49 +08:00
aec4f91f61
* 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]>
64 lines
2.4 KiB
YAML
64 lines
2.4 KiB
YAML
# 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
|