G3: cap candidate walk depth so a stale query re-queries on a fresher scan

The live kidnap run spent ~95 s walking all 16 candidates of a stale first
query (none of whose poses were the true location) before re-querying -- and
it was the second query, on a newer more distinctive scan, that returned the
true pose at rank 1 and locked. Past the top few BBS occupancy maxima a fresh
query beats a deeper walk, so add max_walk_candidates (default 4): abandon the
query after the top few fail to lock and re-query instead of grinding ranks
5-16. Pure policy change with two unit tests; wired through the node param and
the supervisor_max_walk_candidates launch arg (set high to restore full walk).

Author: rsasaki0109

docs/g3_live_closed_loop.md

@@ -225,3 +225,27 @@ make recovery faster and first-try. The test harness should also republish the s
 the localizer acknowledges (the transient-local discovery race still occasionally drops the
 healthy baseline at slow replay rates). But the gate question -- *does tracking recover
 after the guarded reset* -- is now answered yes.
+
+## Speedup lever 1 -- bound the walk, re-query on a fresher scan
+
+The dominant cost in that run was **not** the query itself but the walk: the first query
+(at t+8 s) returned 16 candidates clustered around `(95, -63)` and `(-65, -78)`, none of
+which was the true pose `(-76, 55)` -- the right place simply was not in that scan's
+candidate set. The supervisor nonetheless walked all 16 at `settle_timeout_sec` (6 s) each,
+burning **~95 s**, before cooling down and re-querying. The *second* query (on a newer, more
+distinctive scan) returned the true pose at rank 1 with score 1.0 and locked immediately.
+
+So past the top few BBS occupancy maxima, a fresh query beats a deeper walk into a stale
+list. The policy now caps the walk at `max_walk_candidates` (default 4): after the top few
+candidates fail to lock, it treats the whole query as a miss and re-queries (spending one
+attempt) instead of grinding through ranks 5-16. On this run that turns ~95 s of doomed
+walking into ~24 s, then a fresh query -- the one that actually recovered. The cap is a pure
+policy change (`reinitialization_supervisor_policy.py`), unit-tested in
+`test_reinitialization_supervisor_policy.py` (`test_walk_is_bounded_by_max_walk_candidates`,
+`test_high_max_walk_candidates_restores_full_list_walk`), and exposed as the
+`supervisor_max_walk_candidates` launch arg. Set it high to restore walking the full list.
+
+Note this does *not* help when the true candidate is genuinely in the list but ranked deep
+on registration quality -- that is what per-candidate registration scoring in G2 (lever 2,
+not yet built) would fix. The cap addresses the more common stale-query case observed here,
+where no rank would have recovered and the time is better spent on a new scan.

launch/global_localization_recovery.launch.py

@@ -66,6 +66,7 @@ def generate_launch_description():
     # G2's BBS query can take ~10-25 s; query/settle timeouts must allow for it.
     supervisor_query_timeout_sec = LaunchConfiguration('supervisor_query_timeout_sec')
     supervisor_settle_timeout_sec = LaunchConfiguration('supervisor_settle_timeout_sec')
+    supervisor_max_walk_candidates = LaunchConfiguration('supervisor_max_walk_candidates')
 
     declared = [
         DeclareLaunchArgument(
@@ -100,6 +101,12 @@ def generate_launch_description():
             'supervisor_settle_timeout_sec', default_value='8.0',
             description='Time to observe post-reset recovery before counting the '
                         'attempt failed.'),
+        DeclareLaunchArgument(
+            'supervisor_max_walk_candidates', default_value='4',
+            description='Walk at most this many candidates from one (possibly stale) '
+                        'query before re-querying on a fresher scan; a stale query '
+                        'can return a full list none of whose poses lock '
+                        '(g3_live_closed_loop.md). Set high to walk the whole list.'),
         DeclareLaunchArgument(
             'g2_angular_resolution_deg', default_value='5.0',
             description='G2 BBS yaw sampling step; coarser = faster query, fresher '
@@ -170,6 +177,8 @@ def generate_launch_description():
                 supervisor_query_timeout_sec, value_type=float),
             'settle_timeout_sec': ParameterValue(
                 supervisor_settle_timeout_sec, value_type=float),
+            'max_walk_candidates': ParameterValue(
+                supervisor_max_walk_candidates, value_type=int),
             'use_sim_time': use_sim_time,
         }])
 

scripts/reinitialization_supervisor_node.py

@@ -87,6 +87,7 @@ def __init__(self) -> None:
         self.declare_parameter("query_timeout_sec", 10.0)
         self.declare_parameter("settle_timeout_sec", 8.0)
         self.declare_parameter("recovery_fitness_threshold", 1.5)
+        self.declare_parameter("max_walk_candidates", 4)
 
         self.params = rsp.SupervisorParams(
             request_debounce_sec=float(self.get_parameter("request_debounce_sec").value),
@@ -98,6 +99,7 @@ def __init__(self) -> None:
             settle_timeout_sec=float(self.get_parameter("settle_timeout_sec").value),
             recovery_fitness_threshold=float(
                 self.get_parameter("recovery_fitness_threshold").value),
+            max_walk_candidates=int(self.get_parameter("max_walk_candidates").value),
         )
         self.state = rsp.initial_state()
 

scripts/reinitialization_supervisor_policy.py

@@ -53,6 +53,13 @@
 accepted pose), so walking cannot cause the Phase 3 *acceptance* blowup -- it only
 changes which seed is offered next.
 
+The walk is bounded by ``max_walk_candidates``: the same live run showed a *stale*
+query can return a full ranked list none of whose poses lock, so walking all 16 burned
+~95 s at ``settle_timeout_sec`` each before the next query -- on a newer, more
+distinctive scan -- produced the candidate that recovered. Past the top few occupancy
+maxima a fresh query beats a deeper walk, so the supervisor abandons the query after
+``max_walk_candidates`` and re-queries (spending one attempt) instead.
+
 These map directly onto the G3 Decision Gates in
 ``docs/global_localization_roadmap.md``. ``test_reinitialization_supervisor_policy.py``
 fails if any of them regress.
@@ -113,6 +120,16 @@ class SupervisorParams:
     settle_timeout_sec: float = 8.0
     # Post-reset alignment fitness at or below this counts as recovered.
     recovery_fitness_threshold: float = 1.5
+    # Walk at most this many candidates from one query (counting the top one)
+    # before abandoning the whole query and re-querying with a fresh scan. The
+    # 2026-06-15 live run (docs/g3_live_closed_loop.md) showed a stale query can
+    # return a full ranked list none of whose poses lock, so walking all 16 just
+    # burned ~95 s at settle_timeout_sec each before the *next* query -- on a
+    # newer, more distinctive scan -- produced the candidate that recovered. Past
+    # the top few BBS occupancy maxima a fresh query beats a deeper walk, so cap
+    # the walk and spend the time on a new scan instead. Set very high to restore
+    # walking the entire list.
+    max_walk_candidates: int = 4
 
 
 @dataclass(frozen=True)
@@ -276,6 +293,7 @@ def decide(
             # spending another attempt -- only re-querying consumes max_attempts.
             next_index = state.candidate_index + 1
             if (next_index < len(state.candidate_scores)
+                    and next_index < params.max_walk_candidates
                     and state.candidate_scores[next_index] >= params.min_candidate_score):
                 return SupervisorDecision(
                     ACTION_PUBLISH_RESET, "next_candidate",

test/test_reinitialization_supervisor_policy.py

@@ -308,6 +308,37 @@ def test_walk_stops_at_min_candidate_score_floor():
     assert rsp.ACTION_GIVE_UP in actions(events)
 
 
+def test_walk_is_bounded_by_max_walk_candidates():
+    # A stale query returns a long ranked list none of whose poses lock. The walk
+    # must stop after max_walk_candidates instead of burning settle_timeout_sec on
+    # all 16 -- past the top few occupancy maxima a fresh query is the better spend.
+    # With max_attempts=1 (a single query) exactly max_walk_candidates resets fire.
+    params = rsp.SupervisorParams(
+        request_debounce_sec=1.0, min_candidate_score=0.6, settle_timeout_sec=3.0,
+        max_attempts=1, max_walk_candidates=4)
+    events = run_ranked(
+        params, 120, requested=True,
+        candidate_scores=[0.99] * 16, recover_on_index=None)
+    assert published_indices(events) == [0, 1, 2, 3]
+    assert actions(events).count(rsp.ACTION_QUERY) == 1
+    assert rsp.ACTION_GIVE_UP in actions(events)
+    assert events[-1][3] == rsp.STATE_EXHAUSTED
+
+
+def test_high_max_walk_candidates_restores_full_list_walk():
+    # Raising the cap restores walking the entire ranked list: the true pose at
+    # rank 5 (beyond the default cap of 4) is reached and recovers only because the
+    # cap is lifted -- proving the cap is what gates the deeper walk.
+    params = rsp.SupervisorParams(
+        request_debounce_sec=1.0, min_candidate_score=0.6, settle_timeout_sec=3.0,
+        max_attempts=1, max_walk_candidates=999)
+    events = run_ranked(
+        params, 120, requested=True,
+        candidate_scores=[0.99] * 6, recover_on_index=5)
+    assert published_indices(events) == [0, 1, 2, 3, 4, 5]
+    assert events[-1][3] == rsp.STATE_STANDDOWN
+
+
 if __name__ == "__main__":
     import pytest
     raise SystemExit(pytest.main([__file__, "-v"]))