33 lines
1.3 KiB
Python
33 lines
1.3 KiB
Python
from guardrail_space.collision_oracle import check_pair_conflict, aggregate_check
|
|
|
|
|
|
def _make_line_plan(pid: str, start: float, x0: float, y0: float, x1: float, y1: float) -> dict:
|
|
# simple two-point trajectory from start time to start+1.0
|
|
return {"id": pid, "trajectory": [(start, x0, y0), (start + 1.0, x1, y1)]}
|
|
|
|
|
|
def test_direct_collision_detected():
|
|
a = _make_line_plan("A", 0.0, 0.0, 0.0, 1.0, 0.0)
|
|
b = _make_line_plan("B", 0.0, 0.0, 0.0, -1.0, 0.0)
|
|
res = check_pair_conflict(a, b, distance_threshold=0.1)
|
|
assert res["verdict"] == "conflict"
|
|
assert len(res["conflicts"]) >= 1
|
|
|
|
|
|
def test_spatially_separated_are_compatible():
|
|
a = _make_line_plan("A", 0.0, 0.0, 0.0, 1.0, 0.0)
|
|
b = _make_line_plan("B", 0.0, 10.0, 0.0, 11.0, 0.0)
|
|
res = check_pair_conflict(a, b, distance_threshold=0.5)
|
|
assert res["verdict"] == "compatible"
|
|
|
|
|
|
def test_aggregate_pairwise():
|
|
p1 = _make_line_plan("p1", 0.0, 0.0, 0.0, 1.0, 0.0)
|
|
p2 = _make_line_plan("p2", 0.0, 0.0, 0.0, -1.0, 0.0)
|
|
p3 = _make_line_plan("p3", 0.0, 10.0, 0.0, 11.0, 0.0)
|
|
out = aggregate_check([p1, p2, p3], distance_threshold=0.2)
|
|
# p1 vs p2 conflict, others compatible
|
|
assert out[("p1", "p2")]["verdict"] == "conflict"
|
|
assert out[("p1", "p3")]["verdict"] == "compatible"
|
|
assert out[("p2", "p3")]["verdict"] == "compatible"
|