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 changes frontier selection to prefer useful nearby exploration by real travel cost.

• Adds A* path-cost scoring for frontier ranking.
• Aligns scoring with the configured occupancy threshold.
• Adds a configurable unknown-cell traversal penalty.
• Filters unreachable frontiers before goal selection.
• Keeps heading continuity and anti-revisit weighting in the score.

Confidence Score: 5/5

This looks safe to merge.

• No blocking issues found in the changed code.

Important Files Changed

Filename	Overview
dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py	Updates frontier scoring to use A* path cost, configured traversal rules, and unreachable-frontier filtering.

_{Reviews (3): Last reviewed commit: "fix(navigation): exclude only unreachabl..." | Re-trigger Greptile}