From d27237ab8ccabea5e30ddb72b42450de305c85ed Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Thu, 12 Sep 2019 16:24:39 +0200 Subject: [PATCH] Add ext SOLID ut --- ut/rrtext.t.cc | 39 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/ut/rrtext.t.cc b/ut/rrtext.t.cc index 5fe1ab9..e884763 100644 --- a/ut/rrtext.t.cc +++ b/ut/rrtext.t.cc @@ -4,7 +4,7 @@ #include "rrtext.h" #include "reeds_shepp.h" -WVTEST_MAIN("RRT extensions") +WVTEST_MAIN("RRT extensions 1") { RRTNode n1; RRTNode n2; @@ -20,3 +20,40 @@ WVTEST_MAIN("RRT extensions") ReedsSheppStateSpace rsss(n1.mtr()); WVPASSEQ_DOUBLE(e1.cost_build(n1, n2), rsss.distance(q0, q1), 0.00001); } + +WVTEST_MAIN("RRT extensions 2") +{ + RRTExt2 rrts; + WVPASSEQ_DOUBLE(cc(rrts.nodes().front()), 0, 0.00001); + WVPASSEQ(rrts.nodes().size(), 1); + rrts.next(); + WVPASSLT(1, rrts.nodes().size()); + WVPASSEQ(rrts.samples().size(), 1); + rrts.goals().push_back(BicycleCar()); + rrts.goals().back().x(10); + rrts.goals().back().y(10); + rrts.goals().back().h(0); + WVPASSEQ(rrts.goals().size(), 1); + rrts.set_sample(0, 10, 0, 10, 0, 2 * M_PI); + Obstacle o; + o.poly().push_back(std::make_tuple(5, 5)); + o.poly().push_back(std::make_tuple(6, 5)); + o.poly().push_back(std::make_tuple(6, 6)); + o.poly().push_back(std::make_tuple(5, 6)); + o.poly().push_back(std::make_tuple(5, 5)); + rrts.obstacles().push_back(o); + rrts.init_solid(); + while (rrts.next()) {} + rrts.deinit_solid(); + WVPASS(rrts.path().size() > 0); + WVPASS( + rrts.nodes().size() > 0 + && rrts.path().size() > 0 + && &rrts.nodes().front() == rrts.path().front() + ); + WVPASS( + rrts.goals().size() > 0 + && rrts.path().size() > 0 + && &rrts.goals().front() == rrts.path().back() + ); +} -- 2.39.2