MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
"""Cold bench for explorer2: known maps in, known decisions out.
Every check is a known input giving a known output in physical units
(metres, seconds, a named directive) - not "it returned something". Groups:
A. frontier + goal placement - where the target lands on a plain room
B. prefer-forward - two mirror-image clusters, the one ahead wins
C. failed-target memory - an exclusion WAITS, never ends the run, and
reopens on TRIGGERS (map changed / new
viewpoint), never on a clock
D. born cornered - the pocket signature gives ONE back-off
E. blocked region - a block published as occupied is never targeted
F. the end of a run - None exactly when there is nothing left
H. revisits - fresh ground beats ground already worked
I. shut in - pinched is not finished
K. how far - the distance exponent (a big frontier across
the room does not beat a small one beside
the rover), the goal cap (a farther frontier
waits while a safe one lies within it, never
behind a probe, and never ends a run) and the
exploration radius around the first pose
(nothing left inside it is the end of a run)
G. purity - the costmap, the pose and the rest of the
state come back untouched, and the function
is deterministic
J. the loop around it - which goal a bare goal_reached is credited to,
and the one line that states exp, cap, radius
L. the blueprint's knobs - MOSS_EXPLORE_DISTANCE_EXP, MOSS_GOAL_MAX_M,
MOSS_EXPLORE_RADIUS_M -> the Explorer2 config
No robot and no LCM. A to I, K and G need numpy and scipy only (the geometry is
built on the default Tuning, a 0.50 m wide reference body). Only J and L touch
the module class and the blueprint, and they skip themselves where dimOS is
not installed.
Run from the repository root: python benches/bench_explorer2_cold.py
"""
import math
import sys
import threading
import time
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parent.parent
sys.path.insert(0, str(ROOT))
# moss_dimos/__init__.py imports nothing, so this works with or without dimOS
import moss_dimos.explorer2 as _explorer2_module # noqa: E402
from moss_dimos.explorer2 import ( # noqa: E402
DIRECTIVE_BACK_OFF, DIRECTIVE_FRONTIER, DIRECTIVE_WAIT, DEFAULT_TUNING,
ExploreState, PoseStamped, Tuning, WAIT_REOPEN_POLLS, next_target,
)
FREE, UNKNOWN, OCCUPIED = 0, -1, 100
RES = 0.05
OK = 0
KO = 0
def check(label, ok, detail=""):
global OK, KO
print(f" {'OK ' if ok else 'KO '} {label}{' - ' + detail if detail else ''}")
if ok:
OK += 1
else:
KO += 1
# --- the smallest thing that looks like a costmap --------------------------
class Grid:
"""Everything next_target reads off an OccupancyGrid, and nothing else."""
def __init__(self, grid, res=RES, ox=0.0, oy=0.0, ts=0.0):
self.grid = grid
self.resolution = res
self.frame_id = "world"
self.ts = ts
self.origin = type("O", (), {
"position": type("P", (), {"x": ox, "y": oy, "z": 0.0})()})()
def pose(x, y, yaw=0.0):
p = PoseStamped(ts=0.0, frame_id="world")
p.position.x, p.position.y = x, y
p.orientation.z, p.orientation.w = math.sin(yaw / 2), math.cos(yaw / 2)
return p
def cell(x, y, ox=0.0, oy=0.0, res=RES):
return int((y - oy) / res), int((x - ox) / res)
def room(n=140, x0=30, x1=110, y0=30, y1=110, walls=True):
"""A square of free floor in the middle of unknown, walled all round.
n x n cells at 5 cm; with the defaults that is 7 x 7 m of grid holding a
4 x 4 m room. Openings are punched into the wall afterwards.
"""
g = np.full((n, n), UNKNOWN, dtype=np.int8)
g[y0:y1, x0:x1] = FREE
if walls:
g[y0 - 1:y1 + 1, x0 - 1] = OCCUPIED
g[y0 - 1:y1 + 1, x1] = OCCUPIED
g[y0 - 1, x0 - 1:x1 + 1] = OCCUPIED
g[y1, x0 - 1:x1 + 1] = OCCUPIED
return g
def door(g, side, centre, width_cells, x0=30, x1=110, y0=30, y1=110):
"""Punch an opening of `width_cells` in one wall, centred on `centre`."""
half = width_cells // 2
if side == "east":
g[centre - half:centre + half, x1] = UNKNOWN
elif side == "west":
g[centre - half:centre + half, x0 - 1] = UNKNOWN
elif side == "north":
g[y1, centre - half:centre + half] = UNKNOWN
elif side == "south":
g[y0 - 1, centre - half:centre + half] = UNKNOWN
return g
def w_of(cell_index):
"""Cell index -> the world coordinate of its centre, origin at 0."""
return (cell_index + 0.5) * RES
# ===========================================================================
print("A. a room with one door: the target stands on free floor, at the door")
g = door(room(), "east", 70, 24)
cm = Grid(g)
here = pose(w_of(70), w_of(70), 0.0) # middle of the room, facing +x
st = ExploreState()
target = next_target(cm, here, st, now=100.0)
check("a target comes back", target is not None)
assert target is not None
check("directive is 'frontier'", target.directive == DIRECTIVE_FRONTIER, target.directive)
gy, gx = cell(target.position.x, target.position.y)
check("the target stands on FREE floor, not in the unknown",
g[gy, gx] == FREE and not target.on_frontier, f"grid value {g[gy, gx]}")
check("the target is at the door, not at the centroid of the room",
target.position.x > w_of(100), f"x = {target.position.x:.2f} m")
check("the look-at point is in the unknown beyond the door",
g[cell(*target.look_at_xy)] == UNKNOWN)
nearest_wall = min(math.hypot(target.position.x - w_of(cx), target.position.y - w_of(cy))
for cy, cx in zip(*np.nonzero(g == OCCUPIED)))
check("it is at least a body radius from any wall",
nearest_wall >= DEFAULT_TUNING.lethal_clearance_m,
f"{nearest_wall:.2f} m >= {DEFAULT_TUNING.lethal_clearance_m} m")
check("path cost is the real walking distance, not a straight line",
target.path_cost_m >= abs(target.position.x - here.position.x) - 1e-6,
f"{target.path_cost_m:.2f} m")
check("state: one target issued, one visited, one observed pose",
(st.targets_issued, len(st.visited), len(st.observed)) == (1, 1, 1),
f"{st.targets_issued}, {len(st.visited)}, {len(st.observed)}")
# ===========================================================================
print("B. prefer-forward: two mirror-image doors, the one ahead wins")
g = door(door(room(), "east", 70, 24), "west", 70, 24)
cm = Grid(g)
centre = w_of(70)
east = next_target(cm, pose(centre, centre, 0.0), ExploreState(), now=100.0)
west = next_target(cm, pose(centre, centre, math.pi), ExploreState(), now=100.0)
check("facing +x picks the east door", east is not None and east.position.x > centre,
f"x = {east.position.x:.2f}" if east else "None")
check("facing -x picks the west door", west is not None and west.position.x < centre,
f"x = {west.position.x:.2f}" if west else "None")
check("the two doors really are the same cluster size and the same distance",
east is not None and west is not None
and east.info_cells == west.info_cells
and abs(east.path_cost_m - west.path_cost_m) < 0.11,
f"{east.info_cells} vs {west.info_cells} cells, "
f"{east.path_cost_m:.2f} vs {west.path_cost_m:.2f} m" if east and west else "")
sideways = next_target(cm, pose(centre, centre, math.pi / 2), ExploreState(), now=100.0)
check("facing sideways, neither bonus applies and one is still chosen",
sideways is not None and sideways.directive == DIRECTIVE_FRONTIER)
# ===========================================================================
print("C. failed-target memory: an exclusion WAITS, reopens on triggers, never a clock")
g = door(room(), "east", 70, 24)
cm = Grid(g)
here = pose(centre, centre, 0.0)
probe = next_target(cm, here, ExploreState(), now=0.0)
assert probe is not None
gx, gy = probe.position.x, probe.position.y
st = ExploreState()
st.note_failed(gx, gy, (centre, centre), cm)
waited = next_target(cm, here, st, now=10.0)
check("the only cluster is excluded -> NOT None", waited is not None)
assert waited is not None
check("directive is 'wait'", waited.directive == DIRECTIVE_WAIT, waited.directive)
check("the rover is told to stay where it is",
(waited.position.x, waited.position.y) == (here.position.x, here.position.y))
check("the cluster is still counted as a frontier, not as absence",
waited.n_clusters == 1 and waited.n_excluded == 1,
f"{waited.n_clusters} clusters, {waited.n_excluded} excluded")
much_later = next_target(cm, here, st, now=1e9)
check("NO clock reopens it: same map, same pose, a billion seconds later -> still wait",
much_later is not None and much_later.directive == DIRECTIVE_WAIT,
much_later.directive if much_later else "None")
# starvation escape: after WAIT_REOPEN_POLLS silent asks the oldest entry reopens
starved = ExploreState()
starved.note_failed(gx, gy, (centre, centre), cm)
out = None
for _ in range(WAIT_REOPEN_POLLS + 2):
out = next_target(cm, here, starved, now=10.0)
if out is not None and out.directive == DIRECTIVE_FRONTIER:
break
check(f"starved {WAIT_REOPEN_POLLS} asks -> the oldest failed goal reopens and is targeted",
out is not None and out.directive == DIRECTIVE_FRONTIER and starved.failed == [],
out.directive if out else "None")
# trigger 1: the rover stands a viewpoint away from where it failed
moved_st = ExploreState()
moved_st.note_failed(gx, gy, (centre, centre), cm)
moved = next_target(cm, pose(centre - 1.2, centre, 0.0), moved_st, now=10.0)
check("the rover 1.2 m from where it failed -> the exclusion reopens (viewpoint trigger)",
moved is not None and moved.directive == DIRECTIVE_FRONTIER,
moved.directive if moved else "None")
check("and the reopened entry is pruned from the state", moved_st.failed == [],
f"{moved_st.failed}")
# trigger 2: the map around the goal changed (new cells observed)
changed_st = ExploreState()
changed_st.note_failed(gx, gy, (centre, centre), cm)
g_changed = g.copy()
cgx, cgy = int((gx - cm.origin.position.x) / cm.resolution), int((gy - cm.origin.position.y) / cm.resolution)
patch = g_changed[cgy - 4:cgy + 5, cgx - 4:cgx + 5]
patch[patch == UNKNOWN] = FREE # the world got observed there
changed = next_target(Grid(g_changed), here, changed_st, now=10.0)
check("new observations around the failed goal -> the exclusion reopens (map trigger)",
changed is not None and changed.directive == DIRECTIVE_FRONTIER,
changed.directive if changed else "None")
# two exclusions: both held -> wait; one held -> the other is simply used
g2 = door(door(room(), "east", 70, 24), "west", 70, 24)
cm2 = Grid(g2)
e = next_target(cm2, pose(centre, centre, 0.0), ExploreState(), now=0.0)
wst = next_target(cm2, pose(centre, centre, math.pi), ExploreState(), now=0.0)
assert e is not None and wst is not None
both = ExploreState()
both.note_failed(e.position.x, e.position.y, (centre, centre), cm2)
both.note_failed(wst.position.x, wst.position.y, (centre, centre), cm2)
waited2 = next_target(cm2, pose(centre, centre, 0.0), both, now=45.0)
check("with two exclusions held -> wait, both counted",
waited2 is not None and waited2.directive == DIRECTIVE_WAIT
and waited2.n_excluded == 2,
f"{waited2.n_excluded if waited2 else None} excluded")
one = ExploreState()
one.note_failed(e.position.x, e.position.y, (centre, centre), cm2)
other = next_target(cm2, pose(centre, centre, 0.0), one, now=45.0)
check("only one of two excluded -> the other is targeted, no wait",
other is not None and other.directive == DIRECTIVE_FRONTIER
and other.position.x < centre,
f"{other.directive} x={other.position.x:.2f}" if other else "None")
# ===========================================================================
print("D. born cornered: the pocket signature gives ONE back-off")
# 3 x 3 free cells (0.0225 m2) sealed by a ring of obstacle, unknown beyond.
g = np.full((40, 40), UNKNOWN, dtype=np.int8)
g[19:22, 19:22] = FREE
g[18:23, 18] = OCCUPIED
g[18:23, 22] = OCCUPIED
g[18, 18:23] = OCCUPIED
g[22, 18:23] = OCCUPIED
cm = Grid(g)
here = pose(w_of(20), w_of(20), 0.0)
st = ExploreState()
first = next_target(cm, here, st, now=100.0)
check("a cornered rover gets a directive, not None", first is not None)
assert first is not None
check("directive is 'back_off'", first.directive == DIRECTIVE_BACK_OFF, first.directive)
check("the back-off is 0.22 m straight behind the heading",
abs(first.position.x - (here.position.x - 0.22)) < 1e-9
and abs(first.position.y - here.position.y) < 1e-9,
f"({first.position.x:.3f}, {first.position.y:.3f})")
check("it reports the pocket it measured: 9 cells = 0.0225 m2",
abs(first.reachable_free_m2 - 0.0225) < 1e-9, f"{first.reachable_free_m2}")
check("state remembers the back-off went out", st.back_off_issued is True)
second = next_target(cm, here, st, now=101.0)
check("the SAME pocket does not give a second back-off: it gives None",
second is None, str(second))
# facing another way, the back-off follows the heading
st3 = ExploreState()
turned = next_target(cm, pose(w_of(20), w_of(20), math.pi / 2), st3, now=100.0)
check("heading +y -> back-off is 0.22 m in -y",
turned is not None and abs(turned.position.y - (w_of(20) - 0.22)) < 1e-9
and abs(turned.position.x - w_of(20)) < 1e-9,
f"({turned.position.x:.3f}, {turned.position.y:.3f})" if turned else "None")
# ===========================================================================
print("E. blocked region: a block published as occupied is never targeted")
# The room's only door leads into a corridor the map publishes as occupied (100),
# which is all the explorer ever sees of an obstacle.
g = door(room(), "east", 70, 24)
keep = np.zeros_like(g, dtype=bool)
keep[58:82, 110:118] = True # the corridor behind the door
g[keep] = OCCUPIED
cm = Grid(g)
st = ExploreState()
blocked = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)
check("with the only way out forbidden, exploration ends: None",
blocked is None, str(blocked))
# now give the room a second, legal door and check the target never lands in
# the forbidden block, nor within a body radius of it
g2 = door(g.copy(), "north", 70, 24)
cm2 = Grid(g2)
seen_in_zone = 0
closest = math.inf
st2 = ExploreState()
for i in range(12):
t = next_target(cm2, pose(centre, centre, 0.0), st2, now=100.0 + i)
if t is None or t.directive != DIRECTIVE_FRONTIER:
break
ty, tx = cell(t.position.x, t.position.y)
if keep[ty, tx]:
seen_in_zone += 1
for ky, kx in zip(*np.nonzero(keep)):
closest = min(closest, math.hypot(t.position.x - w_of(kx), t.position.y - w_of(ky)))
check("no target ever lands inside the forbidden block", seen_in_zone == 0,
f"{seen_in_zone} did")
check("and none comes within the lethal clearance of it",
closest >= DEFAULT_TUNING.lethal_clearance_m,
f"closest {closest:.2f} m >= {DEFAULT_TUNING.lethal_clearance_m} m")
# ===========================================================================
print("F. the end of a run: None exactly when there is nothing left")
# a completely known map: free room, wall, and solid obstacle everywhere else
g = np.full((140, 140), OCCUPIED, dtype=np.int8)
g[30:110, 30:110] = FREE
cm = Grid(g)
st = ExploreState()
done = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)
check("a map with no unknown cell at all -> None", done is None, str(done))
check("and it is not the cornered branch: the floor is 16 m2, not a pocket",
st.back_off_issued is False and st.last_directive == "")
# one unknown cell short of complete, but too small to be a frontier cluster
g2 = g.copy()
g2[60:62, 60:62] = UNKNOWN # 4 unknown cells, min cluster is 6
check("an unknown speck below min_frontier_perimeter is not a frontier -> None",
next_target(Grid(g2), pose(centre, centre, 0.0), ExploreState(), now=100.0) is None)
g3 = g.copy()
g3[60:70, 60:70] = UNKNOWN # 100 unknown cells in the middle of the room
kept = next_target(Grid(g3), pose(w_of(100), w_of(100), 0.0), ExploreState(), now=100.0)
check("a real unknown pocket inside the room IS a frontier -> a target",
kept is not None and kept.directive == DIRECTIVE_FRONTIER,
kept.directive if kept else "None")
# ===========================================================================
print("H. fresh ground beats ground already worked")
# Two mirror-image doors. Test B has already shown that, facing east, the east
# door wins. Now the rover has already been sent to the east door once - and
# has been somewhere else since - so the east door is a REVISIT.
g = door(door(room(), "east", 70, 24), "west", 70, 24)
cm = Grid(g)
centre = w_of(70)
plain = next_target(cm, pose(centre, centre, 0.0), ExploreState(), now=100.0)
assert plain is not None
east_goal = (plain.position.x, plain.position.y)
check("(setup) facing east with no memory, the east door is the pick",
east_goal[0] > centre, f"x = {east_goal[0]:.2f}")
faded = next_target(cm, pose(centre, centre, 0.0),
ExploreState(visited=[east_goal, (-9.0, -9.0)]), now=100.0)
check("a door already published, with a goal since, loses to the fresh one",
faded is not None and faded.position.x < centre,
f"x = {faded.position.x:.2f}" if faded else "None")
# ... but the goal it was driving to when it last stopped is not a revisit:
# that one is unfinished business and must not be dropped.
current = next_target(cm, pose(centre, centre, 0.0),
ExploreState(visited=[(-9.0, -9.0), east_goal]), now=100.0)
check("the goal just attempted keeps its score: no fade on unfinished business",
current is not None and current.position.x > centre,
f"x = {current.position.x:.2f}" if current else "None")
# The fade is the SQUARE of the distance: at half the revisit radius a cluster
# keeps a quarter of its score, not a half. One door, so the same cluster comes
# back either way and only the fade differs.
one_door = Grid(door(room(), "east", 70, 24))
clean = next_target(one_door, pose(centre, centre, 0.0), ExploreState(), now=100.0)
assert clean is not None
half = (clean.position.x - DEFAULT_TUNING.revisit_radius_m / 2, clean.position.y)
faded_half = next_target(one_door, pose(centre, centre, 0.0),
ExploreState(visited=[half, (-9.0, -9.0)]), now=100.0)
ratio = (faded_half.score / clean.score) if faded_half is not None else None
check("a cluster half a revisit radius from an older goal keeps a QUARTER of its score",
ratio is not None and abs(ratio - 0.25) < 1e-9, f"ratio {ratio}")
# ===========================================================================
print("H2. the gain is what the viewpoint can SEE, not unknown behind the wall")
# Same room, two doors. Behind the east door, a cupboard: 0.35 m of unknown
# closed by walls. Behind the west door, open unknown. The two frontiers have
# the same cell count and the same distance, and the rover faces EAST, so a
# gain that counts unknown through a wall picks the cupboard.
g = door(door(room(), "east", 70, 24), "west", 70, 24)
g[57:83, 117] = OCCUPIED # cupboard back wall, 0.35 m beyond the door
g[57, 110:118] = OCCUPIED # and its two sides
g[82, 110:118] = OCCUPIED
cm = Grid(g)
seen = next_target(cm, pose(centre, centre, 0.0), ExploreState(), now=100.0)
check("facing the cupboard, the rover still takes the door with a room behind it",
seen is not None and seen.position.x < centre,
f"x = {seen.position.x:.2f}" if seen else "None")
check("and the gain it reports is a fraction of a disc, not a cell count",
seen is not None and 0.0 <= seen.info_gain <= 1.0, f"{seen.info_gain}" if seen else "")
# ===========================================================================
print("I. shut in is not finished")
# A room the body cannot leave: its only opening is 0.50 m wide, and the default
# (reference body) clearance needs 0.60 m to pass. Beyond it, a corridor and
# real unknown.
def pinched_map():
g = np.full((160, 160), OCCUPIED, dtype=np.int8)
g[30:110, 30:110] = FREE # the room the rover is in
g[65:75, 110:140] = FREE # a 0.50 m x 1.50 m throat, floor seen
g[30:110, 140:155] = UNKNOWN # the room beyond, never seen
return g
g = pinched_map()
cm = Grid(g)
st = ExploreState()
shut_in = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)
check("a rover shut in with frontiers left on the map does NOT report completion",
shut_in is not None, str(shut_in))
assert shut_in is not None
check("it gets one back-off: reverse once and look again",
shut_in.directive == DIRECTIVE_BACK_OFF, shut_in.directive)
check("and it says how many clusters the map still holds",
getattr(shut_in, "n_on_the_map", 0) >= 1, f"{getattr(shut_in, 'n_on_the_map', 0)}")
# NOT the born-cornered case: the 4 x 4 m room, minus the 0.30 m the body
# cannot use along each wall, is a 3.50 x 3.50 m square = 12.25 m2, plus the
# six cells at the mouth of the throat where the wall opens out and the
# clearance rises back over 0.30 m: 12.265 m2, twenty-four times the 0.5 m2
# that means "born cornered".
check("this is NOT the born-cornered case: 12.265 m2 of floor under it",
abs(getattr(shut_in, "reachable_free_m2", 0.0) - 12.265) < 1e-9,
f"{getattr(shut_in, 'reachable_free_m2', float('nan')):.4f} m2")
again = next_target(cm, pose(centre, centre, 0.0), st, now=101.0)
check("one back-off per pocket and no more: then the run ends", again is None, str(again))
# ===========================================================================
print("I2. a frontier the body cannot walk to is looked at from where it can stand")
# The same throat, but the unknown starts right at its mouth: the frontier is
# 0.55 m from floor the body can stand on. Dropping a frontier like this would
# let the run declare itself finished with it still on the map.
g = np.full((160, 160), OCCUPIED, dtype=np.int8)
g[30:110, 30:110] = FREE
g[65:75, 110:116] = FREE # 0.50 m throat, 0.30 m of seen floor
g[62:78, 116:150] = UNKNOWN # the unmapped room, opening out
cm = Grid(g)
st = ExploreState()
peek = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)
check("a target comes back rather than None", peek is not None, str(peek))
assert peek is not None
check("it is a frontier goal", peek.directive == DIRECTIVE_FRONTIER, peek.directive)
gy, gx = cell(peek.position.x, peek.position.y)
check("the rover is told to stand on floor it can reach, not in the unknown",
g[gy, gx] == FREE and not peek.on_frontier, f"grid {g[gy, gx]}")
nearest_wall = min(math.hypot(peek.position.x - w_of(cx), peek.position.y - w_of(cy))
for cy, cx in zip(*np.nonzero(g == OCCUPIED)))
check("and it stands a body radius clear of the walls",
nearest_wall >= DEFAULT_TUNING.lethal_clearance_m,
f"{nearest_wall:.2f} m")
check("the look-at point is the unknown behind the throat",
peek.look_at_xy[0] > w_of(110), f"{peek.look_at_xy}")
# ===========================================================================
print("I3. a frontier is retired by being SEEN, not by a viewpoint being used")
# One door, one cluster. The rover drives to the target and decides from there;
# the map does not change (the shadow the 2D scan plane never enters). Standing
# in plain view of it and learning nothing is the end of that frontier - this
# is the 300-goals-and-184-m protection, and it must still hold.
g = door(room(), "east", 70, 24)
cm = Grid(g)
st = ExploreState()
first = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)
assert first is not None
arrived = next_target(cm, pose(first.position.x, first.position.y, 0.0), st, now=101.0)
check("looked straight at it from close up and nothing changed: that cluster is done",
arrived is None, str(arrived))
# Same distance, same everything, one wall of difference. A closed room with
# one unknown pocket in the middle of its floor, and a spot the rover has
# already decided from, 0.70 m from that pocket.
def pocket_room(partition: bool):
g = np.full((140, 140), OCCUPIED, dtype=np.int8)
g[30:110, 30:110] = FREE
g[66:74, 66:74] = UNKNOWN # 0.40 x 0.40 m of unknown floor
if partition:
g[60:80, 80] = OCCUPIED # a screen between the spot and the pocket
return g
stood_at = (w_of(84), w_of(70)) # 0.50 m east of the pocket's edge
in_view = next_target(Grid(pocket_room(False)), pose(*stood_at, 0.0),
ExploreState(observed=[stood_at]), now=100.0)
check("a pocket in plain view of a spot already decided from is retired",
in_view is None, str(in_view))
hidden = next_target(Grid(pocket_room(True)), pose(*stood_at, 0.0),
ExploreState(observed=[stood_at]), now=100.0)
check("the same pocket behind a screen is NOT retired: it was never seen",
hidden is not None and hidden.directive == DIRECTIVE_FRONTIER,
(hidden.directive if hidden else "None"))
if hidden is not None:
check("and the viewpoint it is given is on the pocket's side of the screen",
hidden.position.x < w_of(80), f"x = {hidden.position.x:.2f} m")
# ===========================================================================
print("I4. and it always terminates: no map, however awkward, spins for ever")
for label, grid_in in (("one door", door(room(), "east", 70, 24)),
("three doors", door(door(door(room(), "east", 70, 24),
"west", 70, 24), "north", 70, 24)),
("shut in", pinched_map())):
st = ExploreState()
here = pose(centre, centre, 0.0)
calls = 0
for i in range(60):
calls += 1
t = next_target(Grid(grid_in), here, st, now=100.0 + i)
if t is None:
break
if t.directive == DIRECTIVE_FRONTIER:
here = pose(t.position.x, t.position.y, 0.0) # the rover arrives
check(f"{label}: the map runs out and the function says so ({calls} calls)",
t is None and calls < 60, f"{calls} calls, last {t}")
# ===========================================================================
print("K. how far: the distance exponent, the goal cap, the exploration radius")
# A hall, 6 x 2 m, walled on three sides. Its east end opens onto 1 m of
# unknown (the far frontier: 38 cells, the whole width of the hall). Halfway
# along the north wall, a 0.50 m door into a 1.0 x 0.6 m closet (the near
# frontier: 8 cells). Heading is taken out of the picture (forward_bonus 0) so
# every score below is exactly gain / (1 + path) ** exponent.
#
# With `bay`, a 1.0 m opening in the south wall (x 2.0 .. 3.0 m) gives onto a
# 0.10 m strip of unknown, then 0.25 m of seen floor, then a 0.70 m unknown
# pocket closed by a wall. The pocket's frontier is looked at from that seen
# floor, which the rover can only reach across the unknown strip: a PROBE.
def hall(closet=True, bay=False):
g = np.full((120, 220), UNKNOWN, dtype=np.int8)
g[40:80, 20:140] = FREE # x 1.0 .. 7.0 m, y 2.0 .. 4.0 m
g[39:81, 19] = OCCUPIED # west wall
g[39, 19:161] = OCCUPIED # south wall, on past the open end
g[80, 19:161] = OCCUPIED # north wall, likewise
g[39:81, 160] = OCCUPIED # the far pocket's back wall, 1 m out
if closet:
g[80:93, 57] = OCCUPIED # closet sides, x 2.85 and 3.90 m
g[80:93, 78] = OCCUPIED
g[93, 57:79] = OCCUPIED # closet back, 0.6 m behind the door
g[80, 63:73] = UNKNOWN # the 0.50 m door, x 3.15 .. 3.65 m
if bay:
g[38:40, 40:60] = UNKNOWN # the opening and the unknown strip, y 1.90 .. 2.00 m
g[33:38, 40:60] = FREE # seen floor beyond it, y 1.65 .. 1.90 m
g[18:39, 39] = OCCUPIED # bay sides, x 1.95 and 3.00 m
g[18:39, 60] = OCCUPIED
g[18, 39:61] = OCCUPIED # its back wall, y 0.90 m
return g
def flat(**kw):
return Tuning(forward_bonus=0.0, **kw)
hall_cm = Grid(hall())
start = pose(w_of(50), w_of(60), 0.0) # (2.53, 3.03): 1.2 m from the closet's
# viewpoint, 4.5 m from the far end's
linear = next_target(hall_cm, start, ExploreState(), now=100.0,
tuning=flat(distance_exponent=1.0, max_goal_path_m=0.0))
squared = next_target(hall_cm, start, ExploreState(), now=100.0,
tuning=flat(distance_exponent=2.0, max_goal_path_m=0.0))
assert linear is not None and squared is not None
far_end = (linear.info_gain, linear.path_cost_m)
closet = (squared.info_gain, squared.path_cost_m)
for exp in (1.0, 2.0):
print(f" exponent {exp:g}: far end gain {far_end[0]:.4f} / (1 + {far_end[1]:.2f} m)^{exp:g} "
f"= {far_end[0] / (1 + far_end[1]) ** exp:.5f}, "
f"closet gain {closet[0]:.4f} / (1 + {closet[1]:.2f} m)^{exp:g} "
f"= {closet[0] / (1 + closet[1]) ** exp:.5f}")
check("(setup) two clusters, both on seen floor, the far one the bigger",
linear.n_clusters == 2 and not linear.probe and not squared.probe
and linear.info_cells > squared.info_cells and far_end[0] > closet[0],
f"{linear.info_cells} vs {squared.info_cells} cells, gain {far_end[0]:.4f} vs {closet[0]:.4f}")
check("(setup) the far end is 4.5 m of path away, the closet 1.2 m",
abs(far_end[1] - 4.5) < 0.1 and abs(closet[1] - 1.2) < 0.1,
f"{far_end[1]:.2f} m, {closet[1]:.2f} m")
check("exponent 1 (PR #2830's shape): the big far end wins",
linear.position.x > w_of(130), f"x = {linear.position.x:.2f} m")
check("exponent 2: the small closet beside the rover wins",
w_of(57) < squared.position.x < w_of(78) and squared.position.y > w_of(70),
f"({squared.position.x:.2f}, {squared.position.y:.2f})")
check("the score handed back is gain / (1 + path) ** exponent, exactly",
abs(linear.score - far_end[0] / (1 + far_end[1])) < 1e-12
and abs(squared.score - closet[0] / (1 + closet[1]) ** 2) < 1e-12,
f"{linear.score:.6f}, {squared.score:.6f}")
default = next_target(hall_cm, start, ExploreState(), now=100.0)
check("the defaults (exponent 2, cap 3 m) send the rover to the closet",
default is not None and abs(default.path_cost_m - closet[1]) < 1e-9
and DEFAULT_TUNING.distance_exponent == 2.0 and DEFAULT_TUNING.max_goal_path_m == 3.0,
f"{default.path_cost_m:.2f} m" if default else "None")
capped = next_target(hall_cm, start, ExploreState(), now=100.0,
tuning=flat(distance_exponent=1.0, max_goal_path_m=3.0))
check("cap 3 m, exponent 1: the 4.5 m far end is not a candidate, the closet goes out",
capped is not None and abs(capped.path_cost_m - closet[1]) < 1e-9
and capped.n_beyond_cap == 1 and capped.beyond_cap is False,
f"{capped.path_cost_m:.2f} m, {capped.n_beyond_cap} beyond" if capped else "None")
roomy = next_target(hall_cm, start, ExploreState(), now=100.0,
tuning=flat(distance_exponent=1.0, max_goal_path_m=5.0))
check("cap 5 m: the 4.5 m far end is a candidate again and wins",
roomy is not None and abs(roomy.path_cost_m - far_end[1]) < 1e-9 and roomy.n_beyond_cap == 0)
# The closet mapped, the far end the only frontier left: the cap must not end
# the run (only None does, and only when nothing is left).
bare_hall = Grid(hall(closet=False))
only_far = next_target(bare_hall, start, ExploreState(), now=100.0)
check("nothing within the cap: the far end still goes out, flagged, the run goes on",
only_far is not None and only_far.directive == DIRECTIVE_FRONTIER
and only_far.beyond_cap is True and only_far.path_cost_m > 3.0,
f"{only_far.path_cost_m:.2f} m, beyond_cap {only_far.beyond_cap}" if only_far else "None")
nearer = next_target(bare_hall, pose(w_of(90), w_of(60), 0.0), ExploreState(), now=100.0)
check("and once the rover is within 3 m of it, it is a plain candidate again",
nearer is not None and nearer.path_cost_m <= 3.0 and nearer.beyond_cap is False,
f"{nearer.path_cost_m:.2f} m" if nearer else "None")
# The cap against the probe penalty. In the hall with the bay and no closet,
# only the bay's probe lies within 3 m; the far end, safe, lies beyond. The
# rover faces the bay, so the heading bonus goes to the probe. The defaults
# throughout, except where probe_penalty is named.
bay_hall = Grid(hall(closet=False, bay=True))
facing_bay = pose(w_of(50), w_of(60), -math.pi / 2)
safe_far = next_target(bay_hall, facing_bay, ExploreState(), now=100.0)
no_penalty = next_target(bay_hall, facing_bay, ExploreState(), now=100.0,
tuning=Tuning(probe_penalty=1.0))
assert safe_far is not None and no_penalty is not None
penalised = no_penalty.score * DEFAULT_TUNING.probe_penalty
print(f" probe {no_penalty.path_cost_m:.2f} m away: {no_penalty.score:.5f}, "
f"x {DEFAULT_TUNING.probe_penalty:g} = {penalised:.5f}; "
f"safe far end {safe_far.path_cost_m:.2f} m away: {safe_far.score:.5f}")
check("(setup) the bay's frontier is a probe within the cap, the far end safe beyond it",
no_penalty.probe and no_penalty.path_cost_m < 3.0
and not safe_far.probe and safe_far.path_cost_m > 3.0,
f"probe {no_penalty.probe} at {no_penalty.path_cost_m:.2f} m, "
f"far end probe {safe_far.probe} at {safe_far.path_cost_m:.2f} m")
check("a probe within 3 m, a safe frontier at 4.5 m: under the defaults the safe one goes out",
safe_far.position.x > w_of(130) and safe_far.beyond_cap is True
and safe_far.n_beyond_cap == 1,
f"x = {safe_far.position.x:.2f} m, beyond_cap {safe_far.beyond_cap}, "
f"{safe_far.n_beyond_cap} beyond")
check("the probe penalty decides it, not the cap: at probe_penalty 1 the probe wins",
w_of(40) < no_penalty.position.x < w_of(60) and no_penalty.position.y < w_of(38)
and penalised < safe_far.score < no_penalty.score,
f"({no_penalty.position.x:.2f}, {no_penalty.position.y:.2f})")
# The closet back in: a safe frontier within the cap (1.2 m), the probe within
# it (1.7 m), and the far end, safe, beyond it (4.5 m).
both_hall = Grid(hall(closet=True, bay=True))
near_safe = next_target(both_hall, start, ExploreState(), now=100.0)
check("a safe frontier within 3 m, a safe one at 4.5 m: under the defaults the near one goes out",
near_safe is not None and not near_safe.probe
and w_of(57) < near_safe.position.x < w_of(78) and near_safe.position.y > w_of(70)
and near_safe.path_cost_m < 3.0 and near_safe.beyond_cap is False
and near_safe.n_beyond_cap == 1,
f"({near_safe.position.x:.2f}, {near_safe.position.y:.2f}) {near_safe.path_cost_m:.2f} m"
if near_safe else "None")
open_far = next_target(both_hall, start, ExploreState(), now=100.0,
tuning=flat(distance_exponent=1.0, max_goal_path_m=0.0))
held = next_target(both_hall, start, ExploreState(), now=100.0,
tuning=flat(distance_exponent=1.0, max_goal_path_m=3.0))
check("exponent 1: the far end wins with no cap; cap 3 m holds it back for the near safe one",
open_far is not None and open_far.position.x > w_of(130)
and held is not None and not held.probe and held.beyond_cap is False
and w_of(57) < held.position.x < w_of(78) and held.position.y > w_of(70),
f"no cap x = {open_far.position.x:.2f} m, cap 3 m "
f"({held.position.x:.2f}, {held.position.y:.2f})" if open_far and held else "None")
# The radius. The run starts at (4.03, 2.48): the closet's viewpoint is 1.5 m
# from there, the far end's 3.0 m.
anchor_pose = pose(w_of(80), w_of(49), 0.0)
ax, ay = anchor_pose.position.x, anchor_pose.position.y
st = ExploreState()
wide = next_target(hall_cm, anchor_pose, ExploreState(), now=100.0,
tuning=flat(max_goal_path_m=0.0))
assert wide is not None
d_far = math.hypot(wide.position.x - ax, wide.position.y - ay)
inside = next_target(hall_cm, anchor_pose, st, now=100.0,
tuning=flat(max_goal_path_m=0.0, explore_radius_m=2.0))
d_near = (math.hypot(inside.position.x - ax, inside.position.y - ay)
if inside is not None else float("nan"))
print(f" from the anchor: far end's target {d_far:.2f} m, closet's {d_near:.2f} m")
check("(setup) with no radius the far end wins from there, its target 3.0 m from the start",
wide.position.x > w_of(130) and abs(d_far - 3.0) < 0.05, f"{d_far:.2f} m")
check("radius 2 m: the target 3.0 m from the anchor is dropped, the one 1.5 m away kept",
inside is not None and abs(d_near - 1.5) < 0.05 and inside.n_outside_radius == 1,
f"{d_near:.2f} m, {getattr(inside, 'n_outside_radius', None)} outside")
check("the anchor is the first pose the run was given",
st.anchor == (ax, ay), f"{st.anchor}")
# Later in the same run: the closet is mapped and the rover has driven up the
# hall, 1.5 m from the far end's target. The radius is still centred where the
# run began, so the far end stays out, and nothing is left inside it.
up_hall = pose(w_of(110), w_of(60), 0.0)
fresh = next_target(bare_hall, up_hall, ExploreState(), now=100.0,
tuning=flat(max_goal_path_m=0.0, explore_radius_m=2.0))
check("(setup) a run STARTING up the hall has the far end inside its 2 m",
fresh is not None and fresh.position.x > w_of(130),
f"x = {fresh.position.x:.2f} m" if fresh else "None")
ended = next_target(bare_hall, up_hall, st, now=101.0,
tuning=flat(max_goal_path_m=0.0, explore_radius_m=2.0))
check("only out-of-radius frontiers left -> None: exploration complete",
ended is None, str(ended))
check("an end, not a back-off or a wait, and the anchor did not move",
st.last_directive == "" and st.back_off_issued is False and st.anchor == (ax, ay),
f"{st.last_directive!r}, {st.back_off_issued}, {st.anchor}")
unlimited = next_target(bare_hall, up_hall, st.copy(), now=102.0, tuning=flat(max_goal_path_m=0.0))
check("radius 0 is no limit: the same run, same anchor, goes on to the far end",
unlimited is not None and unlimited.position.x > w_of(130),
f"x = {unlimited.position.x:.2f} m" if unlimited else "None")
# ===========================================================================
print("G. purity: nothing but the state's own fields changes")
g = door(room(), "east", 70, 24)
grid_before = g.copy()
cm = Grid(g)
here = pose(centre, centre, 0.3)
pose_before = (here.position.x, here.position.y, here.orientation.z, here.orientation.w)
st = ExploreState(back_off_issued=False)
st.note_failed(1.0, 2.0, (here.position.x, here.position.y), cm) # robot has not moved since
before = st.copy()
out = next_target(cm, here, st, now=60.0)
check("the costmap is not written to", np.array_equal(g, grid_before))
check("the pose is not written to",
(here.position.x, here.position.y, here.orientation.z, here.orientation.w) == pose_before)
check("failed entries whose triggers have not fired are kept as they were",
st.failed == before.failed, f"{st.failed}")
check("back_off_issued untouched when nothing was cornered",
st.back_off_issued == before.back_off_issued)
check("heading was refreshed from the pose (0.3 rad)", abs(st.heading - 0.3) < 1e-6,
f"{st.heading}")
check("visited / observed / targets_issued / last_directive / anchor are the only other changes",
(len(st.visited), len(st.observed), st.targets_issued, st.last_directive, st.anchor)
== (1, 1, 1, DIRECTIVE_FRONTIER, (here.position.x, here.position.y)))
reopened = ExploreState()
reopened.note_failed(1.0, 2.0, (here.position.x - 5.0, here.position.y), cm) # failed 5 m from here
next_target(cm, here, reopened, now=61.0)
check("a failed entry whose rover has since moved a viewpoint away is pruned",
reopened.failed == [], f"{reopened.failed}")
a = next_target(cm, here, ExploreState(), now=100.0)
b = next_target(cm, here, ExploreState(), now=100.0)
check("same map + same pose + same state -> same answer",
a is not None and b is not None
and (a.position.x, a.position.y) == (b.position.x, b.position.y)
and a.score == b.score)
tuned = next_target(cm, here, ExploreState(), now=100.0,
tuning=Tuning(forward_bonus=0.0, revisit_radius_m=0.1))
check("tuning is honoured and is not global state",
tuned is not None
and next_target(cm, here, ExploreState(), now=100.0).score == a.score)
noclock = ExploreState()
noclock.note_failed(gx, gy, (here.position.x, here.position.y), cm)
check("no clock is read when `now` is given: a 1970 timestamp behaves",
next_target(cm, here, noclock, now=-1e9) is not None)
# ===========================================================================
print("J. the loop: a verdict owed to the PREVIOUS goal never excludes the new one")
# ===========================================================================
#
# The only group that needs dimOS: it drives Explorer2._run_exploration_loop
# itself, with next_target stubbed (A-I own the decisions, this owns the
# plumbing) and a scripted planner. goal_reached is a bare Bool with no goal
# identity in it, so the cancel of a goal that timed out unanswered could be
# credited to the goal published a millisecond earlier - an exclusion on ground
# the rover never drove to, killing every cluster within 0.6 m of it.
Explorer2 = getattr(_explorer2_module, "Explorer2", None)
if Explorer2 is None:
print(" -- skipped: no dimOS here, the module class does not exist")
else:
GOAL_TIMEOUT_S = 0.4
STUB_GOAL = (2.0, 3.0) # metres, the only target the stub hands out
START = (1.0, 3.0) # 1.00 m from it
STEP_M = 0.4 # what the rover covers per goal: > GOAL_PROGRESS_M
FRONTIER_LEFT = [True] # flipped by `explored()`: next_target then returns None
STUB_BEYOND_CAP = [False] # what the stub's target says about the goal cap
def stub_target(costmap, odom, state, **kwargs):
if not FRONTIER_LEFT[0]:
return None # "exploration complete: no reachable frontier left"
t = PoseStamped(ts=0.0, frame_id="world")
t.position.x, t.position.y = STUB_GOAL
t.beyond_cap = STUB_BEYOND_CAP[0]
return t
class DoneOut:
"""The `explore_done` Out, recording what the module publishes on it.
`transport` None reproduces a module built outside a blueprint - the
same guard `bump` uses."""
def __init__(self, wired=True):
self.transport = object() if wired else None
self.sent = []
def publish(self, msg):
self.sent.append(bool(getattr(msg, "data", False)))
class Planner:
"""The goal_request Out plus the planner behind it: `script` says what
the planner does the instant a goal is published - the worst case for
the race."""
transport = object()
def __init__(self, script):
self.script = script
self.goals = []
self.published_at = []
self.explorer = None
def publish(self, goal):
self.goals.append((round(float(goal.position.x), 2), round(float(goal.position.y), 2)))
self.published_at.append(time.perf_counter())
act = self.script.get(len(self.goals))
if act is not None:
act(self.explorer)
def gave_up(ex):
"""The planner publishes goal_reached=False (no path / goal abandoned)."""
ex._on_goal_reached(type("M", (), {"data": False})())
def drove(ex):
"""The rover covers STEP_M toward the goal; the planner says nothing."""
o = ex.latest_odometry
ex.latest_odometry = pose(float(o.position.x) + STEP_M, float(o.position.y))
def last(ex):
ex.exploration_active = False
def explored(ex):
"""The map is fully explored: the NEXT next_target returns None."""
FRONTIER_LEFT[0] = False
def acts(*fns):
return lambda ex: [f(ex) for f in fns]
def run_loop(script, wired=True, tuning=DEFAULT_TUNING):
FRONTIER_LEFT[0] = True
ex = object.__new__(Explorer2)
ex._state = ExploreState()
ex._last_goal = None
ex._last_goal_ok = None
ex._late_answer_owed = False
ex._goal_published_t = 0.0
ex._tuning = tuning
ex.goal_reached_event = threading.Event()
ex.stop_event = threading.Event()
ex.exploration_active = True
ex.latest_costmap = Grid(door(room(), "east", 70, 24))
ex.latest_odometry = pose(*START)
ex.config = type("Cfg", (), {"goal_timeout": GOAL_TIMEOUT_S})()
ex.bump = type("Bump", (), {"transport": None})()
ex.explore_done = DoneOut(wired)
ex.goal_request = Planner(script)
ex.goal_request.explorer = ex
real_next_target = _explorer2_module.next_target
_explorer2_module.next_target = stub_target
try:
ex._run_exploration_loop()
finally:
_explorer2_module.next_target = real_next_target
return ex, ex.goal_request, time.perf_counter()
# Goal 1 times out while the rover is still closing on it (0.40 m of 1.00 m),
# so it is re-decided, not excluded - and the planner still owes it a verdict.
# That verdict lands the instant goal 2 goes out, while the rover drives to
# goal 2 exactly as the planner plans for it.
ex, planner, ended = run_loop({1: drove, 2: acts(gave_up, drove, last)})
check("two goals went out", planner.goals == [STUB_GOAL, STUB_GOAL], f"{planner.goals}")
check("the late verdict excludes nothing: the rover never failed at (2.00, 3.00)",
ex._state.failed == [], f"{ex._state.failed}")
check("the goal in flight keeps its own 0.4 s timeout instead of an instant verdict",
ended - planner.published_at[1] >= GOAL_TIMEOUT_S,
f"{ended - planner.published_at[1]:.2f} s")
check("and the timeout then judges on the gap closed: 0.40 m of 1.00 m, so no exclusion",
ex._state.failed == [] and abs(float(ex.latest_odometry.position.x)
- (START[0] + 2 * STEP_M)) < 1e-6)
# The verdict owed to goal 1 is only swallowed inside LATE_VERDICT_S of goal 2's
# publish. A verdict landing after that window is goal 2's own (its "no path"
# after a replanning): credited, and the loop wakes at once instead of
# sleeping the whole goal_timeout (a 45 s pause on the floor otherwise).
_explorer2_module.LATE_VERDICT_S = 0.05
def gave_up_later(ex):
threading.Timer(0.15, lambda: gave_up(ex)).start()
ex, planner, ended = run_loop({1: drove, 2: acts(gave_up_later, last)})
_explorer2_module.LATE_VERDICT_S = 1.0
check("a verdict past the late window is credited to the goal in flight",
[(round(f[0], 2), round(f[1], 2)) for f in ex._state.failed] == [STUB_GOAL],
f"{ex._state.failed}")
check("and wakes the loop at once (0.15 s, not the 0.4 s timeout)",
ended - planner.published_at[1] < GOAL_TIMEOUT_S - 0.05,
f"{ended - planner.published_at[1]:.2f} s")
# Nothing is owed on the first goal of a run: a no-path verdict on it is the
# planner talking about THAT goal, and it must still bite at once.
ex, planner, ended = run_loop({1: acts(gave_up, last)})
check("a no-path verdict on the first goal is still credited",
[(round(f[0], 2), round(f[1], 2)) for f in ex._state.failed] == [STUB_GOAL],
f"{ex._state.failed}")
check("and it still wakes the loop at once (no 0.4 s slept for nothing)",
ended - planner.published_at[0] < GOAL_TIMEOUT_S,
f"{ended - planner.published_at[0]:.3f} s")
# An ANSWERED goal owes nothing either: two verdicts in a row are both real.
ex, planner, ended = run_loop({1: gave_up, 2: acts(gave_up, last)})
check("two answered goals in a row are both credited (the swallow is one deep)",
len(ex._state.failed) == 2, f"{ex._state.failed}")
# -- the end of the mission, SAID on the bus ------------------------------
# The base loop only sets exploration_active False, logs one line and
# breaks: nothing crosses the process boundary. explore_done is what tells
# the other processes (the cockpit) that the run is over.
ex, planner, ended = run_loop({1: acts(explored, gave_up)})
check("the loop ends on 'no reachable frontier left'", ex.exploration_active is False)
check("and explore_done=True goes out exactly once",
ex.explore_done.sent == [True], f"{ex.explore_done.sent}")
ex, planner, ended = run_loop({1: acts(gave_up, last)})
check("a run stopped from outside publishes NO explore_done "
"(that end already crosses on stop_explore_cmd)",
ex.explore_done.sent == [], f"{ex.explore_done.sent}")
ex, planner, ended = run_loop({1: acts(explored, gave_up)}, wired=False)
check("an unwired Out is never published to, and does not crash the loop",
ex.explore_done.sent == [] and ex.exploration_active is False)
# -- exp, cap and radius: said once, when exploration starts ----------
class Said:
"""Stands in for the module's logger and keeps every line."""
def __init__(self):
self.lines = []
def info(self, msg, *args, **kwargs):
self.lines.append(str(msg))
warning = error = debug = info
real_logger = _explorer2_module.logger
said = Said()
_explorer2_module.logger = said
try:
ex, planner, ended = run_loop({1: drove, 2: acts(drove, last)})
said_default = list(said.lines)
said.lines.clear()
ex, planner_r, ended = run_loop({1: acts(explored, gave_up)},
tuning=Tuning(explore_radius_m=4.0))
said_radius = list(said.lines)
said.lines.clear()
STUB_BEYOND_CAP[0] = True
ex, planner_c, ended = run_loop({1: acts(explored, gave_up)})
said_cap = list(said.lines)
finally:
STUB_BEYOND_CAP[0] = False
_explorer2_module.logger = real_logger
starts = [line for line in said_default if "exp 2, cap 3 m, radius 0 m" in line]
check("one line at the start says 'exp 2, cap 3 m, radius 0 m', and no goal line repeats it",
len(planner.goals) == 2 and len(starts) == 1 and said_default[0] == starts[0]
and sum("exp " in line for line in said_default) == 1,
f"{len(planner.goals)} goals; {said_default[:1]}")
check("a radius run says its radius, and its end says 'within 4 m of the start'",
any("radius 4 m" in line for line in said_radius[:1])
and any("no reachable frontier left within 4 m of the start" in line for line in said_radius),
f"{said_radius}")
cap_lines = [line for line in said_cap if line.startswith("goal ")]
check("a goal beyond the cap says so on its line, and a goal within it does not",
len(cap_lines) == 1 and "(beyond the cap: nothing safe within it)" in cap_lines[0]
and not any("beyond the cap" in line for line in said_default),
f"{cap_lines}")
# ===========================================================================
print("L. the blueprint: three env knobs -> the Explorer2 config")
# ===========================================================================
if Explorer2 is None:
print(" -- skipped: no dimOS here, the blueprint does not import")
else:
import os
import moss_dimos.blueprints as mb
from moss_dimos.explorer2 import Explorer2Config
check("unset: nothing passed, the explorer keeps its defaults", mb.explorer_env({}) == {})
check("'1', '0', ' 4 ' -> exponent 1.0, cap 0.0 m (no limit), radius 4.0 m",
mb.explorer_env({"MOSS_EXPLORE_DISTANCE_EXP": "1", "MOSS_GOAL_MAX_M": "0",
"MOSS_EXPLORE_RADIUS_M": " 4 "})
== {"distance_exponent": 1.0, "max_goal_path_m": 0.0, "explore_radius_m": 4.0})
refused = mb.explorer_env({"MOSS_EXPLORE_DISTANCE_EXP": "two", "MOSS_GOAL_MAX_M": "-1",
"MOSS_EXPLORE_RADIUS_M": "inf"})
check("'two', '-1', 'inf' are refused (warned): the defaults stay", refused == {}, f"{refused}")
steep = [mb.explorer_env({"MOSS_EXPLORE_DISTANCE_EXP": raw}) for raw in ("250", "7", "6")]
check("an exponent of 250 or 7 is refused (warned), 6 is the largest taken",
steep == [{}, {}, {"distance_exponent": 6.0}], f"{steep}")
try:
overflow = repr(31.0 ** 250)
except OverflowError as exc:
overflow = f"OverflowError {exc}"
check("(why) a float power of 250 raises instead of giving inf, at 6 it is finite",
overflow.startswith("OverflowError") and math.isfinite(31.0 ** 6),
f"31.0 ** 250: {overflow}")
check("the config's defaults are the Tuning's: exponent 2, cap 3 m, radius 0",
(Explorer2Config.model_fields["distance_exponent"].default,
Explorer2Config.model_fields["max_goal_path_m"].default,
Explorer2Config.model_fields["explore_radius_m"].default)
== (DEFAULT_TUNING.distance_exponent, DEFAULT_TUNING.max_goal_path_m,
DEFAULT_TUNING.explore_radius_m) == (2.0, 3.0, 0.0))
knobs = {"MOSS_CAMERA": "0", "MOSS_RECORD": "0", "MOSS_WRIST": "0",
"MOSS_EXPLORE_DISTANCE_EXP": "1.5", "MOSS_GOAL_MAX_M": "2.5", "MOSS_EXPLORE_RADIUS_M": "4"}
saved = {k: os.environ.get(k) for k in knobs}
os.environ.update(knobs)
try:
built = mb.build_moss_explore()
finally:
for k, v in saved.items():
if v is None:
os.environ.pop(k, None)
else:
os.environ[k] = v
kw = next(b for b in built.active_blueprints if b.module.__name__ == "Explorer2").kwargs
check("moss-explore built with the three set: Explorer2 gets 1.5, 2.5 m, 4 m",
(kw.get("distance_exponent"), kw.get("max_goal_path_m"), kw.get("explore_radius_m"))
== (1.5, 2.5, 4.0), f"{kw}")
cfg = Explorer2Config(**kw)
check("and its config holds them", (cfg.distance_exponent, cfg.max_goal_path_m,
cfg.explore_radius_m) == (1.5, 2.5, 4.0))
print(f"{OK} OK, {KO} KO")
if not KO:
print("TEST PASSED")
sys.exit(1 if KO else 0)