| """closed-loop μλ² νλ‘ν μ½(src/vkgs/src/serve.hpp)κ³Ό drive.hpp μ μ΄ νμ°μ g++ λ‘ μ»΄νμΌν΄ μννλ€ (μ°½Β·GPU μμ΄). |
| Python μͺ½ νμ (nusim.sim.geometry)μ΄ C++ vehicle_contact μ κ°μ μ μ μΈλμ§λ λμ‘°νλ€.""" |
|
|
| import os |
| import shutil |
| import subprocess |
| from pathlib import Path |
|
|
| import numpy as np |
| import pytest |
|
|
| from nusim.sim.closed_loop import format_command, parse_reply |
| from nusim.sim.geometry import ContactParams, VehicleGeom, World |
|
|
| ROOT = Path(__file__).resolve().parents[1] |
| CXX = os.environ.get("CXX") or "g++" |
| needs_cxx = pytest.mark.skipif(shutil.which(CXX) is None, reason="g++ μμ") |
|
|
|
|
| def _build(tmp_path): |
| exe = tmp_path / "test_serve" |
| subprocess.run([CXX, "-std=c++20", "-O1", "-I", str(ROOT / "src/vkgs/src"), str(ROOT / "tests/cpp/test_serve.cpp"), |
| "-o", str(exe)], check=True) |
| return exe |
|
|
|
|
| @needs_cxx |
| def test_serve_protocol_and_control(tmp_path): |
| r = subprocess.run([str(_build(tmp_path)), "self"], capture_output=True, text=True) |
| assert r.returncode == 0, r.stdout + r.stderr |
|
|
|
|
| def test_python_protocol_roundtrip(): |
| st = parse_reply("ok t=0.500000 x=-9.5 obs=- cams=CAM_FRONT,CAM_BACK contact=1\n") |
| assert st == {"t": 0.5, "x": -9.5, "obs": "", "cams": "CAM_FRONT,CAM_BACK", "contact": 1.0} |
| with pytest.raises(RuntimeError, match="μλ² μ€λ₯"): |
| parse_reply("err step: dt= κ° νμν¨") |
| with pytest.raises(RuntimeError): |
| parse_reply("") |
| assert format_command("step", accel=1.25, delta=-0.1, dt=0.05, render=True, sensors=None) == \ |
| "step accel=1.25 delta=-0.1 dt=0.05 render=1" |
| with pytest.raises(ValueError): |
| format_command("actors", boxes="1 2") |
|
|
|
|
| @needs_cxx |
| def test_python_contact_matches_cpp(tmp_path): |
| """κ°μ μ§λΒ·μμΈμμ Python World μ C++ vehicle_contact κ° κ°μ μμ μ μ μΌλ€.""" |
| res, n, x0 = 0.2, 150, -15.0 |
| rng = np.random.default_rng(0) |
| g = (rng.random((n, n)) < 0.03).astype(np.uint8) * 255 |
| g[60:64, :] = 255 |
| pgm = tmp_path / "obst.pgm" |
| pgm.write_bytes(f"P5\n# nusim obstacles {x0} {x0} {res}\n{n} {n}\n255\n".encode() + g.tobytes()) |
| driv = tmp_path / "drivable.txt" |
| |
| driv.write_text("# t\nP 2 4 -10 -10 10 -10 10 10 -10 10 4 2 2 4 2 4 4 2 4\n") |
| poses = np.c_[rng.uniform(-9, 9, 60), rng.uniform(-9, 9, 60), rng.uniform(-np.pi, np.pi, 60)] |
| r = subprocess.run([str(_build(tmp_path)), "contact", str(pgm), str(driv)], input="\n".join( |
| f"{x:.4f} {y:.4f} {yaw:.4f}" for x, y, yaw in poses), capture_output=True, text=True, check=True) |
| cpp = np.array([list(map(int, l.split())) for l in r.stdout.split("\n") if l.strip()]) |
| w = World.load(VehicleGeom(), ContactParams(curb=True, curb_tol=0.15, obst_tol=0.1, graze=2), pgm, driv) |
| poses32 = poses.astype(np.float32).astype(np.float64) |
| py_obst = [w.hard_contacts(*p) for p in poses32] |
| py_road = [w.on_road(*p) for p in poses32] |
| assert np.abs(cpp[:, 0] - np.array(py_obst)).max() <= 1, (cpp[:, 0], py_obst) |
| assert (cpp[:, 0] > 0).sum() > 5 |
| assert ((cpp[:, 1] == 0) == np.array(py_road)).all() |
|
|
