Add fuse odometry localization for the hangar_sim mobile base#790
Open
bkanator wants to merge 5 commits into
Open
Add fuse odometry localization for the hangar_sim mobile base#790bkanator wants to merge 5 commits into
bkanator wants to merge 5 commits into
Conversation
|
Note Reviews pausedIt looks like this branch is under active development. To avoid overwhelming you with review comments due to an influx of new commits, CodeRabbit has automatically paused this review. You can configure this behavior by changing the Use the following commands to manage reviews:
Use the checkboxes below for quick actions:
📝 WalkthroughWalkthroughChangesAdds slip-aware odometry forwarding, dynamic odom-to-world drift correction, and AMCL map-to-odom gating. Fuse is enabled by default, new nodes are wired into simulation launch, localization parameters are retuned, and navigation trees no longer reset the initial pose. Fuse localization integration
Possibly related issues
Possibly related PRs
Suggested reviewers: Caution Pre-merge checks failedPlease resolve all errors before merging. Addressing warnings is optional.
❌ Failed checks (1 error)
✅ Passed checks (3 passed)
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
|
|
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 19, 2026 15:34
ecb0e20 to
98ef35f
Compare
|
bkanator
marked this pull request as ready for review
July 21, 2026 11:47
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py`:
- Around line 319-335: Update the odom_world_drift and slip_aware_odom Node
definitions to pass the use_sim_time LaunchConfiguration as their ROS parameter,
ensuring both nodes use simulation time consistently when stamping outputs.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro Plus
Run ID: 6834a0a6-300e-4af5-bd44-d25a11fda468
📒 Files selected for processing (10)
src/hangar_sim/CMakeLists.txtsrc/hangar_sim/config/control/picknik_ur.ros2_control.yamlsrc/hangar_sim/config/fuse/fuse.yamlsrc/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacrosrc/hangar_sim/description/ur5e_ridgeback.xmlsrc/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.pysrc/hangar_sim/package.xmlsrc/hangar_sim/params/nav2_params.yamlsrc/hangar_sim/script/odom_world_drift.pysrc/hangar_sim/src/slip_aware_odom.cpp
…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 21, 2026 12:59
98ef35f to
b3952a6
Compare
Author
|
[written by AI] Pre-merge reviewer gateRan code-reviewer, documentation-bot, platform-architect-bot, roboticist-bot, and the Applied
Verified, not a defect (measured, not assumed)
Deferred (works as-is; retuning needs re-validation)
Pre-existing CI blocker (not introduced by this pass)The |
Author
|
@coderabbitai full review |
✅ Action performedFull review finished. |
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py`:
- Around line 190-193: Update the state estimator launch configuration
associated with the use_fuse argument to pass the use_sim_time launch value as
the node parameter {"use_sim_time": use_sim_time} alongside the existing
fuse.yaml configuration, ensuring Fuse uses the simulation clock.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro Plus
Run ID: 137c1df9-a38e-4fc2-bd73-ece87806e72f
📒 Files selected for processing (10)
src/hangar_sim/CMakeLists.txtsrc/hangar_sim/config/control/picknik_ur.ros2_control.yamlsrc/hangar_sim/config/fuse/fuse.yamlsrc/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacrosrc/hangar_sim/description/ur5e_ridgeback.xmlsrc/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.pysrc/hangar_sim/package.xmlsrc/hangar_sim/params/nav2_params.yamlsrc/hangar_sim/script/odom_world_drift.pysrc/hangar_sim/src/slip_aware_odom.cpp
|
Widen the likelihood_field sensor model (sigma_hit 0.1->0.25, z_hit 0.9->0.65, z_rand 0.1->0.3) so obstacles that are not in the map read as outliers instead of spreading the particle cloud and lurching the pose, and score the full merged scan (max_beams 180->720). Fixes catastrophic divergence near the unmapped picking boxes (~18.8 m -> ~9 cm median) while holding the fuselage route (~8 cm / 0.77 deg).
|
…bustness AMCL's scan-match goes degenerate where the base hugs the smooth fuselage (slide-along ambiguity) or crosses the transient unmapped boxes: the pose estimate teleports and the map flips, and with likelihood_field it never recovers -- the robot is left unable to plan from a garbage pose. Add amcl_odom_gate, which holds the last good map->odom and coasts on fuse's odometry (locally trustworthy) through those zones, then blends back once AMCL is trustworthy again. A large innovation -- gated on both position and yaw -- is neither hard-accepted nor hard-rejected: it goes provisional and is accepted only if it persists at one pose for persist_time (a valid correction persists; an ambiguity teleport thrashes), so a real large correction such as recovering from a bad initial-pose seed still gets through. Particle-spread hysteresis is a second, independent hold trigger. AMCL runs with tf_broadcast:=false so the gate is the sole map->odom publisher. The pure decision logic (detail::updateGate) is split into a header with no ROS/TF deps and unit-tested for every case: transparent tracking, thrashing teleport held, yaw-only teleport held, persistent correction accepted (and held until the timer elapses), spread hysteresis, and the SE(2) transform algebra. Plane-nose stress (goal 0.3 m off the fuselage): 75% permanent-cascade failures with likelihood_field -> ~10% transient/recovered with the gate. Boxes contained, no strand. Also: remove the now-superseded per-objective SetInitialPose reseed from the clicked-point Objectives (slip_aware_odom's cross-controller re-anchoring covers the whole-body<->nav handoff it was added for), and complete the truncated BSD headers on slip_aware_odom.cpp and odom_world_drift.py that ament_copyright rejected as license=unknown. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 23, 2026 23:20
995a373 to
e9fa7da
Compare
There was a problem hiding this comment.
Actionable comments posted: 2
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py (1)
401-411: 🗄️ Data Integrity & Integration | 🟠 Major
fuse_state_estimatorstill doesn't passuse_sim_time— this was already flagged in a prior review round and appears unaddressed here.
fuse_state_estimatoronly loadsfuse.yaml; withuse_fusenow defaulting totrue, Fuse stays on the wall clock while the rest of the sim stack (including the newodom_world_drift,slip_aware_odom, andamcl_odom_gatenodes just above, which now all correctly receive{"use_sim_time": use_sim_time}) runs on sim time. This is the same gap previously raised onstate_estimator.Suggested fix
fuse_state_estimator = Node( package="fuse_optimizers", executable="fixed_lag_smoother_node", name="state_estimator", parameters=[ - PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"]) + PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"]), + {"use_sim_time": use_sim_time}, ], output="screen", condition=IfCondition(LaunchConfiguration("use_fuse")), )🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py` around lines 401 - 411, Update the fuse_state_estimator Node parameters to include the launch use_sim_time value alongside fuse.yaml, matching the {"use_sim_time": use_sim_time} configuration used by the surrounding simulation nodes. Preserve the existing package, executable, condition, and output settings.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py`:
- Around line 340-349: The launch configuration must provide a map->odom
publisher when localization is enabled and use_fuse is false. Update the
amcl_odom_gate/AMCL launch conditions or add a fallback publisher so the
use_fuse=false localization path receives map->odom, while preserving the
existing static_tf_map_to_odom behavior for localization=false.
In `@src/hangar_sim/package.xml`:
- Around line 44-48: Add the missing test dependency declaration for
ament_cmake_gmock in src/hangar_sim/package.xml, alongside the existing
dependency entries, so the test_amcl_odom_gate.cpp gmock target configured by
ament_add_gmock has its required package dependency.
---
Outside diff comments:
In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py`:
- Around line 401-411: Update the fuse_state_estimator Node parameters to
include the launch use_sim_time value alongside fuse.yaml, matching the
{"use_sim_time": use_sim_time} configuration used by the surrounding simulation
nodes. Preserve the existing package, executable, condition, and output
settings.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro Plus
Run ID: b7ab5b22-211b-48ce-90f4-5c1623ea3a3c
📒 Files selected for processing (11)
src/hangar_sim/CMakeLists.txtsrc/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.pysrc/hangar_sim/objectives/navigate_to_clicked_point.xmlsrc/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xmlsrc/hangar_sim/package.xmlsrc/hangar_sim/params/nav2_params.yamlsrc/hangar_sim/script/odom_world_drift.pysrc/hangar_sim/src/amcl_odom_gate.cppsrc/hangar_sim/src/amcl_odom_gate_logic.hppsrc/hangar_sim/src/slip_aware_odom.cppsrc/hangar_sim/test/test_amcl_odom_gate.cpp
💤 Files with no reviewable changes (2)
- src/hangar_sim/objectives/navigate_to_clicked_point.xml
- src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
🚧 Files skipped from review as they are similar to previous changes (3)
- src/hangar_sim/params/nav2_params.yaml
- src/hangar_sim/script/odom_world_drift.py
- src/hangar_sim/src/slip_aware_odom.cpp
Comment on lines
+340
to
+349
| # Confidence-gated map->odom: holds AMCL's correction and rides fuse odom when the | ||
| # particle cloud spreads on unmapped obstacles (AMCL runs with tf_broadcast:=false). | ||
| amcl_odom_gate = Node( | ||
| condition=IfCondition(LaunchConfiguration("use_fuse")), | ||
| package="hangar_sim", | ||
| executable="amcl_odom_gate", | ||
| name="amcl_odom_gate", | ||
| output="log", | ||
| parameters=[{"use_sim_time": use_sim_time}], | ||
| ) |
There was a problem hiding this comment.
🎯 Functional Correctness | 🟠 Major | ⚡ Quick win
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
fd nav2_params.yaml src/hangar_sim
rg -n "tf_broadcast" src/hangar_sim/params/nav2_params.yaml
rg -n "tf_broadcast|use_fuse" src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 853
🏁 Script executed:
#!/bin/bash
set -euo pipefail
echo "== launch around use_fuse/localization =="
sed -n '120,170p' src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
echo
sed -n '180,360p' src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
echo
echo "== nav2 params around tf_broadcast =="
sed -n '35,60p' src/hangar_sim/params/nav2_params.yamlRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 10281
🏁 Script executed:
#!/bin/bash
set -euo pipefail
python3 - <<'PY'
from pathlib import Path
p = Path('src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py')
text = p.read_text()
for needle in ['use_fuse', 'localization', 'static_tf_map_to_odom', 'amcl_odom_gate', 'RewrittenYaml', 'param_substitutions']:
print(f'== {needle} ==')
for i, line in enumerate(text.splitlines(), 1):
if needle in line:
start = max(1, i-3)
end = min(len(text.splitlines()), i+6)
for j in range(start, end+1):
print(f'{j}:{text.splitlines()[j-1]}')
print()
PYRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 12422
🏁 Script executed:
#!/bin/bash
set -euo pipefail
python3 - <<'PY'
from pathlib import Path
p = Path('src/hangar_sim/params/nav2_params.yaml')
for i, line in enumerate(p.read_text().splitlines(), 1):
if 'tf_broadcast' in line or 'amcl:' in line or 'map->odom' in line:
start = max(1, i-5)
end = min(len(p.read_text().splitlines()), i+5)
print(f'== around line {i} ==')
for j in range(start, end+1):
print(f'{j}:{p.read_text().splitlines()[j-1]}')
print()
PYRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 2118
🏁 Script executed:
#!/bin/bash
set -euo pipefail
grep -RIn --line-number --context=3 -E 'use_fuse|localization|static_tf_map_to_odom|amcl_odom_gate|RewrittenYaml|param_substitutions' src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py src/hangar_sim/params/nav2_params.yamlRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 19881
use_fuse:=false still needs a map->odom source. nav2_params.yaml forces amcl.tf_broadcast: false, and amcl_odom_gate only launches when use_fuse is true. That leaves use_fuse=false + localization=true with no map->odom publisher; static_tf_map_to_odom only covers localization:=false. Consider making the AMCL broadcast/gate conditional on use_fuse, or adding a fallback publisher for the localization-on path.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py` around
lines 340 - 349, The launch configuration must provide a map->odom publisher
when localization is enabled and use_fuse is false. Update the
amcl_odom_gate/AMCL launch conditions or add a fallback publisher so the
use_fuse=false localization path receives map->odom, while preserving the
existing static_tf_map_to_odom behavior for localization=false.
Comment on lines
+44
to
+48
| <!-- Runtime deps for the installed Python nodes (odom_world_drift, odom_qos_relay). | ||
| geometry_msgs and tf2_ros are covered by the <depend> tags above. --> | ||
| <exec_depend>rclpy</exec_depend> | ||
| <exec_depend>sensor_msgs</exec_depend> | ||
|
|
There was a problem hiding this comment.
📐 Maintainability & Code Quality | 🟡 Minor | ⚡ Quick win
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
rg -n "gmock|gtest|ament_add_gmock|ament_add_gtest|find_package" src/hangar_sim/CMakeLists.txt
rg -n "gmock|gtest" src/hangar_sim/package.xmlRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 603
Add the missing gmock test dependency
src/hangar_sim/package.xml is missing <test_depend>ament_cmake_gmock</test_depend> for test_amcl_odom_gate.cpp; src/hangar_sim/CMakeLists.txt already uses find_package(ament_cmake_gmock REQUIRED) and ament_add_gmock(...).
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/hangar_sim/package.xml` around lines 44 - 48, Add the missing test
dependency declaration for ament_cmake_gmock in src/hangar_sim/package.xml,
alongside the existing dependency entries, so the test_amcl_odom_gate.cpp gmock
target configured by ament_add_gmock has its required package dependency.
|
|
…latency Halving the fixed-lag smoother window from 0.5 to 0.3 lowers fuse-direct moving-yaw error (fuselage route, GT-referenced) from a 0.70deg median to 0.465deg -- under the 0.5deg target -- and shrinks the worst-case error spike from 20deg to 5.9deg. 0.25 reaches a marginally lower median (0.426) but reintroduces a rare ~125deg transient (shorter window, less averaging); 0.3 keeps the median win with a clean transient tail. Validated no regression: fuselage route 40/40, box cluster 12/12 (offset bounded ~10cm), close-plane repeated-nav bounded by the gate (no cascade). Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 24, 2026 15:40
5096b5f to
9071054
Compare
|
|
|
…re spread The map->odom gate's persistence check accepts a large correction that stays put for persist_time, assuming a real correction persists while an ambiguity teleport thrashes. But a confident-WRONG lock (AMCL sliding along a smooth surface like the plane fuselage, where many poses give near-identical scans) also persists at one pose -- with a severely spread particle cloud. Persistence alone cannot tell it from a real correction, so the gate adopted the wrong pose after ~20s and the estimate cascaded to meters of error with no recovery. Add spread_accept_max (default 3.0 = 2x spread_hold): a persisted correction is accepted only while spread is at/below the cap. The check is folded INTO the persistence requirement (a spread excursion re-anchors the clock), so persist_time must elapse with the cloud continuously tight -- a single-frame spread dip during a wrong lock cannot ratchet the held pose toward it. A legitimate correction converges so its spread drops below the cap and is still accepted (bad-seed recovery preserved); a divergence stays wide, so the gate keeps coasting on odom until the robot leaves the ambiguous region and AMCL re-locks with a tight cloud. Because the gate is the sole map->odom broadcaster, also add two watchdogs so a degraded state is never silent: a coast warning when it holds beyond 5s, and a stale-input warning in the publish timer when /particle_cloud stops updating (input starvation the onCloud-side check cannot see). Validated: the organic plane-nose divergence that previously plateaued at 6.7m for 30s+ now stays bounded (gate error median 9-10cm, p99 ~20cm through the same stress; longest sustained >100cm run is a single 0.1s glitch, was 20s+). Five new unit tests cover the rejected wrong-lock, the preserved recovery, the threshold boundary, the full-window bound, and the anti-ratchet oscillating-spread case; all 14 gate tests pass. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 25, 2026 17:56
ddaba6c to
0a17521
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 — including where the scene is degenerate for scan matching (the smooth fuselage, the unmapped picking boxes).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.sigma_hit0.1→0.25,z_hit0.9→0.65,z_rand0.1→0.3 so the unmapped picking boxes read as outliers instead of yanking the whole estimate — the box-divergence axis the fuse fix alone didn't touch.amcl_odom_gate, new C++ node): where AMCL's scan-match goes degenerate — the base hugging the smooth fuselage (slide-along ambiguity) or crossing the transient unmapped boxes — it holds the last goodmap→odomand coasts on fuse's odometry through the zone, then blends back once AMCL is trustworthy again. A large correction is neither hard-accepted nor hard-rejected: it is accepted only if it persists over a sliding window (a real fix persists at one pose; an ambiguity teleport thrashes), gated on both position and yaw, with particle-spread hysteresis as a second, independent trigger — so a valid large correction such as recovering from a bad initial-pose seed still gets through. AMCL runstf_broadcast:=false; the gate is the solemap→odompublisher. The pure decision logic (detail::updateGate) has no ROS/TF deps and is unit-tested for every case (transparent tracking, thrashing/yaw teleports held, persistent correction accepted, spread hysteresis, SE(2) math).SetInitialPosereseed from the clicked-point Objectives: superseded byslip_aware_odom's cross-controller re-anchoring (which keepsodomcontinuous across the whole-body↔nav handoff it was added for), and the unconditional reseed could cement a drifting estimate.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.
The degeneracy gate targets the two failure modes that remained: on a stress test that drives a goal 0.3 m from the smooth fuselage (deep in costmap inflation), bare
likelihood_fielddiverges and never recovers on ~75% of attempts; with the gate that drops to ~10% transient failures that self-recover within 1–2 attempts. The box cluster is contained with no strand. The gate assumes odometry is trustworthy for the (transient) duration of a degenerate zone — a bound documented in the node.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, with a degeneracy-aware gate that keeps localization stable where the scan match is ambiguous (the smooth fuselage and unmapped boxes), so Navigate to Clicked Point reaches goals reliably during simulated navigation.