From 31eed8025ee7aaf43652cf8c256eb8a2be25c496 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Fri, 24 Apr 2020 15:55:30 +0200 Subject: [PATCH] Add goal found zone ut --- ut/rrts.t.cc | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/ut/rrts.t.cc b/ut/rrts.t.cc index 693dcd1..554f1ce 100644 --- a/ut/rrts.t.cc +++ b/ut/rrts.t.cc @@ -43,3 +43,81 @@ WVTEST_MAIN("RRT* basic tests") && &rrts.goals().front() == rrts.path().back() ); } + +WVTEST_MAIN("goal found zone") +{ + class P : public RRTS { + public: + bool goal_found(RRTNode &f) { + return RRTS::goal_found(f); + } + }; + double tmp_double_1 = 0; + double tmp_double_2 = 0; + P p; + p.goals().push_back(BicycleCar()); + BicycleCar &g = p.goals().front(); + // TODO set g.x, g.y to different values + // TODO set g.h to cover all 4 quadrants + RRTNode n; + n.x(g.x()); + n.y(g.y()); + n.h(g.h()); + WVPASS(p.goal_found(n)); // pass the same pose + n.sp(-0.01); + n.st(0); + n.next(); + WVPASS(!p.goal_found(n)); // fail backward direction + + n = RRTNode(g); + n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2); + WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001); + tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2)); + tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2)); + WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001); + WVPASS(p.goal_found(n)); // pass right corner case + n.rotate(g.ccr().x(), g.ccr().y(), -0.01); + WVPASS(!p.goal_found(n)); // fail right corner case + + n = RRTNode(g); + n.rotate(g.ccl().x(), g.ccl().y(), M_PI/2); + WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001); + tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2)); + tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)); + WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001); + WVPASS(p.goal_found(n)); // pass left corner case + n.rotate(g.ccl().x(), g.ccl().y(), 0.01); + WVPASS(!p.goal_found(n)); // fail left corner case + + n = RRTNode(g); + n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2))); + n.st(0); + n.next(); + WVPASS(p.goal_found(n)); // pass forward corner case + n.sp(0.01); + n.next(); + WVPASS(!p.goal_found(n)); // fail forward corner case + + for (double a = 0; a > -M_PI / 2; a -= 0.01) { + n = RRTNode(g); + n.rotate(g.ccr().x(), g.ccr().y(), a); + WVPASS(p.goal_found(n)); // pass drivable border + n.x(n.x() + 0.01 * cos(g.h())); + n.y(n.y() + 0.01 * sin(g.h())); + WVPASS(p.goal_found(n)); // pass near drivable border + n.x(n.x() - 0.02 * cos(g.h())); + n.y(n.y() - 0.02 * sin(g.h())); + WVPASS(!p.goal_found(n)); // fail near drivable border + } + for (double a = 0; a < M_PI / 2; a += 0.01) { + n = RRTNode(g); + n.rotate(g.ccl().x(), g.ccl().y(), a); + WVPASS(p.goal_found(n)); // pass drivable border + n.x(n.x() + 0.01 * cos(g.h())); + n.y(n.y() + 0.01 * sin(g.h())); + WVPASS(p.goal_found(n)); // pass near drivable border + n.x(n.x() - 0.02 * cos(g.h())); + n.y(n.y() - 0.02 * sin(g.h())); + WVPASS(!p.goal_found(n)); // fail near drivable border + } +} -- 2.39.2