Add goal found zone ut

diff --git a/ut/rrts.t.cc b/ut/rrts.t.cc
index 693dcd1ee504d3e277c681dc3221140993ad0386..554f1cee42f1e5aeed9b01fc5479c72896ff2d25 100644 (file)
--- 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
+        }
+}