Add fuse odometry localization for the hangar_sim mobile base - #790
Open
bkanator wants to merge 2 commits into
Open
Add fuse odometry localization for the hangar_sim mobile base#790bkanator wants to merge 2 commits into
bkanator wants to merge 2 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:
📝 WalkthroughSummary by CodeRabbit
WalkthroughChangesThe PR adds slip-aware odometry, AMCL transform gating, and dynamic Fuse localization integration
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
3 times, most recently
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
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 21, 2026 12:59
98ef35f to
b3952a6
Compare
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
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
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
5 times, most recently
from
July 28, 2026 23:18
c57b768 to
72b4afe
Compare
Author
|
[written by AI] Addressed the CodeRabbit review comments (force-pushed; the branch was also squashed to a single commit and rebased onto latest main):
@coderabbitai full review |
There was a problem hiding this comment.
Actionable comments posted: 3
🧹 Nitpick comments (3)
src/hangar_sim/src/amcl_odom_gate.cpp (1)
115-115: 🎯 Functional Correctness | 🔵 Trivial | ⚡ Quick winWall timer paired with sim-time stamps.
create_wall_timeris unaffected by/clock, butpublish()stamps withnow()(ROS time underuse_sim_time). If the sim runs slower than realtime, consecutive broadcasts can carry identical or barely-advancing stamps; faster-than-realtime under-publishes.rclcpp::create_timer(this, get_clock(), rclcpp::Duration::from_seconds(kPubPeriod), cb)ties the period to the same clock the stamps come from.🤖 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/src/amcl_odom_gate.cpp` at line 115, Replace the wall-clock timer assigned to timer_ in the constructor with an rclcpp timer created using get_clock() and an rclcpp::Duration derived from kPubPeriod, while preserving the existing publish() callback.src/hangar_sim/CMakeLists.txt (1)
11-12: 📐 Maintainability & Code Quality | 🔵 Trivial | ⚡ Quick winSet
cxx_std_17onslip_aware_odomtoo.
src/slip_aware_odom.cppusesstd::clamp(C++17), but unlikeamcl_odom_gatethis target doesn't declare the standard and relies on whatever the toolchain/dependency defaults provide.♻️ Proposed change
add_executable(slip_aware_odom src/slip_aware_odom.cpp) +target_compile_features(slip_aware_odom PRIVATE cxx_std_17) ament_target_dependencies(slip_aware_odom rclcpp nav_msgs)🤖 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/CMakeLists.txt` around lines 11 - 12, Update the slip_aware_odom target declaration to require C++17, matching the existing amcl_odom_gate target configuration, so src/slip_aware_odom.cpp can use std::clamp consistently across toolchains.src/hangar_sim/src/slip_aware_odom.cpp (1)
66-69: 📐 Maintainability & Code Quality | 🔵 Trivial | ⚡ Quick winConsider exposing
kHoldGap/kHoldPeriodas node parameters.The hold threshold (15 ms) is sized against the controller's measured ~500 Hz rate while its config declares
publish_rate: 50.0. If the fork is ever fixed to honour that param, every inter-message interval (20 ms) exceedskHoldGapand the node starts injecting fabricated zero-velocity samples into fuse during normal driving. Making these declarable parameters (or deriving the gap from the observed inter-message interval) removes the hidden coupling.🤖 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/src/slip_aware_odom.cpp` around lines 66 - 69, Expose kHoldGap and kHoldPeriod as configurable node parameters, and use the resolved values in the silent-source hold logic instead of fixed constants. Preserve the current defaults (0.015 and 0.02 seconds) so existing behavior remains unchanged unless parameters are explicitly overridden.
🤖 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 94-105: Update the localization include in the launch flow, rather
than only the nav2_container parameters, so localization_launch.py receives
params with the tf_broadcast rewrite from param_substitutions. Ensure
use_fuse:=false produces AMCL tf_broadcast=true and use_fuse:=true preserves
false, either by passing the rewritten YAML or applying the equivalent
RewrittenYaml transformation inside localization_launch.py.
In `@src/hangar_sim/script/odom_world_drift.py`:
- Around line 97-110: Update _joints so the incoming name list is cached even
when resolving RAIL_JOINTS fails, and guard the position lookup against self.idx
being None. Preserve the early return for messages missing rail joints while
ensuring repeated messages with the same names cannot iterate over a cleared
index cache.
In `@src/hangar_sim/test/test_amcl_odom_gate.cpp`:
- Around line 165-176: Update InterpolateOdomDuplicateStampsNoNan so it no
longer claims to exercise the unreachable zero-span interpolation guard: either
revise the test comment and assertions to verify the target-at-back clamp
behavior, or remove the test while retaining the span guard as defensive code.
---
Nitpick comments:
In `@src/hangar_sim/CMakeLists.txt`:
- Around line 11-12: Update the slip_aware_odom target declaration to require
C++17, matching the existing amcl_odom_gate target configuration, so
src/slip_aware_odom.cpp can use std::clamp consistently across toolchains.
In `@src/hangar_sim/src/amcl_odom_gate.cpp`:
- Line 115: Replace the wall-clock timer assigned to timer_ in the constructor
with an rclcpp timer created using get_clock() and an rclcpp::Duration derived
from kPubPeriod, while preserving the existing publish() callback.
In `@src/hangar_sim/src/slip_aware_odom.cpp`:
- Around line 66-69: Expose kHoldGap and kHoldPeriod as configurable node
parameters, and use the resolved values in the silent-source hold logic instead
of fixed constants. Preserve the current defaults (0.015 and 0.02 seconds) so
existing behavior remains unchanged unless parameters are explicitly overridden.
🪄 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: d46e613a-c8d7-412f-b899-1395492dfccf
📒 Files selected for processing (16)
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/include/hangar_sim/amcl_odom_gate_logic.hppsrc/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.cppsrc/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_with_replanning.xml
- src/hangar_sim/objectives/navigate_to_clicked_point.xml
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
2 times, most recently
from
July 29, 2026 14:34
3dd5f34 to
6a0255e
Compare
There was a problem hiding this comment.
Actionable comments posted: 3
🤖 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/description/ur5e_ridgeback.xml`:
- Around line 218-221: Update the base_gt site’s rgba value to use a visible
alpha while preserving its red color and existing position, size, and TF
behavior.
In `@src/hangar_sim/params/nav2_params.yaml`:
- Around line 56-58: Update the AMCL likelihood-field weights in the parameter
block so the active values z_hit and z_rand sum to 1.0, increasing one by 0.05;
keep z_short and z_max unchanged because they are inactive for this laser model.
In `@src/hangar_sim/src/slip_aware_odom.cpp`:
- Around line 91-108: Replace the wall-clock timer creation in SlipAwareOdom’s
constructor with a node-clock/ROS-time timer so the hold-gap logic follows
simulated time. Apply the same timer change in amcl_odom_gate.cpp for its 30 Hz
publishing and stale-check timers; update both affected sites, preserving their
existing periods and callbacks.
🪄 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: c9118a06-f1cf-43b6-ad24-2a778c5bae8e
📒 Files selected for processing (16)
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/include/hangar_sim/amcl_odom_gate_logic.hppsrc/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.cppsrc/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
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
4 times, most recently
from
July 31, 2026 15:56
c055aa7 to
1ea32dc
Compare
Author
|
@coderabbitai full review |
✅ Action performedFull review finished. |
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 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 375-401: Update the condition in amcl_odom_gate to require slam to
be false in addition to use_fuse and localization being true. Match the existing
static_tf_map_to_odom condition’s slam check so amcl_odom_gate is not launched
during SLAM mode.
In `@src/hangar_sim/src/slip_aware_odom.cpp`:
- Around line 131-135: Update the timing logic in the slip update block so decay
uses the full nonnegative elapsed time since last_t_, while the
|m.twist.twist.angular.z| growth term uses the existing kMaxDt-clamped duration.
Preserve the initial-timestamp behavior and continue updating last_t_ with the
current timestamp.
🪄 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: ae0e174d-9450-42aa-af7e-992aa274f77d
📒 Files selected for processing (19)
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/include/hangar_sim/amcl_odom_gate_logic.hppsrc/hangar_sim/include/hangar_sim/odom_world_drift_logic.hppsrc/hangar_sim/include/hangar_sim/se2.hppsrc/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/src/amcl_odom_gate.cppsrc/hangar_sim/src/amcl_odom_gate_logic.cppsrc/hangar_sim/src/odom_world_drift.cppsrc/hangar_sim/src/slip_aware_odom.cppsrc/hangar_sim/test/test_amcl_odom_gate.cppsrc/hangar_sim/test/test_odom_world_drift.cpp
💤 Files with no reviewable changes (2)
- src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
- src/hangar_sim/objectives/navigate_to_clicked_point.xml
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 31, 2026 16:37
1ea32dc to
7f95724
Compare
Author
|
[written by AI] @coderabbitai full review |
|
✅ Action performedFull review finished. |
There was a problem hiding this comment.
Actionable comments posted: 2
🧹 Nitpick comments (4)
src/hangar_sim/include/hangar_sim/amcl_odom_gate_logic.hpp (1)
101-104: 📐 Maintainability & Code Quality | 🔵 Trivial | 💤 Low valueTrim the buffer with a single range erase.
The loop erases one element at a time from the front. Each erase shifts the remaining elements. The buffer holds about 30 samples, so the cost is small, but a single range erase states the intent more clearly.
♻️ Proposed refactor
- const double cutoff = s.t - keep_window_sec; - while (buf.size() > 1 && buf.front().t < cutoff) - { - buf.erase(buf.begin()); - } + const double cutoff = s.t - keep_window_sec; + // Keep at least the newest sample so interpolateOdom always has an anchor. + const auto last_keep = buf.end() - 1; + const auto first_keep = std::find_if(buf.begin(), last_keep, [cutoff](const OdomSample& o) { return o.t >= cutoff; }); + buf.erase(buf.begin(), first_keep);🤖 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/include/hangar_sim/amcl_odom_gate_logic.hpp` around lines 101 - 104, Update the buffer-trimming logic in the surrounding AMCL odometry gate code to identify the first sample at or after cutoff and remove all older samples with a single range erase. Preserve the existing buf.size() > 1 guard and cutoff boundary behavior.src/hangar_sim/src/amcl_odom_gate.cpp (1)
257-260: 🩺 Stability & Availability | 🔵 Trivial | ⚡ Quick winWarn when
have_map_odom_stays false.This node is the sole
map->odompublisher when fuse is enabled. If/particle_cloudnever arrives, or theodom->ridgeback_base_linklookup keeps failing,publish()returns here on every tick and the node emits nothing. ThekStaleInputSecondswatchdog below cannot report that case, because it sits after this guard. Add a throttled warning before the return so a startup failure is visible.🔍 Proposed diagnostic
if (!have_map_odom_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs, + "amcl_odom_gate: no map->odom yet -- waiting for /particle_cloud and the " + "odom->ridgeback_base_link TF. No map->odom is being broadcast."); return; }🤖 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/src/amcl_odom_gate.cpp` around lines 257 - 260, Add a throttled warning immediately before the early return guarded by have_map_odom_ in publish(), so repeated missing map-to-odom state is visible during startup or lookup failures. Preserve the existing return behavior and use the node’s established logging/throttling mechanism.src/hangar_sim/test/test_amcl_odom_gate.cpp (1)
427-441: 📐 Maintainability & Code Quality | 🔵 Trivial | ⚡ Quick winThe "just below the cap" case passes with a thin margin.
p.spread_accept_max - kEpsis 2.9, which also exceedsspread_hold(1.5) and never falls belowspread_resume(0.6). So the gate accepts the correction through persistence, rampsalpha, and then latchesspread_holdingthe moment the innovation drops underjump_hold.alphasnaps to 0 and freezesheldpart-way.Tracing the default
alpha_slewof 0.05: the residual distance reaches about 0.52 m, then about 0.26 m on the next step, where the gate freezes. The assertion threshold is 0.3, so the margin is about 0.04 m. A small change toalpha_sleworjump_holdbreaks this test for a reason unrelated tospread_accept_max.Isolate the boundary under test. Lower
spread_holdinterference by raisingspread_hold/spread_resumeabove the probed spread, or assert ons.alphareaching 1.0 instead of on the converged distance.💚 Proposed adjustment
// GIVEN spread just UNDER spread_accept_max: a persistent correction is accepted. { GateParams p; // spread_accept_max = 3.0 + // Keep the spread-hysteresis trigger out of the way so this test probes spread_accept_max alone. + p.spread_hold = p.spread_accept_max + 1.0; + p.spread_resume = p.spread_accept_max; GateState s;🤖 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/test/test_amcl_odom_gate.cpp` around lines 427 - 441, Update the “spread just UNDER spread_accept_max” test to isolate the acceptance boundary by configuring GateParams spread_hold and spread_resume above p.spread_accept_max, preventing jump-hold latching and alpha freezing from affecting the result. Keep the existing spread_accept_max probe and acceptance assertion focused on boundary behavior.src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py (1)
335-335: 📐 Maintainability & Code Quality | 🔵 Trivial | ⚡ Quick winExtract
use_fuseinto a namedLaunchConfigurationvariable.
LaunchConfiguration("use_fuse")is constructed inline at each use site (Lines 335, 344, 358, 386, 481) instead of once, unlikeslam,localization, anduse_sim_time, which are assigned to local variables near the top ofgenerate_launch_description. This file'sPythonExpressioncondition strings have needed several fixes in past reviews. Declaringuse_fuseonce reduces the risk of a future typo breaking one of these conditions silently.♻️ Proposed refactor
use_sim_time = LaunchConfiguration("use_sim_time") params_file = LaunchConfiguration("params_file") autostart = LaunchConfiguration("autostart") use_composition = LaunchConfiguration("use_composition") use_respawn = LaunchConfiguration("use_respawn") log_level = LaunchConfiguration("log_level") + use_fuse = LaunchConfiguration("use_fuse")Then replace each inline
LaunchConfiguration("use_fuse")occurrence withuse_fuse.Also applies to: 344-344, 358-358, 380-392, 481-481
🤖 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` at line 335, In generate_launch_description, assign LaunchConfiguration("use_fuse") to a named local variable alongside slam, localization, and use_sim_time, then replace every inline use_fuse LaunchConfiguration construction in the listed condition sites with that variable.
🤖 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/src/amcl_odom_gate_logic.cpp`:
- Around line 85-97: Update the persistence-timer logic in updateGate so a now
value earlier than s.provisional_since re-anchors the timer by setting
s.provisional_since to now before evaluating persist_time. Preserve the existing
follow behavior for non-backward timestamps and keep the candidate persistence
state intact.
In `@src/hangar_sim/src/amcl_odom_gate.cpp`:
- Around line 89-98: Validate the declared gate parameters after the declaration
block in the node initialization flow, including requiring alpha_slew to be
positive and spread_resume to be less than spread_hold. Follow the existing
warning treatment used for latency_compensation_sec >= kOdomBufferSec, and
ensure invalid configurations are reported without silently allowing a frozen
alpha or ineffective hysteresis.
---
Nitpick comments:
In `@src/hangar_sim/include/hangar_sim/amcl_odom_gate_logic.hpp`:
- Around line 101-104: Update the buffer-trimming logic in the surrounding AMCL
odometry gate code to identify the first sample at or after cutoff and remove
all older samples with a single range erase. Preserve the existing buf.size() >
1 guard and cutoff boundary behavior.
In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py`:
- Line 335: In generate_launch_description, assign
LaunchConfiguration("use_fuse") to a named local variable alongside slam,
localization, and use_sim_time, then replace every inline use_fuse
LaunchConfiguration construction in the listed condition sites with that
variable.
In `@src/hangar_sim/src/amcl_odom_gate.cpp`:
- Around line 257-260: Add a throttled warning immediately before the early
return guarded by have_map_odom_ in publish(), so repeated missing map-to-odom
state is visible during startup or lookup failures. Preserve the existing return
behavior and use the node’s established logging/throttling mechanism.
In `@src/hangar_sim/test/test_amcl_odom_gate.cpp`:
- Around line 427-441: Update the “spread just UNDER spread_accept_max” test to
isolate the acceptance boundary by configuring GateParams spread_hold and
spread_resume above p.spread_accept_max, preventing jump-hold latching and alpha
freezing from affecting the result. Keep the existing spread_accept_max probe
and acceptance assertion focused on boundary behavior.
🪄 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: 4799d7cb-5e12-4484-acbf-10cbd29fdf69
📒 Files selected for processing (19)
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/include/hangar_sim/amcl_odom_gate_logic.hppsrc/hangar_sim/include/hangar_sim/odom_world_drift_logic.hppsrc/hangar_sim/include/hangar_sim/se2.hppsrc/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/src/amcl_odom_gate.cppsrc/hangar_sim/src/amcl_odom_gate_logic.cppsrc/hangar_sim/src/odom_world_drift.cppsrc/hangar_sim/src/slip_aware_odom.cppsrc/hangar_sim/test/test_amcl_odom_gate.cppsrc/hangar_sim/test/test_odom_world_drift.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
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 31, 2026 17:41
7f95724 to
e1f65a4
Compare
Author
|
[written by AI] @coderabbitai full review |
|
✅ Action performedFull review finished. |
There was a problem hiding this comment.
🧹 Nitpick comments (1)
src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py (1)
476-494: 📐 Maintainability & Code Quality | 🔵 Trivial | 💤 Low valueConsider emitting this guard at warning severity.
The message text says
UNSUPPORTED, and the comment says "Fail loudly".LogInfoprints at INFO level, so the line blends into normal launch output. Launch has noLogWarnaction, so use anOpaqueFunction(orLogInfoplus alaunch.loggingwarning call) if you want the higher severity.♻️ Example: log at warning level
def _warn_unsupported(context): launch.logging.get_logger("hangar_sim").warning( "UNSUPPORTED: use_fuse:=false with localization:=true -- ..." ) return [] warn_unsupported_localization = OpaqueFunction( function=_warn_unsupported, condition=IfCondition( PythonExpression( [ "'", LaunchConfiguration("use_fuse"), "'.lower() == 'false' and '", localization, "'.lower() == 'true'", ] ) ), )🤖 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 476 - 494, Update warn_unsupported_localization to emit the existing UNSUPPORTED message at warning severity instead of using LogInfo. Define an OpaqueFunction callback that logs through launch.logging.get_logger("hangar_sim").warning and returns no actions, while preserving the current IfCondition and message text.
🤖 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.
Nitpick comments:
In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py`:
- Around line 476-494: Update warn_unsupported_localization to emit the existing
UNSUPPORTED message at warning severity instead of using LogInfo. Define an
OpaqueFunction callback that logs through
launch.logging.get_logger("hangar_sim").warning and returns no actions, while
preserving the current IfCondition and message text.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro Plus
Run ID: 470c6147-a102-41c3-810c-dfc613dd6ffa
📒 Files selected for processing (19)
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/include/hangar_sim/amcl_odom_gate_logic.hppsrc/hangar_sim/include/hangar_sim/odom_world_drift_logic.hppsrc/hangar_sim/include/hangar_sim/se2.hppsrc/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/src/amcl_odom_gate.cppsrc/hangar_sim/src/amcl_odom_gate_logic.cppsrc/hangar_sim/src/odom_world_drift.cppsrc/hangar_sim/src/slip_aware_odom.cppsrc/hangar_sim/test/test_amcl_odom_gate.cppsrc/hangar_sim/test/test_odom_world_drift.cpp
💤 Files with no reviewable changes (2)
- src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
- src/hangar_sim/objectives/navigate_to_clicked_point.xml
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
July 31, 2026 21:16
e1f65a4 to
c8c2181
Compare
Give hangar_sim's mobile base a realistic localization stack so beluga_amcl is exercised against fuse-fused odometry (wheel + IMU) with real drift, including where the scene is degenerate for scan matching (the smooth fuselage, the unmapped picking boxes). - Fuse on by default (use_fuse=true) -> /odom_filtered; odom_world_drift injects a live odom->world from the fuse estimate so AMCL sees drift to correct while world->base stays ground truth for whole-body planning. - slip_aware_odom (new C++ node): grows wheel-yaw covariance during sustained in-place spin (mecanum roller slip) so fuse defers to the IMU while spinning and trusts the wheels driving straight; also re-anchors odom across the whole-body<->nav controller handoff. - AMCL tuning: OmniMotionModel; alpha1 0.1->0.4; update_min_a 0.1 with resample_interval 3; likelihood relaxed (sigma_hit 0.1->0.25, z_hit 0.9->0.65, z_rand 0.1->0.3) so unmapped boxes read as outliers instead of yanking the pose. - fuse lag_duration 0.5->0.3: shorter smoother window cuts output latency (moving-yaw 0.70->0.465deg) while still smoothing transient spikes. - amcl_odom_gate (new C++ node): sole map->odom publisher (AMCL tf_broadcast false). Holds the last good map->odom and coasts on fuse odom where AMCL is degenerate, blending back when trustworthy. A large correction is accepted only if it persists over a sliding window (gated on position AND yaw) with particle-spread hysteresis; spread_accept_max additionally rejects a confident-but-WRONG lock (which persists yet stays spread) so a scan-slide divergence is coasted through while a real recovery (which converges) is still adopted. latency_compensation_sec composes the correction with odom->base from one estimator-lag ago (referenced to the cloud stamp), forward-projecting it with real buffered odometry -> moving-pose error 13.8->8.2cm, moving-yaw 1.2->0.9deg, no overshoot on turns; 0 disables, hangar_sim uses 0.30. The pure decision logic (detail::updateGate, interpolateOdom, appendOdomSample) has no ROS/TF deps and is fully unit-tested. - Removed the per-objective SetInitialPose reseed from the clicked-point Objectives (superseded by slip_aware_odom re-anchoring; the unconditional reseed could cement a drifting estimate). - wz_max 0.6 (below the velocity_smoother cap so it binds) keeps spins within AMCL's correction bandwidth; odom_rate 50Hz / tf_publish_rate 30Hz and the broadcaster rates decoupled from the control loop give the stack CPU headroom. Closes #19667. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
August 6, 2026 16:00
c8c2181 to
32da10b
Compare
…eness guards Extracts slip_aware_odom's re-anchor/covariance-ramp logic into a pure, unit-tested header (mirroring amcl_odom_gate's existing pattern), and adds staleness guards flagged by review: odom_world_drift now withholds odom->world when /odom_filtered goes stale instead of broadcasting a frozen estimate with a fresh timestamp; amcl_odom_gate now treats a stale odom buffer as unavailable (with a dedicated warning) instead of silently clamping to an old sample; and slip_aware_odom's hold() now stops (and warns) after a bounded gap instead of fabricating "parked" odometry forever if a controller dies. Routes the three new nodes' logs to console (output=both) so their watchdog warnings are actually visible, and drops internal issue-number references from code comments per .claude/rules/cpp-style.md. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
bkanator
force-pushed
the
feat/19667-fuse-odometry
branch
from
August 6, 2026 19:04
c84e2d5 to
84e1708
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.fusesmoother window (lag_duration0.5→0.3): a shorter fixed-lag window cuts the estimator's output latency so the fused pose tracks the truth more tightly during motion — moving-yaw error 0.70°→0.465° — while staying long enough to smooth transient spikes (0.25 reached a marginally lower median but reintroduced a rare ~125° transient).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. A confident-but-wrong lock is the hard case the sliding window alone cannot catch: sliding along the smooth fuselage, AMCL settles at a wrong pose that also persists, with a severely spread cloud.spread_accept_maxcloses that gap — a persisted correction is accepted only while the cloud is tight enough, so a real recovery (which converges) is still adopted while a scan-slide divergence (which stays spread) is coasted through instead. 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, severe-spread wrong-lock rejected, spread hysteresis, SE(2) math).latency_compensation_seconamcl_odom_gate): during motion themap→baseestimate trailed the truth by ~14 cm — not the gate's smoothing but AMCL's own scan-processing lag (~300 ms), which its message stamp under-reports. The gate now buffersodom→basehistory and composes AMCL's correction with the odometry from one lag ago (referenced to the cloud's own stamp), forward-projecting it to the present using real buffered odometry (so it cannot overshoot on sharp turns).0reproduces the old compose-at-latest behaviour exactly;hangar_simsets 0.30, tuned to the measured lag (re-measure per platform). A backward sim-clock jump flushes the buffer so a reset cannot poison it.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.Latency compensation cut the residual in-motion error further: on the fuselage route, moving-pose error dropped 13.8→8.2 cm and moving-yaw 1.2→0.9° (3-run medians, tuning
latency_compensation_sec), with at-rest error and objective success unchanged and the wrong-lock protection intact. A long soak (252 goals across the hangar) ran with zero failures.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) and compensates the estimator's motion latency so the map tracks the base more tightly while driving, so Navigate to Clicked Point reaches goals reliably during simulated navigation.