File size: 9,363 Bytes
247b0c2
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
2d1925c
 
 
 
 
 
 
 
 
 
 
 
 
 
247b0c2
 
 
 
 
 
 
 
4eecd03
 
 
 
 
 
 
 
 
 
 
 
 
 
 
247b0c2
 
4eecd03
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
#!/usr/bin/env python3
"""Fast geometry regression tests for robot_planner (no torch/cv2 needed).

Run with ``python3 test_robot_planner.py``. These pin the pure-numpy/scipy
behaviour: contour tracing, polygon simplification, homography round-trips, and
stroke planning/coverage, including the failure path when the roller cannot fit.
"""
import numpy as np

import robot_planner as rp


def rectangle(height, width, top, left, bottom, right):
    mask = np.zeros((height, width), dtype=bool)
    mask[top:bottom, left:right] = True
    return mask


def test_trace_follows_concavity():
    # L-shape: a convex hull would have 4 corners and bridge the notch.
    mask = rectangle(60, 60, 5, 5, 55, 20)
    mask |= rectangle(60, 60, 40, 5, 55, 55)
    traced = [(x, y) for y, x in rp.trace_boundary(mask)]
    simplified = rp.simplify_closed(traced, tolerance=2.0)
    assert 5 <= len(simplified) <= 8, f"expected ~6 corners for an L-shape, got {len(simplified)}"
    assert len(traced) > 40


def test_rectangle_simplifies_to_four_corners():
    traced = [(x, y) for y, x in rp.trace_boundary(rectangle(40, 70, 5, 6, 33, 61))]
    simplified = rp.simplify_closed(traced, tolerance=1.5)
    assert len(simplified) == 4, simplified


def test_homography_round_trip():
    corners = np.array([[10, 12], [410, 8], [420, 308], [5, 300]], float)
    matrix = rp.homography(corners, [(0, 0), (400, 0), (400, 300), (0, 300)])
    mapped = rp.apply_homography(matrix, corners)
    assert np.allclose(mapped, [[0, 0], [400, 0], [400, 300], [0, 300]], atol=1e-6)
    back = rp.apply_homography(np.linalg.inv(matrix), mapped)
    assert np.allclose(back, corners, atol=1e-6)


def test_plan_covers_open_wall():
    paintable = rectangle(400, 600, 20, 20, 380, 580)
    keepout = np.zeros_like(paintable)
    plan, debug = rp.build_plan(paintable, keepout, stroke_width=80, overlap=0.2, margin=20,
                                orientation="horizontal", corners=None, mm_per_unit=2.5,
                                boundary_tolerance=6.0)
    assert plan["warnings"] == [], plan["warnings"]
    assert plan["wall"]["corners"] == 4
    assert plan["plan"]["segments"] > 0
    assert plan["coverage"]["planned_fraction"] >= 0.8, plan["coverage"]
    assert plan["plan"]["path_length_mm"] > plan["plan"]["path_length_units"]
    assert debug["allowed"].any()


def test_plan_reports_infeasible_roller():
    paintable = rectangle(200, 200, 0, 0, 200, 200)
    keepout = np.zeros_like(paintable)
    plan, _ = rp.build_plan(paintable, keepout, stroke_width=900, overlap=0.2, margin=100,
                            orientation="horizontal", corners=None, mm_per_unit=None,
                            boundary_tolerance=4.0)
    assert plan["plan"]["segments"] == 0
    assert plan["warnings"] and "centre-line" in plan["warnings"][0]


def test_keepout_blocks_strokes():
    paintable = rectangle(300, 300, 10, 10, 290, 290)
    keepout = rectangle(300, 300, 0, 140, 300, 160)  # vertical stripe: splits every pass
    plan, _ = rp.build_plan(paintable, keepout, stroke_width=40, overlap=0.2, margin=10,
                            orientation="horizontal", corners=None, mm_per_unit=None,
                            boundary_tolerance=4.0)
    assert plan["plan"]["segments"] > plan["plan"]["passes"], "keep-out stripe should split passes"
    assert 0.0 < plan["coverage"]["planned_fraction"] < 1.0


def test_trim_tool_reaches_edges_and_fixtures():
    paintable = rectangle(400, 600, 20, 20, 380, 580)
    keepout = rectangle(400, 600, 0, 290, 400, 310)  # fixture stripe the roller keeps clear of
    roller_only, _ = rp.build_plan(paintable, keepout, stroke_width=90, overlap=0.2, margin=25,
                                   orientation="horizontal", corners=None, mm_per_unit=None,
                                   boundary_tolerance=4.0)
    combined, _ = rp.build_plan(paintable, keepout, stroke_width=90, overlap=0.2, margin=25,
                                orientation="horizontal", corners=None, mm_per_unit=None,
                                boundary_tolerance=4.0, trim_width=20, trim_overlap=0.3)
    assert roller_only["tools"] == ["roller"]
    assert combined["tools"] == ["roller", "trim"]
    assert combined["plan"]["trim"]["segments"] > 0
    assert combined["coverage"]["planned_fraction"] > roller_only["coverage"]["planned_fraction"]
    assert combined["coverage"]["remaining_fraction"] < roller_only["coverage"]["remaining_fraction"]
    assert combined["coverage"]["remaining_fraction"] < 0.05


def test_waypoints_carry_paint_flags():
    paintable = rectangle(200, 200, 10, 10, 190, 190)
    plan, _ = rp.build_plan(paintable, np.zeros_like(paintable), stroke_width=40, overlap=0.2, margin=10,
                            orientation="horizontal", corners=None, mm_per_unit=None, boundary_tolerance=4.0)
    waypoints = plan["plan"]["waypoints"]
    segments = plan["plan"]["segments"]
    assert waypoints[0]["paint"] is True
    assert sum(1 for w in waypoints if not w["paint"]) == max(0, segments - 1)
    assert all(set(("x", "y", "paint", "tool")) <= set(w) for w in waypoints)


def test_progress_state_round_trip_and_replan():
    import tempfile
    from pathlib import Path

    painted = rectangle(120, 160, 10, 10, 90, 150)
    with tempfile.TemporaryDirectory() as folder:
        path = Path(folder) / "state.npz"
        assert rp.load_state(path) is None
        rp.save_state(path, painted, (160, 120))
        loaded, size = rp.load_state(path)
        assert size == (160, 120) and np.array_equal(loaded, painted)
        # A fully painted target is recognised and yields no new strokes.
        plan, _ = rp.build_plan(painted, np.zeros_like(painted), stroke_width=30, overlap=0.2, margin=5,
                                orientation="horizontal", corners=None, mm_per_unit=None,
                                boundary_tolerance=3.0, already_painted=painted)
        assert plan["plan"]["segments"] == 0
        assert plan["coverage"]["already_painted_fraction"] == 1.0
        assert any("already fully painted" in w for w in plan["warnings"])


def test_progress_state_rejects_size_mismatch():
    paintable = rectangle(120, 160, 10, 10, 90, 150)
    try:
        rp.build_plan(paintable, np.zeros_like(paintable), stroke_width=30, overlap=0.2, margin=5,
                      orientation="horizontal", corners=None, mm_per_unit=None, boundary_tolerance=3.0,
                      already_painted=rectangle(100, 100, 0, 0, 100, 100))
    except ValueError as exc:
        assert "does not match plane" in str(exc)
    else:
        raise AssertionError("expected a size-mismatch ValueError")


def test_measured_plane_size_sets_true_aspect():
    # A foreshortened wall in the image: without the measured size the plane takes the
    # image's longest edges; with it, the plane has the wall's true proportions.
    paintable = rectangle(400, 600, 0, 0, 400, 600)
    corners = [(100, 80), (500, 120), (500, 300), (100, 340)]
    _, guessed = rp.build_plan(paintable, np.zeros_like(paintable), stroke_width=20, overlap=0.2, margin=5,
                               orientation="horizontal", corners=corners, mm_per_unit=None, boundary_tolerance=4.0)
    plan, measured = rp.build_plan(paintable, np.zeros_like(paintable), stroke_width=20, overlap=0.2, margin=5,
                                   orientation="horizontal", corners=corners, mm_per_unit=10.0,
                                   boundary_tolerance=4.0, plane_size=(366, 229))
    assert tuple(guessed["size"]) == (402, 260) and tuple(measured["size"]) == (366, 229)
    assert plan["plane_size"]["width"] == 366 and plan["plane_size"]["mm_per_unit"] == 10.0


def main():
    tests = [value for name, value in sorted(globals().items()) if name.startswith("test_")]
    for test in tests:
        test()
        print(f"ok  {test.__name__}")
    print(f"{len(tests)} tests passed")


def test_windows_are_passed_once_not_crossed_on_every_row():
    wall = rectangle(600, 900, 0, 0, 600, 900)
    windows = rectangle(600, 900, 150, 120, 360, 330) | rectangle(600, 900, 150, 560, 360, 780)
    plan, _ = rp.build_plan(wall, windows, 60, 0.2, 10, "horizontal", None, 5.0, 4.0)
    strokes = plan["plan"]["strokes"]
    painted = sum(abs(s[-1][0] - s[0][0]) for s in strokes)
    travel = sum(np.hypot(b[0][0] - a[-1][0], b[0][1] - a[-1][1]) for a, b in zip(strokes, strokes[1:]))
    assert travel / (travel + painted) < 0.25, travel / (travel + painted)      # the bug-fixed row sweep gives ~0.27 here
    old = rp.plan_strokes(wall & ~windows, 48, "horizontal", 60, order="rows")
    new = rp.plan_strokes(wall & ~windows, 48, "horizontal", 60, order="cells")
    key = lambda segs: sorted((min(s[0][0], s[-1][0]), max(s[0][0], s[-1][0]), s[0][1]) for s in segs)
    assert key(old) == key(new)                    # the same passes, only their order and direction changed
    assert plan["coverage"]["planned_fraction"] > 0.8


if __name__ == "__main__":
    main()


def test_adjacent_passes_stay_inside_the_wet_edge_window():
    import eval_paths as ep
    for name, (wall, keep) in ep.walls().items():
        r = ep.evaluate(wall, keep)
        assert r["wet_edge_max_s"] < 120, (name, r["wet_edge_max_s"])      # latex stays workable for minutes; 57 s worst here
        assert r["keepout_intrusion"] < 0.02, (name, r["keepout_intrusion"])