Hugging Face's logo

kimjunhan
/
vkgs-sim

Model card Files
xet
 Community
vkgs-sim / tests /cpp /test_serve.cpp
kimjunhan's picture
kimjunhan
Add files using upload-large-folder tool
4c75661 verified
Raw History Blame Contribute Delete
5.71 kB
// closed-loop μ„œλ²„ ν”„λ‘œν† μ½œ(serve.hpp)Β·μ œμ–΄ ν™˜μ‚°(drive.hpp control_for, step_with_contact) μ‹œν—˜ β€” tests/test_serve_cpp.py κ°€
// μ»΄νŒŒμΌν•΄ λŒλ¦°λ‹€. μ°½Β·GPU 없이.
// test_serve self 자체 μ‹œν—˜ (μ‹€νŒ¨ 수 = μ’…λ£Œ μ½”λ“œ)
// test_serve contact OBST.pgm DRIV.txt ν‘œμ€€ μž…λ ₯ μ€„λ§ˆλ‹€ 'x y yaw' β†’ 'μž₯μ• λ¬Όμ μˆ˜ μ—°μ„μ μˆ˜' (Python geometry 와 λŒ€μ‘°)
#include "drive.hpp"
#include "serve.hpp"
#include <cmath>
#include <cstdio>
#include <iostream>
#include <string>
using namespace vkgs;
static int fails = 0;
#define CHECK(c, msg) \
do { \
if (!(c)) { \
std::printf("FAIL %s\n", msg); \
++fails; \
} \
} while (0)
static bool throws(const std::string& line) {
try {
parse_command(line);
} catch (const std::exception&) {
return true;
}
return false;
}
int self_test() {
// 1) λͺ…λ Ή νŒŒμ„œ
ServeCommand c = parse_command("step accel=1.5 delta=-0.1 dt=0.05 render=1");
CHECK(c.name == "step", "name");
CHECK(std::fabs(c.num("accel") - 1.5f) < 1e-6f && std::fabs(c.num("delta") + 0.1f) < 1e-6f, "numbers");
CHECK(c.flag("render", false) && !c.flag("sensors", false), "flags");
CHECK(std::fabs(c.num("throttle", 0.25f) - 0.25f) < 1e-6f, "default");
CHECK(parse_command("").name.empty() && parse_command(" # 주석").name.empty(), "empty/comment");
CHECK(throws("step accel"), "token without =");
CHECK(throws("step =3"), "empty key");
bool bad_num = false;
try { parse_command("step dt=0.05x").num("dt"); } catch (const std::exception&) { bad_num = true; }
CHECK(bad_num, "trailing garbage number");
bool missing = false;
try { parse_command("step").num("dt"); } catch (const std::exception&) { missing = true; }
CHECK(missing, "missing key");
bool bad_flag = false;
try { parse_command("render sensors=yes").flag("sensors", false); } catch (const std::exception&) { bad_flag = true; }
CHECK(bad_flag, "bad flag");
CHECK(parse_command("reset x=1 x=2").num("x") == 2.0f, "last key wins");
// 2) μƒμž λͺ©λ‘
auto b = parse_boxes("1,2,0.5,2.2,0.9;3,4,0,1,1;");
CHECK(b.size() == 2 && b[1][0] == 3.0f && b[0][3] == 2.2f, "boxes");
CHECK(parse_boxes("").empty(), "no boxes");
bool bad_box = false;
try { parse_boxes("1,2,3"); } catch (const std::exception&) { bad_box = true; }
CHECK(bad_box, "short box");
// 3) 응닡 쀄
std::string l = ServeReply().add("t", 0.5, 2).add("cams", "A,B").add("obs", "").add("n", 3).line();
CHECK(l == "ok t=0.50 cams=A,B obs=- n=3\n", "reply line");
CHECK(serve_error("a\nb") == "err a b\n", "error line");
// 4) 가속·쑰ν–₯ λͺ…λ Ή β†’ νŽ˜λ‹¬Β·ν•Έλ“€: 정상 μƒνƒœμ—μ„œ λͺ…λ Ήν•œ 가속이 κ·ΈλŒ€λ‘œ λ‚˜μ˜¨λ‹€
Vehicle car;
car.v = 5;
for (float a : {1.5f, 0.0f, -2.0f, -6.5f}) {
Vehicle t = car;
float v0 = t.v;
t.step(control_for(t, a, 0), 0.01f);
float got = (t.v - v0) / 0.01f;
std::printf("accel cmd %.2f -> %.3f\n", a, got);
CHECK(std::fabs(got - a) < 0.01f, "accel tracking");
}
// μ„œ μžˆμ„ λ•Œ 음의 가속은 후진이 μ•„λ‹ˆλΌ 정지 μœ μ§€
Vehicle st;
for (int i = 0; i < 100; ++i) st.step(control_for(st, -3.0f, 0), 0.01f);
CHECK(std::fabs(st.v) < 1e-6f && std::fabs(st.x) < 1e-6f, "hold at standstill");
// μ‘°ν–₯각 λͺ…λ Ή: μ‘°ν–₯ 속도 μ œν•œ(1.2 rad/s) λ’€ λͺ©ν‘œκ°μ— 수렴
Vehicle s2;
s2.v = 4;
for (int i = 0; i < 100; ++i) s2.step(control_for(s2, 0.0f, 0.2f), 0.01f);
std::printf("delta after 1 s: %.4f (cmd 0.2)\n", s2.delta);
CHECK(std::fabs(s2.delta - 0.2f) < 2e-3f, "steer angle");
// 5) step_with_contact: λ²½(x = 10 m) μ•žμ—μ„œ λ©ˆμΆ”κ³  λ§‰νž˜μ„ μ•Œλ¦°λ‹€ (빈 지도·주행가λŠ₯μ˜μ—­ = 막지 μ•ŠμŒ)
ObstacleGrid none;
Drivable road;
ContactParams cp;
cp.curb = false;
Vehicle w;
ContactInfo cur;
int blocked = 0;
for (int i = 0; i < 300; ++i) blocked += step_with_contact(w, control_for(w, 2.0f, 0), 0.01f, cur, none, road, cp).blocked;
CHECK(blocked == 0 && w.x > 5.0f, "free driving");
// λ‹€λ₯Έ μ°¨λŸ‰ μƒμž: μ•ž 8 m 에 정지 μ°¨λŸ‰ β†’ λ§‰ν˜€ λ©ˆμΆ˜λ‹€
std::vector<Footprint> others{{w.x + 3.43f + 1.0f + 2.2f, w.y, 0, 2.2f, 0.9f}};
int vb = 0;
Contact kind = Contact::None;
for (int i = 0; i < 300; ++i) {
SubstepResult r = step_with_contact(w, control_for(w, 2.0f, 0), 0.01f, cur, none, road, cp, &others);
if (r.blocked) ++vb, kind = r.kind;
}
std::printf("vehicle blocked steps %d, front %.2f, other rear %.2f\n", vb, w.x + w.geom.front, others[0].x - 2.2f);
CHECK(vb > 0 && kind == Contact::Vehicle, "blocked by vehicle");
CHECK(w.x + w.geom.front < others[0].x - 2.2f + 0.3f, "stopped at vehicle");
return fails;
}
int contact_dump(const char* pgm, const char* driv) {
ObstacleGrid og;
og.load(pgm);
Drivable road;
road.load(driv);
float x, y, yaw;
while (std::cin >> x >> y >> yaw) {
Vehicle car;
car.x = x, car.y = y, car.yaw = yaw;
ContactInfo ci = vehicle_contact(car, og, road, true, 0.15f, 0.1f);
std::printf("%d %d\n", ci.obstacle, ci.curb);
}
return 0;
}
int main(int argc, char** argv) {
std::string mode = argc > 1 ? argv[1] : "self";
if (mode == "contact" && argc > 3) return contact_dump(argv[2], argv[3]);
int f = self_test();
std::printf("%s (%d)\n", f ? "FAILED" : "OK", f);
return f;
}