first commit
This commit is contained in:
198
test_mprim_gen.py
Normal file
198
test_mprim_gen.py
Normal file
@@ -0,0 +1,198 @@
|
||||
#!/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)
|
||||
Reference in New Issue
Block a user