fix(navigation): score frontiers by info-gain per A* path cost (#1255)

[samuelokpor]

Problem

The frontier explorer jumps across the map — it explores ~1 m in one spot, then walks to the opposite side of the building for another ~1 m, then back, wasting a lot of travel.

Root cause is in _compute_comprehensive_frontier_score. The weighted sum gave 30% to a "distance from explored goals" term that rewards frontiers far from anywhere already visited (i.e. it incentivizes teleporting across the map), and its distance term peaked at 5 m so it penalized nearby frontiers. Distance was also straight-line, so a frontier just past a wall looked cheap even when it was a long detour to reach.

Closes #1255

Solution

Replace the weighted sum with a single objective — information gained per unit of real travel — plus heading continuity and an anti-revisit term:

• score = info_gain / (1 + path_cost), where path_cost is the A* route length over the (inflated) costmap via the existing min_cost_astar (which already supports traversing unknown cells) — so "near" means near to actually reach, not as the crow flies.
• *= 1 + 0.5 * heading_alignment for smooth sweeps instead of zig-zags.
• anti-revisit: fade out frontiers within safe_distance of an already-explored goal.
• Removed the "distance from explored goals" reward (the main teleport driver) and the 5 m-centered distance term. Falls back to a penalized straight-line distance if A* finds no path.

Iteration for context: v1 (reward far-from-explored) → ping-pong; v2 (pure nearest-first) → stalls at the first wall; v3 (info-gain / A* path cost) → smooth full-map sweep.

How to Test

Run the Go2 office sim and trigger exploration:

dimos --simulation run unitree-go2 mcp-server --daemon
dimos mcp call begin_exploration

The robot sweeps the space in a continuous loop instead of ping-ponging, and self-terminates on no-information-gain. Existing test_wavefront_frontier_goal_selector.py passes.

Measured travel distance to reach a given map coverage (same office sim, old scorer vs new):

Coverage	Before	After	Improvement
80%	45.0 m	31.8 m	−29%
90%	50.7 m	39.9 m	−21%
95%	55.2 m	47.1 m	−15%

Contributor License Agreement

• I have read and approved the CLA.

[greptile-apps]

Greptile Summary

This PR updates frontier selection to score goals by expected information gain over real travel cost. The main changes are:

• Adds A*-based path cost calculation for frontier scoring.
• Filters unreachable frontiers before ranking so they are not published as goals.
• Replaces the previous weighted score with information-per-cost, heading continuity, and an anti-revisit fade.
• Uses the configured occupancy threshold and unknown-cell penalty when planning scoring paths.

Confidence Score: 5/5

Safe to merge with minimal risk.

The changed scorer aligns A* blocking with the configured occupancy threshold, keeps unknown cells costly but traversable, and filters unreachable frontiers before selection.

Files Needing Attention: No files require special attention.

T-Rex Logs

What T-Rex did

• Verified that the compile and import step succeeded and WavefrontFrontierExplorer was imported.
• Checked for an existing test file and confirmed no match was found, completing the test-discovery check.
• Ran the harness to compare candidates; the straight-line-near candidate had Euclid 4.000, A* path 17.657, and score 0.040200, while the direct high-info candidate (1.0,8.0) scored 0.125000 and was selected.
• Observed anti-revisit behavior where the revisit-adjacent high-info candidate score dropped to 0.021575, resulting in the non-revisit candidate being chosen, with the harness reporting PASS and exit code 0.
• Collected and indexed seven artifacts documenting the compile/import step, test-search check, harness scoring runs, and anti-revisit validation.

_{Ran code and verified through T-Rex}

Important Files Changed

Filename	Overview
dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py	Replaces weighted frontier scoring with A*-based information-per-travel-cost scoring and filters unreachable frontiers before ranking.

_{Reviews (5): Last reviewed commit: "fix(navigation): score frontiers by info..." | Re-trigger Greptile}

[codecov]

Codecov Report

❌ Patch coverage is 85.18519% with 4 lines in your changes missing coverage. Please review.
✅ All tests successful. No failed tests found.

Files with missing lines	Patch %	Lines
...er_exploration/wavefront_frontier_goal_selector.py	85.18%	4 Missing ⚠️

@@            Coverage Diff             @@
##             main    #2830      +/-   ##
==========================================
- Coverage   71.10%   68.83%   -2.27%     
==========================================
  Files         897      997     +100     
  Lines       80290    93895   +13605     
  Branches     7183     9230    +2047     
==========================================
+ Hits        57088    64635    +7547     
- Misses      21319    27090    +5771     
- Partials     1883     2170     +287     

Flag	Coverage Δ
OS-ubuntu-24.04-arm	64.58% <85.18%> (+1.06%)	⬆️
OS-ubuntu-latest	67.05% <85.18%> (+0.82%)	⬆️
Py-3.10	67.04% <85.18%> (+0.82%)	⬆️
Py-3.11	67.05% <85.18%> (+0.82%)	⬆️
Py-3.12	67.04% <85.18%> (+0.81%)	⬆️
Py-3.13	67.04% <85.18%> (+0.81%)	⬆️
Py-3.14	67.05% <85.18%> (+0.81%)	⬆️
Py-3.14t	67.04% <85.18%> (+0.81%)	⬆️
SelfHosted-macOS	?

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...er_exploration/wavefront_frontier_goal_selector.py	75.41% <85.18%> (+6.61%)	⬆️

... and 240 files with indirect coverage changes

🚀 New features to boost your workflow:

• 📦 JS Bundle Analysis: Save yourself from yourself by tracking and limiting bundle sizes in JS merges.

[leshy]

Hey @samuelokpor I'm sorry this PR went under our radar, thanks for your work, we will test this

[samuelokpor]

Thanks @leshy no worries at all.

I've rebased onto current main, so this is clean to test now.

One note: #3272 removed dimos/navigation/frontier_exploration/test_wavefront_frontier_goal_selector.py, which is where my two new tests lived (unreachable-frontier exclusion and A* exception recovery). I've accepted that deletion rather than reinstating the file, so this PR is now just the scorer change.

I see @metrox-eth's #3627 re-adds a test file at that path — if that lands first I'm happy to rebase on top of it and put my two tests back there. Additionally, agreed with his read that the two changes are complementary (his detect_frontiers, mine _compute_comprehensive_frontier_score / _rank_frontiers); happy to go in either order and do the rebase on my side.

[metrox-eth]

Sounds good @samuelokpor. Either order works for me too, and if yours lands first I'll rebase #3627 on top. Your two tests are welcome in the test file mine re-adds, the reference BFS in there might even be useful for them. Nice work on the scorer.

[metrox-eth]

First contribution here: we've been using dimOS for about a week, so corrections welcome.

We have an offline harness for comparing exploration strategies on recorded maps, and pointed it at this PR. Both arms are the real files, unmodified: stock = dimensionalOS/dimos@6fcc4e2 (this PR's base, byte-identical to the installed 0.0.14b1), #2830 = samuelokpor/dimos@ff9d5ae; min_cost_astar is the installed module, C++ extension included. Only shim: instantiating the selector without an LCM bus. The harness simulates incremental discovery (360-ray 12 m lidar, one revolution per 0.25 m of travel), same planner and parameters for both arms. Maps: four recorded maps from our rover (2D lidar, mecanum, small flat) and your go2_bigoffice dataset. There is no costmap stream in the .db, so we accumulated the 2 251 lidar frames and ran dimos.mapping.pointclouds.occupancy on them; the extraction chain checks out against the shipped big_office.ply → ground-truth PNG at IoU 1.0000. 128 runs, 352 head-to-head decisions. Harness and raw CSVs: https://github.com/metrox-eth/vector-dimos/tree/main/benchmarks/pr2830.

Goal dispersion: reproduces. Median robot→goal distance drops on every map in both configs (12/12). Big office: 8.39 → 6.16 m, goals beyond 5 m 79.8 % → 58.3 %. Head-to-head on identical inputs (same frontiers, costmap and history): same pick about half the time; when they differ, #2830 takes the nearer frontier in 143 of 164 decisions.

Travel at given coverage: reproduces on the large map only. Big office: path to 80 % of visible ceiling 24.8 → 20.9 m (−15.6 %), total path −7.6 %, better on 10 of 12 paired starts. One caveat against our own favorable number: the 45 s goal timeout in the shipped config mostly truncates stock's long goals; in the config without it, the advantage shrinks. Our own flat could not test this leg at all: a geodesic check shows only 0–2.4 % of its visible area lies beyond one lidar range of walking (13.8 % on bigoffice). Nothing to save there.

Two observations. (1) Both scorers issue goals at frontiers our 46 cm body cannot reach. The selector inflates the costmap by a fixed 0.25 m. Is that meant to be robot-specific, or should callers pre-inflate to their own footprint? (#2830 abandons such goals faster, unreachable-goal churn median 6.5 to 2, which helped us.) (2) The added A* runs once per candidate, so its cost tracks frontier count: +1.5 % at 15 clusters, +27 % at 29. The overall hot spot remains detect_frontiers (pure-Python BFS per decision), unrelated to this PR.

Caveats: go2_bigoffice is one recording read through two occupancy algos (12 paired starts, not 24); our simulated body is 46 cm and wheeled, the Go2 that recorded it is ~31 cm and walks, so two thirds of that floor is closed to our body; two of our four flat maps are snapshots of the same map minutes apart; simulated pose is perfect (no slip, no reloc jumps); "better on N/M pairs" are counts, not significance tests.

Happy to rerun any configuration on either map set.

[leshy]

Thanks both, this looks really cool, I'll test irl etc

[leshy]

so with this - starting in (L shaped hallway with a dead end behind) it's a bit hard for me to tell if it's fixed as all you can do is go down the hallway itself and strategy doesn't get to shine.

Issue with explore was that in the middle of a large space with a lot of areas to explore, explore would go check one hallway, it would see a small part of it, then go all the way to the other side, check some other space a bit, then go all the way back, so it would walk a lot.

IDK exactly how I'd do this test, probably robot spawned in the middle, synthetic lidar (low range 3-5m or something, less info shows you exploration strategy more), seems like that's what you already do, that sounds good! thanks

synthetic lidar implementation could be something like

get_lidar_for_pose(global_map, Pose) -> fake_frame

doesn't have to be realistic at all, you just take a chunk of a global map and return this as a fake frame

[metrox-eth]

Follow up on the rerun, plus a correction to our own earlier comment: the "45 s goal timeout in the shipped config" we mentioned was our deployment's value, not upstream's (default is 15 s). Corrected in the repo.

Where this PR lands in our runs: removing explored_goals_score clearly helps goal dispersion (our first comment stands), and that term is indeed half of the root cause. But at short lidar range the cross-map swings remain (11 vs 11 and 22 vs 20 in our mid-start runs), because the other half is the direction term making U-turns free. So this PR looks necessary but not sufficient for #1255; full analysis and data there: #1255