frontier.py

from astar import astar

UNKNOWN = -1  # Cells not yet observed by the sensor


def get_frontiers(known_map) -> list:
    """
    Identify all frontier cells in the current known map.

    Args:
        known_map: Grid whose cells are 0 (free), 1 (obstacle), or UNKNOWN (-1).

    Returns:
        List of (x, y) frontier cell coordinates.
    """
    frontiers = []
    for y in range(known_map.height):
        for x in range(known_map.width):
            if known_map.grid[y][x] != 0:
                continue
            for dx, dy in [(0, 1), (1, 0), (0, -1), (-1, 0)]:
                nx, ny = x+dx, y+dy
                if 0 <= nx < known_map.width and 0 <= ny < known_map.height:
                    if known_map.grid[ny][nx] == UNKNOWN:
                        frontiers.append((x, y))
                        break
    return frontiers


def nearest_frontier(known_map, position: tuple) -> tuple | None:
    """
    Find the nearest reachable frontier to the given position.

    Candidates are sorted by Manhattan distance and the first reachable
    one (verified via A*) is returned.

    Args:
        known_map: Current known Grid.
        position: Rover's current (x, y).

    Returns:
        (x, y) of the nearest reachable frontier, or None if none exist.
    """
    frontiers = get_frontiers(known_map)
    if not frontiers:
        return None

    frontiers.sort(key=lambda f: abs(f[0]-position[0]) + abs(f[1]-position[1]))
    for f in frontiers:
        path, _ = astar(known_map, position, f)
        if path:
            return f
    return None