199 lines
7.7 KiB
Python
199 lines
7.7 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
test_mprim_gen.py
|
|
------------------
|
|
Test tự động cho generator: sinh cả 3 config mẫu ra thư mục tạm rồi kiểm tra
|
|
output theo đúng các ràng buộc SBPL enforce lúc nạp file + chất lượng hình học.
|
|
|
|
Chạy:
|
|
python3 test_mprim_gen.py
|
|
|
|
Các kiểm tra:
|
|
1. Header khớp số primitive thực tế; parse trọn vẹn không dư token.
|
|
2. additionalactioncostmult là SỐ NGUYÊN (SBPL đọc fscanf %d).
|
|
3. Pose cuối rời rạc hóa ra đúng endpose_c (đúng phép check của
|
|
ReadinMotionPrimitive — sai là SBPL từ chối cả file).
|
|
4. Heading cuối khớp CHÍNH XÁC góc rời rạc đích (bug l=0 cũ gây lệch 10 độ).
|
|
5. Khoảng cách 2 pose liên tiếp <= 1.05 cell (footprint sweep không sót cell)
|
|
và ĐỀU trong từng primitive (max/min <= 1.5).
|
|
6. Vi phạm nonholonomic (góc lệch vector di chuyển vs heading) < 9 độ với
|
|
differential/ackermann — bắt lỗi kiểu cung lùi sai hình học (28 độ cũ).
|
|
7. Không có primitive suy biến, không trùng endpose trong cùng start angle.
|
|
8. Config sai (costmult lẻ, spacing > resolution, ackermann turn_radius <
|
|
min_turning_radius) phải bị validate() chặn.
|
|
"""
|
|
|
|
import math
|
|
import os
|
|
import sys
|
|
import tempfile
|
|
|
|
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
|
|
|
from mprim_config import RobotConfig, KinematicType
|
|
from mprim_core import generate_mprim_file
|
|
from generate_mprim import KINEMATICS_MAP, load_config
|
|
|
|
CONFIG_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "configs")
|
|
CHECKS = []
|
|
|
|
|
|
def check(name, cond, detail=""):
|
|
CHECKS.append((name, cond, detail))
|
|
status = "PASS" if cond else "FAIL"
|
|
print(f" [{status}] {name}" + (f" — {detail}" if detail and not cond else ""))
|
|
|
|
|
|
def parse(path):
|
|
toks = open(path).read().split()
|
|
it = iter(toks)
|
|
def nxt():
|
|
return next(it)
|
|
assert nxt() == "resolution_m:"
|
|
res = float(nxt())
|
|
assert nxt() == "numberofangles:"
|
|
nang = int(nxt())
|
|
assert nxt() == "totalnumberofprimitives:"
|
|
ntot = int(nxt())
|
|
prims = []
|
|
while True:
|
|
try:
|
|
tag = nxt()
|
|
except StopIteration:
|
|
break
|
|
assert tag == "primID:"
|
|
pid = int(nxt())
|
|
assert nxt() == "startangle_c:"
|
|
sa = int(nxt())
|
|
assert nxt() == "endpose_c:"
|
|
end = (int(nxt()), int(nxt()), int(nxt()))
|
|
assert nxt() == "additionalactioncostmult:"
|
|
cm = nxt()
|
|
assert nxt() == "intermediateposes:"
|
|
n = int(nxt())
|
|
poses = [(float(nxt()), float(nxt()), float(nxt())) for _ in range(n)]
|
|
prims.append(dict(pid=pid, sa=sa, end=end, cm=cm, poses=poses))
|
|
return res, nang, ntot, prims
|
|
|
|
|
|
def ang_diff(a, b):
|
|
return (a - b + math.pi) % (2 * math.pi) - math.pi
|
|
|
|
|
|
def contxy2disc(x, cell):
|
|
return int(math.floor(x / cell))
|
|
|
|
|
|
def verify_file(path, kinematic, min_turning_radius=None):
|
|
res, nang, ntot, prims = parse(path)
|
|
step = 2 * math.pi / nang
|
|
|
|
check("header khớp số primitive parse được", ntot == len(prims),
|
|
f"header {ntot} != parsed {len(prims)}")
|
|
|
|
bad_cm = [p["cm"] for p in prims if not p["cm"].lstrip("-").isdigit()]
|
|
check("costmult toàn số nguyên (fscanf %d)", not bad_cm, f"lẻ: {sorted(set(bad_cm))}")
|
|
|
|
# phép check của SBPL ReadinMotionPrimitive
|
|
bad_end = 0
|
|
for p in prims:
|
|
lx, ly, lth = p["poses"][-1]
|
|
got = (contxy2disc(res / 2 + lx, res), contxy2disc(res / 2 + ly, res),
|
|
int(round((lth % (2 * math.pi)) / step)) % nang)
|
|
if got != p["end"]:
|
|
bad_end += 1
|
|
check("pose cuối rời rạc hóa đúng endpose_c (điều kiện SBPL nạp file)", bad_end == 0,
|
|
f"{bad_end} primitive sai")
|
|
|
|
worst_heading = max(abs(ang_diff(p["poses"][-1][2], p["end"][2] * step)) for p in prims)
|
|
check("heading cuối khớp chính xác góc đích", worst_heading < 1e-4,
|
|
f"lệch tối đa {math.degrees(worst_heading):.2f} độ")
|
|
|
|
worst_gap, worst_ratio = 0.0, 1.0
|
|
for p in prims:
|
|
gaps = [math.hypot(x1 - x0, y1 - y0)
|
|
for (x0, y0, _), (x1, y1, _) in zip(p["poses"], p["poses"][1:])]
|
|
moving = [g for g in gaps if g > 1e-9]
|
|
if moving:
|
|
worst_gap = max(worst_gap, max(moving))
|
|
worst_ratio = max(worst_ratio, max(moving) / min(moving))
|
|
check("bước giữa 2 pose <= 1.05 cell (footprint sweep không sót)",
|
|
worst_gap <= res * 1.05, f"max {worst_gap:.4f}m")
|
|
check("spacing đều trong từng primitive (max/min <= 1.5)", worst_ratio <= 1.5,
|
|
f"tỉ lệ {worst_ratio:.2f}")
|
|
|
|
if kinematic in (KinematicType.DIFFERENTIAL, KinematicType.ACKERMANN):
|
|
worst_nh = 0.0
|
|
for p in prims:
|
|
if p["end"][0] == 0 and p["end"][1] == 0:
|
|
continue
|
|
for (x0, y0, t0), (x1, y1, t1) in zip(p["poses"], p["poses"][1:]):
|
|
dx, dy = x1 - x0, y1 - y0
|
|
if math.hypot(dx, dy) < 1e-9:
|
|
continue
|
|
hd = math.atan2(dy, dx)
|
|
th = 0.5 * (t0 + t1)
|
|
worst_nh = max(worst_nh, min(abs(ang_diff(hd, th)),
|
|
abs(ang_diff(hd, th + math.pi))))
|
|
check("vi phạm nonholonomic < 9 độ", math.degrees(worst_nh) < 9.0,
|
|
f"max {math.degrees(worst_nh):.2f} độ")
|
|
|
|
degenerate = [p for p in prims
|
|
if p["end"] == (0, 0, p["sa"])]
|
|
check("không có primitive suy biến (0,0,không đổi góc)", not degenerate,
|
|
f"{len(degenerate)} cái")
|
|
|
|
dup = 0
|
|
for sa in range(nang):
|
|
seen = set()
|
|
for p in prims:
|
|
if p["sa"] != sa:
|
|
continue
|
|
if p["end"] in seen:
|
|
dup += 1
|
|
seen.add(p["end"])
|
|
check("không trùng endpose trong cùng start angle", dup == 0, f"{dup} cặp trùng")
|
|
|
|
|
|
def test_configs_generate():
|
|
with tempfile.TemporaryDirectory() as tmp:
|
|
for fname in ("differential.py", "omnidirectional.py", "ackermann.py"):
|
|
cfg = load_config(os.path.join(CONFIG_DIR, fname))
|
|
out = os.path.join(tmp, cfg.name + ".mprim")
|
|
n, warnings = generate_mprim_file(cfg, KINEMATICS_MAP[cfg.kinematic_type], out)
|
|
print(f"\n== {fname}: {n} primitives, {len(warnings)} cảnh báo ==")
|
|
for w in warnings:
|
|
print(f" (warn) {w}")
|
|
verify_file(out, cfg.kinematic_type,
|
|
min_turning_radius=getattr(cfg, "min_turning_radius_m", None))
|
|
|
|
|
|
def test_validation_rejects_bad_configs():
|
|
print("\n== validate() phải chặn config sai ==")
|
|
|
|
def must_fail(name, **overrides):
|
|
base = dict(name="t", kinematic_type=KinematicType.DIFFERENTIAL)
|
|
base.update(overrides)
|
|
try:
|
|
RobotConfig(**base).validate()
|
|
check(name, False, "validate() đã KHÔNG chặn")
|
|
except ValueError:
|
|
check(name, True)
|
|
|
|
must_fail("chặn costmult không nguyên", forwardandturncostmult=2.5)
|
|
must_fail("chặn costmult < 1", backwardcostmult=0)
|
|
must_fail("chặn pose_spacing_m > resolution", pose_spacing_m=0.08)
|
|
must_fail("chặn forward quá ngắn (quantize về 0,0)", forward_short_m=0.02)
|
|
must_fail("chặn turn_steps rỗng", turn_steps=())
|
|
must_fail("chặn ackermann turn_radius < min_turning_radius",
|
|
kinematic_type=KinematicType.ACKERMANN,
|
|
min_turning_radius_m=0.45, turn_radius_m=0.30)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
test_configs_generate()
|
|
test_validation_rejects_bad_configs()
|
|
failed = [c for c in CHECKS if not c[1]]
|
|
print(f"\n{'='*60}\nTổng: {len(CHECKS)} check, {len(failed)} FAIL")
|
|
sys.exit(1 if failed else 0)
|