Add fuse odometry localization for the hangar_sim mobile base#790
Draft
bkanator wants to merge 1 commit into
Draft
Add fuse odometry localization for the hangar_sim mobile base#790bkanator wants to merge 1 commit into
bkanator wants to merge 1 commit into
Conversation
|
Important Review skippedDraft detected. Please check the settings in the CodeRabbit UI or the ⚙️ Run configurationConfiguration used: Organization UI Review profile: CHILL Plan: Pro Plus Run ID: You can disable this status message by setting the Use the checkbox below for a quick retry:
Comment |
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 17, 2026 18:18
236f955 to
99cb7f5
Compare
|
|
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 17, 2026 18:41
99cb7f5 to
ecb0e20
Compare
|
|
…667) Localize the Ridgeback mobile base with fuse (wheel + IMU) odometry and tuned beluga_amcl so Navigate to Clicked Point reaches goals during simulated navigation. - use_fuse defaults to true; fuse fuses wheel odometry + IMU into /odom_filtered. - odom_world_drift publishes a live odom->world transform from the fuse estimate so AMCL corrects real drift while world->base stays ground truth for whole-body planning. - slip_aware_odom (new node) forwards the active platform controller's odometry to fuse with a spin-aware yaw covariance, stitches the nav2 <-> whole-body controller handoff into one continuous stream, and holds the last pose across the brief switch gap so fuse never coasts (which otherwise jumped the estimate and flipped the map in whole-body). - AMCL tuned for the mecanum base: OmniMotionModel, alpha1 0.4, update_min_a 0.1 with resample_interval 3. - MPPI wz_max capped at 0.6 (below the velocity_smoother cap so it binds) to keep commanded spins within AMCL's correction bandwidth. - odom/tf and broadcaster rates decoupled from the control loop for CPU headroom. - base_gt MuJoCo site publishes ground-truth base TF for localization QA. Closes #19667. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 19, 2026 15:34
ecb0e20 to
98ef35f
Compare
|
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
[written by AI]
Closes #19667.
Problem
hangar_sim's mobile base drove on raw MuJoCo odometry — too clean to exercise the real localization stack. To represent a real Ridgeback, the base needs fuse-fused odometry (wheel + IMU) with realistic drift, and beluga_amcl must localize reliably against it during Navigate to Clicked Point.Approach
use_fuse=true): fuse fuses wheel odometry + IMU into/odom_filtered.odom→worlddrift injection: with fuse on,odom_world_driftpublishes a liveodom→worldtransform from the fuse estimate, so AMCL sees real drift to correct whileworld→basestays ground truth for whole-body planning.odom_topicstays on raw/odom— no manual swap.slip_aware_odom, new C++ node): republishes wheel odometry with a yaw covariance that grows during sustained in-place spin (mecanum rollers slip), so fuse defers to the IMU while spinning and trusts the wheels when driving straight.OmniMotionModel;alpha10.1→0.4 to stop yaw lock-loss during spins;update_min_a0.1 withresample_interval3 to correct often without particle depletion.wz_max0.6 — below the velocity_smoother's 1.0 cap so it actually binds — keeps commanded spins within AMCL's correction bandwidth so the map tracks instead of lagging.odom_rate50 Hz,tf_publish_rate30 Hz, and the broadcasterupdate_rates decoupled from the high-rate control loop, giving the localization stack headroom.Results
On a fresh sim with adequate CPU, a 40-goal aggressive-turning route reaches 40/40 goals at ~0.12 m ATE with no divergence, reproducibly across runs. The map still lags slightly during fast turns (inherent AMCL correction latency), but Navigate to Clicked Point reaches every goal.
Localization quality is CPU-bound: a co-scheduled second sim can starve AMCL into divergence even with this config, so run one sim per host when benchmarking.
Docs
Paired documentation PR: PickNikRobotics/moveit_pro#20578 (Localization Tuning + Whole-Body Mobile Architecture guides).
Manual verification
ros2 launchhangar_simand run Navigate to Clicked Point to a goal that requires a large heading change; the robot reaches the clicked point.use_fuse:=falsefalls back to raw odometry.Release notes
Enhancement:
hangar_simnow localizes its mobile base with fuse odometry (wheel + IMU) and tuned beluga_amcl, so Navigate to Clicked Point reaches goals reliably during simulated navigation.