feat: hangar_sim, add beluga_amcl localization with hardcoded initial…#605
Open
bkanator wants to merge 1 commit into
Open
feat: hangar_sim, add beluga_amcl localization with hardcoded initial…#605bkanator wants to merge 1 commit into
bkanator wants to merge 1 commit into
Conversation
bkanator
force-pushed
the
add_beluga_amcl
branch
2 times, most recently
from
9a9d7a0 to
308ed11
Compare
📝 WalkthroughWalkthroughThis PR enables beluga_amcl-based localization in simulation. It updates the localization launch, parent launch wiring, AMCL parameters, runtime dependencies, and navigation objectives to set an initial pose in ChangesLocalization feature enablement
Possibly related issues
Suggested reviewers
Caution Pre-merge checks failedPlease resolve all errors before merging. Addressing warnings is optional.
❌ Failed checks (1 error, 1 warning)
✅ Passed checks (2 passed)
Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
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)
329-337:⚠️ Potential issue | 🔴 Critical | ⚡ Quick winFix duplicate
conditionkeyword instatic_tf_map_to_odomNode call (parse-time failure).
static_tf_map_to_odom = Node(...)passescondition=twice (lines 329-337), which prevents the launch file from compiling/executing. Remove the secondcondition.Suggested fix
static_tf_map_to_odom = Node( condition=IfCondition( PythonExpression(["not ", slam, " and not ", localization]) ), package="tf2_ros", executable="static_transform_publisher", name="static_tf_map_to_odom", output="log", arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"], - condition=IfCondition(PythonExpression(["not ", slam])), )🤖 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 329 - 337, The Node call that creates static_tf_map_to_odom passes the condition= keyword twice causing a parse error; edit the Node(...) for static_tf_map_to_odom to remove the duplicate condition argument (keep the intended condition IfCondition(PythonExpression(["not ", slam, " and not ", localization])) and delete the later condition=IfCondition(PythonExpression(["not ", slam]))). Ensure only one condition kwarg exists on the static_tf_map_to_odom Node to restore valid launch file syntax.
🤖 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/localization_launch.py`:
- Line 60: Remove the unused LaunchConfiguration assignment by deleting the
log_level = LaunchConfiguration("log_level") line (remove the symbol log_level
and its LaunchConfiguration call) so the unused variable no longer triggers
F841; ensure no other references to log_level remain and run lint to confirm the
warning is resolved.
- Around line 82-83: The gate compares
context.perform_substitution(localization) to the exact string "True", which
fails for "true"/" TRUE"/etc.; normalize the substituted localization value
before comparing (e.g., call .strip().lower() on the result) so the check uses a
boolean-safe comparison in the if statement that currently references
localization; update that condition in localization_launch.py to compare the
normalized string to "true".
---
Outside diff comments:
In `@src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py`:
- Around line 329-337: The Node call that creates static_tf_map_to_odom passes
the condition= keyword twice causing a parse error; edit the Node(...) for
static_tf_map_to_odom to remove the duplicate condition argument (keep the
intended condition IfCondition(PythonExpression(["not ", slam, " and not ",
localization])) and delete the later
condition=IfCondition(PythonExpression(["not ", slam]))). Ensure only one
condition kwarg exists on the static_tf_map_to_odom Node to restore valid launch
file syntax.
🪄 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: 00862f77-0187-4d32-8292-dec8e39708d2
📒 Files selected for processing (4)
src/hangar_sim/launch/sim/localization_launch.pysrc/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.pysrc/hangar_sim/package.xmlsrc/hangar_sim/params/nav2_params.yaml
|
|
Author
|
here are initial videos with beluga integrated with ground truth odom but not tuning. first is a successful run, and the second is showing failure with getting near the plane. particle_first-2026-06-09_09.52.29.mp4particle_fail-2026-06-09_09.47.02.mp4 |
Author
|
Todo: fix reset of particles after failure, tune resampling weights. |
bkanator
force-pushed
the
add_beluga_amcl
branch
from
9c936bf to
1f0a472
Compare
|
bkanator
force-pushed
the
add_beluga_amcl
branch
from
1f0a472 to
781f710
Compare
|
bkanator
force-pushed
the
add_beluga_amcl
branch
from
781f710 to
12d564f
Compare
|
bkanator
force-pushed
the
add_beluga_amcl
branch
from
12d564f to
75c8fbc
Compare
|
bkanator
force-pushed
the
add_beluga_amcl
branch
from
75c8fbc to
cca756e
Compare
|
Author
|
Here is an updated working video: good_particles-2026-06-24_10.31.26.mp4 |
Author
|
[written by AI] Stress-testing localization stability. beluga_amcl stays converged in normal navigation (mean particle spread ~0.3 m, zero divergence across multiple multi-minute runs). We hit one intermittent catastrophic divergence ( |
bkanator
force-pushed
the
add_beluga_amcl
branch
from
cca756e to
642d2db
Compare
|
Author
|
and after found stress-test failure, then started using front and back merged lidar, some runs near fuselage. dual_good_particles-2026-06-25_12.52.30.mp4 |
Author
|
[written by AI] Dual-lidar fix — near-airplane localization AMCL now consumes a merged front+rear 360deg scan (
|
bkanator
force-pushed
the
add_beluga_amcl
branch
3 times, most recently
from
241e7c7 to
261a4be
Compare
|
There was a problem hiding this comment.
Pull request overview
Adds map-based localization to hangar_sim by integrating beluga_amcl (optionally enabled via a new localization launch argument) and wiring it into the existing Nav2 container setup, along with initial seeding support from Objectives.
Changes:
- Enabled AMCL configuration in
nav2_params.yaml(beluga) with initial pose seeding and updated scan topic. - Added
beluga_amcl+dual_laser_mergerto the localization launch path, with alocalizationtoggle and a staticmap->odomfallback when disabled. - Added
SetInitialPoseat the start of the clicked-point navigation Objectives.
Reviewed changes
Copilot reviewed 6 out of 6 changed files in this pull request and generated 5 comments.
Show a summary per file
| File | Description |
|---|---|
| src/hangar_sim/params/nav2_params.yaml | Introduces beluga AMCL parameters (including initial pose seeding) and points AMCL at /scan_merged. |
| src/hangar_sim/package.xml | Adds runtime dependencies needed for beluga AMCL and scan merging. |
| src/hangar_sim/objectives/navigate_to_clicked_point.xml | Adds SetInitialPose at the start of the clicked-point navigation Objective. |
| src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml | Adds SetInitialPose at the start of the replanning variant of the clicked-point navigation Objective. |
| src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py | Adds localization argument, forwards it to localization launch, and gates the static map->odom TF fallback. |
| src/hangar_sim/launch/sim/localization_launch.py | Loads dual_laser_merger, map_server, beluga_amcl, and lifecycle manager into nav2_container when localization is enabled; otherwise loads map_server only. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+78
to
+89
| def check_map_exists(context): | ||
| # Match IfCondition's accepted truthy values (true/1) so this guard | ||
| # stays consistent with the nodes gated on `localization` below. | ||
| if context.perform_substitution(localization).lower() not in ("true", "1"): | ||
| return [] | ||
| map_file = context.perform_substitution(map_yaml_file) | ||
| if not os.path.exists(map_file): | ||
| raise RuntimeError( | ||
| f"Map file not found: {map_file}\n" | ||
| "Run SLAM first (slam:=True) to build a map, or provide a map path via map:=<path>." | ||
| ) | ||
| return [] |
| # Load map_server only (no AMCL) when localization is disabled. | ||
| # A static map->odom TF is expected from the parent launch in this case. | ||
| load_map_server_only = LoadComposableNodes( | ||
| condition=IfCondition(PythonExpression(["not ", localization])), |
| # Static map->odom TF fallback: only used when neither SLAM nor AMCL is publishing it. | ||
| static_tf_map_to_odom = Node( | ||
| condition=IfCondition( | ||
| PythonExpression(["not ", slam, " and not ", localization]) |
| _favorite="true" | ||
| > | ||
| <Control ID="Sequence" name="TopLevelSequence"> | ||
| <Action ID="SetInitialPose" robot_frame_id="ridgeback_base_link" /> |
| _favorite="true" | ||
| > | ||
| <Control ID="Sequence" name="TopLevelSequence"> | ||
| <Action ID="SetInitialPose" robot_frame_id="ridgeback_base_link" /> |
bkanator
force-pushed
the
add_beluga_amcl
branch
from
261a4be to
ad53140
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/objectives/navigate_to_clicked_point.xml`:
- Line 10: The SetInitialPose action in navigate_to_clicked_point.xml should
only run when AMCL is active, since robot_drivers_to_persist_sim.launch.py and
localization_launch.py do not always start beluga_amcl. Guard or scope the
SetInitialPose step in the navigate_to_clicked_point objective so it is skipped
for slam:=True or localization:=False runs, and keep the change aligned with the
existing localization configuration flow.
🪄 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: 7fccad2a-ba3b-4659-9d17-1b43765a6b21
📒 Files selected for processing (6)
src/hangar_sim/launch/sim/localization_launch.pysrc/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.yaml
🚧 Files skipped from review as they are similar to previous changes (1)
- src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
|
|
Approve but waiting for https://github.com/PickNikRobotics/moveit_pro/pull/19710 |
Adds beluga_amcl for map-based localization on the Ridgeback mecanum base. AMCL consumes a merged front+rear 360-degree scan (dual_laser_merger): with the front lidar alone, the hangar airplane's long featureless fuselage gives the particle filter no position lock and it can diverge catastrophically; the rear lidar supplies the structure the front cannot see. AMCL uses an omnidirectional motion model (diff-drive mis-models strafing) and self-seeds at the spawn pose via set_initial_pose. The SetInitialPose Behavior re-seeds at the robot's current pose at the start of each navigation Objective. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
bkanator
force-pushed
the
add_beluga_amcl
branch
from
ad53140 to
c736b1d
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
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.
needs: moveit_pro/#19710
Closes PickNikRobotics/moveit_pro#18065
Adds
beluga_amclmap-based localization to hangar_sim, replacing the static identitymap → odomTF with laser-scan localization.What
beluga_amclas the localizer whenslam:=False(defaultlocalization:=True).dual_laser_merger+map_server+beluga_amcl+lifecycle_managerload into the existingnav2_container.dual_laser_mergerfuses the front and rear lidar scans into a single 360°/scan_mergedfor AMCL — with the front lidar alone, the airplane's long featureless fuselage gave the particle filter no position lock and it could diverge catastrophically.set_initial_poseparam, so the filter starts without a manual 2D pose estimate.SetInitialPoseBehavior is added at the start of the clicked-point nav Objectives, re-seeding the filter at the robot's current pose each run.Why
The old setup had no real localization (a static identity TF stood in for
map → odom). beluga estimates the pose from laser scans; merging both lidars is what keeps it stable near the fuselage (validated: max particle spread 149 m front-only → 0.69 m merged, zero divergence on the same route).Depends on
PickNikRobotics/moveit_pro#19710 — adds the
SetInitialPoseBehavior these Objectives use. Merge #19710 first; this PR's CI tests against #19710's image via theneeds:token above.Release notes
Enhancement: Added
beluga_amcllocalization tohangar_sim, replacing the staticmap → odomidentity TF with laser-scan-based localization. AMCL consumes a merged front+rear 360° scan so the particle filter stays stable near the hangar airplane.Claude agent checks
code-reviewertest-runnerplatform-architect-botReviewed via the rubrics in moveit_pro's
.claude/agents/. Findings applied:code-reviewer(unusedlog_levelremoved; map-existence check made unconditional),platform-architect-bot(dead launch args removed;not <bareword>launch conditions replaced withUnlessCondition/string compares so lowercasetrue/falseno longer crash),roboticist-bot(merge geometry/frames confirmed;scan_time/laser_max_rangecorrected; syncqueue_sizeraised for headroom). Copilot findings applied (same gating fixes).test-runner: example_ws integration tests run against #19710's image via theneeds:token.