323 lines
15 KiB
Python
323 lines
15 KiB
Python
import sys
|
|
from pathlib import Path
|
|
import numpy as np
|
|
|
|
# Add project root to sys.path
|
|
root_dir = Path(__file__).resolve().parent.parent
|
|
if str(root_dir) not in sys.path:
|
|
sys.path.insert(0, str(root_dir))
|
|
|
|
import config
|
|
from utils.route_solver import (
|
|
solve_optimal_route, turn_radius_from_grid,
|
|
side_slope_magnitude, compute_gradients,
|
|
)
|
|
|
|
INSTANCE_DIR = root_dir / "instance"
|
|
PROJECT_ID = "upload_sample"
|
|
FILTER_KEY = "csf"
|
|
DTM_PATH = INSTANCE_DIR / PROJECT_ID / "terrain_models" / f"dtm_{FILTER_KEY}_smooth.npz"
|
|
|
|
|
|
def _valid_points_from_cost_surface(n=2):
|
|
"""Pick points that lie exactly on valid (buildable) cost-surface cells, so grid
|
|
snap distance is ~0 — required for the 1m required-point checks (I-103/I-104)."""
|
|
from utils.route_solver import _build_cost_surface
|
|
td = INSTANCE_DIR / PROJECT_ID / "terrain_models"
|
|
x_sub, y_sub, z_sub, valid_sub, *_ = _build_cost_surface(td, FILTER_KEY, "dtm", True)
|
|
ys, xs = np.where(valid_sub)
|
|
# Spread the chosen cells across the valid region.
|
|
picks = np.linspace(0, len(ys) - 1, n, dtype=int)
|
|
return [{"x": float(x_sub[xs[i]]), "y": float(y_sub[ys[i]]), "z": 0.0} for i in picks]
|
|
|
|
|
|
def _points_from_dtm():
|
|
"""Two valid (buildable) points across the terrain, derived from the real cost
|
|
surface so grid snap distance is ~0 and the fixture tracks the sample data."""
|
|
p = _valid_points_from_cost_surface(2)
|
|
return {"bp": p[0], "ep": p[1], "cp": [], "ap": []}
|
|
|
|
|
|
def _solve(options, points=None):
|
|
return solve_optimal_route(
|
|
project_id=PROJECT_ID,
|
|
filter_key=FILTER_KEY,
|
|
smooth=True,
|
|
points_data=points or _points_from_dtm(),
|
|
options=options,
|
|
instance_dir=INSTANCE_DIR,
|
|
)
|
|
|
|
|
|
def test_side_slope_axis():
|
|
"""Cross-slope must use the gradient PERPENDICULAR to travel (I-101).
|
|
|
|
On a plane tilted purely in +x (gx=s, gy=0):
|
|
- N/S travel (perp = ±x) sees the full cross slope s,
|
|
- E/W travel (along x) sees zero cross slope,
|
|
- a 45° diagonal sees s/√2.
|
|
"""
|
|
s = 0.3
|
|
gx, gy = s, 0.0 # plane rises only in x
|
|
# E/W travel: dc=±1, dr=0 -> perpendicular is y -> cross slope 0
|
|
assert abs(side_slope_magnitude(0, 1, gx, gy) - 0.0) < 1e-9
|
|
assert abs(side_slope_magnitude(0, -1, gx, gy) - 0.0) < 1e-9
|
|
# N/S travel: dr=±1, dc=0 -> perpendicular is x -> full slope s
|
|
assert abs(side_slope_magnitude(1, 0, gx, gy) - s) < 1e-9
|
|
assert abs(side_slope_magnitude(-1, 0, gx, gy) - s) < 1e-9
|
|
# Diagonal NE: perpendicular (-1,1)/√2 -> |gx*-1|/√2 = s/√2
|
|
assert abs(side_slope_magnitude(1, 1, gx, gy) - s / np.sqrt(2)) < 1e-9
|
|
# Symmetric check: plane tilted only in +y, E/W travel sees full slope
|
|
assert abs(side_slope_magnitude(0, 1, 0.0, s) - s) < 1e-9
|
|
assert abs(side_slope_magnitude(1, 0, 0.0, s) - 0.0) < 1e-9
|
|
print("[ok] side slope uses perpendicular (cross) axis for all directions")
|
|
|
|
|
|
def test_compute_gradients_axis_order():
|
|
"""compute_gradients returns (dz_dx, dz_dy) aligned to columns(x)/rows(y)."""
|
|
# z increases with x (columns): z[r,c] = 2*c, res=1 -> dz_dx=2, dz_dy=0
|
|
z = np.tile(np.arange(5, dtype=float) * 2.0, (4, 1)) # shape (rows=4, cols=5)
|
|
dz_dx, dz_dy = compute_gradients(z, 1.0)
|
|
assert np.allclose(dz_dx, 2.0), dz_dx
|
|
assert np.allclose(dz_dy, 0.0), dz_dy
|
|
print("[ok] compute_gradients axis order correct (dz_dx along columns)")
|
|
|
|
|
|
def test_cost_surface_is_reduced_grid():
|
|
"""The built cost surface must be the reduced ~2 m grid, not the full source grid
|
|
(계획서 I-102: 원본 전체 meshgrid 없이 2m 비용면 생성)."""
|
|
from utils.route_solver import _build_cost_surface, _load_dtm_grid
|
|
td = INSTANCE_DIR / PROJECT_ID / "terrain_models"
|
|
x_full, y_full, _, _ = _load_dtm_grid(td, FILTER_KEY, True)
|
|
src_cells = len(x_full) * len(y_full)
|
|
x_sub, y_sub, z_sub, valid_sub, dz_dx, dz_dy, grid_res = _build_cost_surface(td, FILTER_KEY, "dtm", True)
|
|
route_cells = z_sub.shape[0] * z_sub.shape[1]
|
|
assert z_sub.shape == (len(y_sub), len(x_sub))
|
|
assert route_cells <= config.ROUTE_MAX_COST_CELLS
|
|
assert route_cells < src_cells # genuinely downsampled
|
|
assert 1.5 <= grid_res <= 3.0 # actual spacing near the 2 m target
|
|
print(f"[ok] cost surface reduced: src_cells={src_cells:,} -> route_cells={route_cells:,}, grid_res={grid_res:.2f}m")
|
|
|
|
|
|
def test_size_guard_blocks_oversized_grid():
|
|
"""Predicted grid beyond the cell limit is rejected before allocation."""
|
|
from utils.route_solver import _build_cost_surface
|
|
td = INSTANCE_DIR / PROJECT_ID / "terrain_models"
|
|
saved = config.ROUTE_MAX_COST_CELLS
|
|
config.ROUTE_MAX_COST_CELLS = 1000
|
|
try:
|
|
_build_cost_surface(td, FILTER_KEY, "dtm", True)
|
|
raise AssertionError("size guard did not trigger")
|
|
except ValueError:
|
|
print("[ok] size guard rejects oversized cost surface")
|
|
finally:
|
|
config.ROUTE_MAX_COST_CELLS = saved
|
|
|
|
|
|
def test_turn_radius_monotonic():
|
|
"""Sharper turns (larger diff) imply a smaller radius; straight is infinite."""
|
|
res = 2.0
|
|
assert turn_radius_from_grid(0, res) == float("inf")
|
|
r45 = turn_radius_from_grid(1, res)
|
|
r90 = turn_radius_from_grid(2, res)
|
|
assert r45 > r90 > 0
|
|
print(f"[ok] turn radius: 45deg={r45:.2f}m, 90deg={r90:.2f}m")
|
|
|
|
|
|
def test_grade_hard_constraint_caps_max_grade():
|
|
"""The resolved max-grade is a hard constraint: the produced route must not exceed
|
|
it (a small tolerance covers the spline smoothing post-process)."""
|
|
limit = config.FOREST_ROAD_MAX_GRADE["work"]
|
|
res = _solve({"grade_class": "work", "min_curve_radius_m": 8.0, "paved": False})
|
|
assert res["polyline"], "경로가 생성되어야 합니다"
|
|
assert res["metrics"]["max_grade_pct"] <= limit * 100 + 2.0, res["metrics"]
|
|
print(f"[ok] grade hard-capped: max_grade={res['metrics']['max_grade_pct']}% "
|
|
f"(limit {limit*100:.0f}%), length={res['metrics']['length_m']}m")
|
|
|
|
|
|
def test_grade_class_changes_feasibility():
|
|
"""Work roads (steeper limit) can cross terrain that trunk roads cannot, proving
|
|
grade_class actually drives the grade limit."""
|
|
pts = _points_from_dtm()
|
|
work = _solve({"grade_class": "work", "min_curve_radius_m": 8.0, "paved": False}, pts)
|
|
assert work["polyline"]
|
|
try:
|
|
_solve({"grade_class": "trunk", "min_curve_radius_m": 8.0, "paved": False}, pts)
|
|
trunk_ok = True
|
|
except ValueError:
|
|
trunk_ok = False
|
|
# On this steep sample, trunk (14%) should be the harder/failing case.
|
|
print(f"[ok] grade_class drives limit: work=feasible, trunk_feasible={trunk_ok}")
|
|
|
|
|
|
def _count_turns(polyline):
|
|
"""Number of direction changes along a polyline (a straightness proxy)."""
|
|
turns = 0
|
|
for i in range(1, len(polyline) - 1):
|
|
ax, ay = polyline[i][0] - polyline[i - 1][0], polyline[i][1] - polyline[i - 1][1]
|
|
bx, by = polyline[i + 1][0] - polyline[i][0], polyline[i + 1][1] - polyline[i][1]
|
|
# Cross product magnitude > tiny => heading changed.
|
|
if abs(ax * by - ay * bx) > 1e-6:
|
|
turns += 1
|
|
return turns
|
|
|
|
|
|
def test_required_points_passed_within_tolerance():
|
|
"""BP/CP/EP must be passed within ROUTE_REQUIRED_POINT_TOLERANCE_M after grid
|
|
snap + smoothing, and reported in required_point_checks (I-103)."""
|
|
p = _valid_points_from_cost_surface(3)
|
|
pts = {
|
|
"bp": p[0],
|
|
"ep": p[2],
|
|
"cp": [{"order": 1, **p[1]}],
|
|
"ap": [],
|
|
}
|
|
r = _solve({"grade_class": "work", "min_curve_radius_m": 8.0, "paved": False}, pts)
|
|
checks = r["required_point_checks"]
|
|
assert len(checks) == 3 # BP, CP1, EP
|
|
labels = [c["point"] for c in checks]
|
|
assert labels == ["BP", "CP1", "EP"], labels
|
|
tol = config.ROUTE_REQUIRED_POINT_TOLERANCE_M
|
|
# Required vertices are pinned to the EXACT user (x,y), so the polyline passes
|
|
# essentially through each point — well within tolerance.
|
|
worst = max(c["distance_m"] for c in checks)
|
|
assert worst <= tol, [c["distance_m"] for c in checks]
|
|
assert r["required_points_ok"] is True
|
|
print(f"[ok] required-point checks: {[(c['point'], c['distance_m']) for c in checks]}, "
|
|
f"all_ok={r['required_points_ok']}")
|
|
|
|
|
|
def test_circle_intrusion_detector():
|
|
"""The intrusion detector reports clearance + inside-length correctly (I-202)."""
|
|
from utils.route_solver import _circle_intrusions
|
|
# A straight polyline along x=0..100 at y=0; circle centred at (50,0) r=10.
|
|
poly = [[float(x), 0.0, 0.0] for x in range(0, 101, 2)]
|
|
res = _circle_intrusions(poly, [{"x": 50.0, "y": 0.0, "radius_m": 10.0}], lambda i: f"AP{i+1}")
|
|
a = res[0]
|
|
assert a["intrudes"] is True
|
|
assert abs(a["min_clearance_m"]) < 1e-6 # path passes through the centre
|
|
assert 18.0 <= a["intrusion_length_m"] <= 22.0 # ~2*radius inside the circle
|
|
# A path far from the circle does not intrude.
|
|
far = _circle_intrusions(poly, [{"x": 50.0, "y": 50.0, "radius_m": 10.0}], lambda i: f"AP{i+1}")
|
|
assert far[0]["intrudes"] is False
|
|
print(f"[ok] intrusion detector: clearance={a['min_clearance_m']}m, inside={a['intrusion_length_m']}m")
|
|
|
|
|
|
def test_ap_on_route_is_cleared():
|
|
"""Placing an AP on the baseline route pushes the route outside the AP circle
|
|
(the strong soft-avoid keeps clearance >= radius) (I-202)."""
|
|
p = _valid_points_from_cost_surface(2)
|
|
bp, ep = p[0], p[1]
|
|
opts = {"grade_class": "work", "min_curve_radius_m": 6.0, "paved": False}
|
|
base = _solve(opts, {"bp": bp, "ep": ep, "cp": [], "ap": []})
|
|
mid = base["polyline"][len(base["polyline"]) // 2]
|
|
radius = 30.0
|
|
pts = {"bp": bp, "ep": ep, "cp": [], "ap": [{"x": mid[0], "y": mid[1], "z": 0.0, "radius_m": radius}]}
|
|
r = _solve(opts, pts)
|
|
ai = r["avoid_intrusions"][0]
|
|
# Route now clears the AP centre by at least ~the radius (it was on the path before).
|
|
assert ai["min_clearance_m"] >= radius - 2.5, ai # allow one grid cell of slack
|
|
print(f"[ok] AP on route cleared: clearance={ai['min_clearance_m']}m (radius={radius}m), "
|
|
f"intrudes={ai['intrudes']}")
|
|
|
|
|
|
def test_separate_uphill_downhill_limits():
|
|
"""max_uphill_grade is a hard constraint on z-increasing links only; the resulting
|
|
route's max uphill grade must respect it, and conditions_snapshot records it (I-303)."""
|
|
pts = _points_from_dtm()
|
|
# Find a feasible uphill limit (terrain may need a fairly high one corner-to-corner).
|
|
r, up_limit = None, None
|
|
for cand in (0.15, 0.22, 0.30, 0.40):
|
|
try:
|
|
r = _solve({"grade_class": "work", "min_curve_radius_m": 6.0, "paved": False,
|
|
"max_uphill_grade": cand, "max_downhill_grade": 0.40}, pts)
|
|
up_limit = cand
|
|
break
|
|
except ValueError:
|
|
continue
|
|
assert r is not None, "어떤 오르막 한계에서도 경로가 생성되지 않았습니다"
|
|
# Uphill grade respects the chosen hard limit (small smoothing tolerance).
|
|
assert r["metrics"]["max_uphill_pct"] <= up_limit * 100 + 2.5, (up_limit, r["metrics"])
|
|
assert r["conditions_snapshot"]["max_uphill_grade_pct"] == round(up_limit * 100, 2)
|
|
print(f"[ok] uphill/downhill split: max_uphill={r['metrics']['max_uphill_pct']}% "
|
|
f"(limit {up_limit*100:.0f}%), max_downhill={r['metrics']['max_downhill_pct']}%")
|
|
|
|
|
|
def test_curve_warning_segments_merge():
|
|
"""Sub-radius samples merge into contiguous warning segments with chainage ranges
|
|
and polyline indices; count of segments <= count of violations (I-301)."""
|
|
pts = _points_from_dtm()
|
|
# Large required radius on a jagged grid route -> guaranteed warnings.
|
|
r = _solve({"grade_class": "work", "min_curve_radius_m": 50.0, "paved": False}, pts)
|
|
segs = r["curve_warning_segments"]
|
|
viol = r["metrics"]["curve_violations"]
|
|
if viol == 0:
|
|
print("[ok] curve warnings: none on this route (acceptable)")
|
|
return
|
|
assert len(segs) >= 1 and len(segs) <= viol
|
|
for s in segs:
|
|
assert s["chainage_end_m"] >= s["chainage_start_m"]
|
|
assert s["min_radius_m"] < s["required_radius_m"]
|
|
assert 0 <= s["polyline_start_index"] <= s["polyline_end_index"] < len(r["polyline"])
|
|
print(f"[ok] curve warning segments: {len(segs)} merged from {viol} violations; "
|
|
f"first={segs[0]['chainage_start_m']}-{segs[0]['chainage_end_m']}m "
|
|
f"minR={segs[0]['min_radius_m']}m")
|
|
|
|
|
|
def test_fp_is_hard_constraint():
|
|
"""FP forbidden zones are a HARD constraint: the route never enters one, so the
|
|
polyline keeps clearance >= radius and forbidden_intrusions is all clear (I-203)."""
|
|
p = _valid_points_from_cost_surface(2)
|
|
bp, ep = p[0], p[1]
|
|
opts = {"grade_class": "work", "min_curve_radius_m": 6.0, "paved": False}
|
|
base = _solve(opts, {"bp": bp, "ep": ep, "cp": [], "ap": []})
|
|
mid = base["polyline"][len(base["polyline"]) // 2]
|
|
radius = 30.0
|
|
pts = {"bp": bp, "ep": ep, "cp": [], "ap": [],
|
|
"fp": [{"x": mid[0], "y": mid[1], "z": 0.0, "radius_m": radius}]}
|
|
r = _solve(opts, pts)
|
|
fi = r["forbidden_intrusions"][0]
|
|
assert fi["intrudes"] is False, fi # hard constraint: never inside
|
|
assert fi["intrusion_length_m"] == 0.0, fi
|
|
assert fi["min_clearance_m"] >= radius - 2.5, fi # outside the circle (cell slack)
|
|
print(f"[ok] FP hard constraint: clearance={fi['min_clearance_m']}m (radius={radius}m), "
|
|
f"intrudes={fi['intrudes']}")
|
|
|
|
|
|
def test_curve_radius_is_wired_and_steers_straighter():
|
|
"""min_curve_radius_m is consumed by the solver: a larger required radius must not
|
|
produce a *more* twisty route than a small one (soft penalty; both stay feasible).
|
|
2D length is NOT used here because a straighter route can be shorter."""
|
|
pts = _points_from_dtm()
|
|
gentle = _solve({"grade_class": "work", "min_curve_radius_m": 30.0, "paved": False}, pts)
|
|
tight = _solve({"grade_class": "work", "min_curve_radius_m": 5.0, "paved": False}, pts)
|
|
assert gentle["polyline"] and tight["polyline"]
|
|
g_turns, t_turns = _count_turns(gentle["polyline"]), _count_turns(tight["polyline"])
|
|
assert g_turns <= t_turns, f"gentle should be no twistier: {g_turns} vs {t_turns}"
|
|
print(f"[ok] curve radius steers straighter: turns tight(5m)={t_turns}, "
|
|
f"gentle(30m)={g_turns}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# Unit tests need no data.
|
|
test_side_slope_axis()
|
|
test_compute_gradients_axis_order()
|
|
test_turn_radius_monotonic()
|
|
|
|
if not DTM_PATH.exists():
|
|
print("샘플 DTM 캐시가 없어 통합 테스트를 건너뜁니다.")
|
|
sys.exit(0)
|
|
|
|
test_cost_surface_is_reduced_grid()
|
|
test_size_guard_blocks_oversized_grid()
|
|
test_grade_hard_constraint_caps_max_grade()
|
|
test_grade_class_changes_feasibility()
|
|
test_required_points_passed_within_tolerance()
|
|
test_circle_intrusion_detector()
|
|
test_ap_on_route_is_cleared()
|
|
test_fp_is_hard_constraint()
|
|
test_separate_uphill_downhill_limits()
|
|
test_curve_warning_segments_merge()
|
|
test_curve_radius_is_wired_and_steers_straighter()
|
|
print("\n[PASS] 모든 테스트 통과")
|