diag: run #790's hangar_sim against instrumented moveit_pro image (DO NOT MERGE) - #842
Closed
bkanator wants to merge 2 commits into
Closed
diag: run #790's hangar_sim against instrumented moveit_pro image (DO NOT MERGE)#842bkanator wants to merge 2 commits into
bkanator wants to merge 2 commits into
Conversation
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>
…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>
|
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 |
|
Author
|
[written by AI] Diagnostic pairing, its purpose served. The measurement it produced identified the root cause, now tracked as PickNikRobotics/moveit_pro#21442 with the fix in PickNikRobotics/moveit_pro#21443, paired to this PR's branch via the |
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.
DIAGNOSTIC ONLY — do not merge. This branch is #790's code unmodified (head
d6450d5a); the only purpose is to run hangar_sim against an instrumented moveit_pro image.needs: moveit_pro/#21433
Why
hangar_sim
Plan Path Along Surfaceand3 Passesfail on #790 with an empty point cloud after the crop. Local reproduction is possible but the local box has become an unreliable proxy — its failures are dominated by environmental modes (bind errors, server crashes, planning failures) rather than the CI bug. CI fails the real way essentially every run, so the measurement belongs here.moveit_pro#21433 instruments
TransformPointCloudFrameandCropOrRemovePointsInBoxto log, per capture: cloud age, how long the TF lookup blocked, finite-vs-total points, points inside the crop box with per-axis counts, the crop's in/out counts and frames, and — the key field —world → ridgeback_base_linkat the cloud's own timestamp.What each outcome will mean
The interpretation is fixed in advance:
IN_BOX=0in_z=0IN_BOX>0 butout_pts=0finite=0Already eliminated with evidence
Publish rates (reverted, still failed), stale camera frames (1.5 s settle, still failed), a settle between transform and crop (1 s, failed 6/6), TF latency (8–19 ms measured in-process), NaN (zero NaN in 921600 points), the crop itself (output matched an independent in-box count 8/8),
odom_world_drift, nav2 controller contention (never activates in CI), andallow_partial_joints_goal(base joints are commanded — reference travels 10.3 m; andfalsestill failed 6/6 in CI).