Merge branch 'feature/tune-and-refactor'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 853558a236fc02dea476f511bca76463c6b2e1cc..382a3e2d309e2e45a0b4f4f6d3ed0d4168ea2911 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -13,6 +13,9 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Possible inits method.
 - Number of cusps.
 
+### Changed
+- Return 10 possible goals (5m together) as default.
+
 ### Fixed
 - Heading overflow/underflow.
 - Negative discriminant.
@@ -20,6 +23,10 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Guess goal method for angled perpendicular parking slots. `guess_gc` method
   now decides if the parking will be forward or backward.
 - Perpendicular parking. Not dependent on `fer_perpendicular` anymore.
+- Guess goal car for perpendicular parking.
+
+### Removed
+- Unit tests removed in `f97dfa1`. Convert it and update tests in the future.
 
 ## 0.2.0 - 2019-08-05
 ### Deleted

diff --git a/api/psp.h b/api/psp.h
index 0028fde0a827fce274b44c1319992d751410716e..7ee2bb81a90528f1d9bf279bcd2955329f151618 100644 (file)
--- a/api/psp.h
+++ b/api/psp.h
@@ -20,6 +20,7 @@ class PSPlanner {
                 ParkingSlot ps_;
 
                 unsigned int c_ = 0; // number of cusps
+                std::vector<BicycleCar> cusps_;
 
                 // find entry to slot
                 void fe_parallel();
@@ -79,7 +80,7 @@ class PSPlanner {
                 );
                 std::vector<BicycleCar> possible_goals()
                 {
-                        return this->possible_goals(10, 1);
+                        return this->possible_goals(10, 0.5);
                 }
 
                 // find entry
@@ -98,6 +99,7 @@ class PSPlanner {
                 ParkingSlot &ps() { return this->ps_; }
 
                 unsigned int c() const { return this->c_; }
+                std::vector<BicycleCar> &cusps() { return this->cusps_; }
 
                 PSPlanner();
 };

diff --git a/src/psp.cc b/src/psp.cc
index cd3aba8f5081ed4afe1bf794d0f8b0fd4fd34f58..f33568e350a37202baba19a88444ade44492f20a 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -96,103 +96,32 @@ void PSPlanner::guess_gc()
         if (this->ps().right())
                 dts = - M_PI / 2;
         if (this->ps().parallel()) {
-                dts *= 0.99; // precision workaround
                 x += (this->gc().w() / 2 + 0.01) * cos(h + dts);
                 x += (this->gc().dr() + 0.01) * cos(h);
                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
                 y += (this->gc().dr() + 0.01) * sin(h);
         } else {
-                if (std::abs(
-                        atan2(
-                                this->ps().y2() - this->ps().y1(),
-                                this->ps().x2() - this->ps().x1()
-                        )
-                        - this->ps().heading()
-                ) < M_PI / 2) {
-                        // forward parking
-                        this->gc_to_4();
-                        double bx;
-                        double by;
-                        double cx;
-                        double cy;
-                        if (this->ps().right()) {
-                                bx = this->gc().lfx();
-                                by = this->gc().lfy();
-                                cx = this->gc().ccr().x();
-                                cy = this->gc().ccr().y();
-                        } else {
-                                bx = this->gc().rfx();
-                                by = this->gc().rfy();
-                                cx = this->gc().ccl().x();
-                                cy = this->gc().ccl().y();
-                        }
-                        double radi_angl = atan2(by - cy, bx - cx);
-                        radi_angl += dts;
-                        double angl_delta = this->gc().h() - radi_angl;
-                        this->gc().rotate(bx, by, angl_delta);
-                        // TODO there is a bug somewhere :/
-                        //
-                        // cli returns not exact intersection, therefore the
-                        // distance to x1, y1 of border is shorter. Then, when
-                        // moving, the distance `dist_o` is not sufficient and
-                        // car still collide with parking slot. It shouldn't be
-                        // problem until it collides with obstacle.
-                        //
-                        if (this->ps().right()) {
-                                cx = this->gc().ccr().x();
-                                cy = this->gc().ccr().y();
-                        } else {
-                                cx = this->gc().ccl().x();
-                                cy = this->gc().ccl().y();
-                        }
-                        auto cli = circle_line_intersection(
-                                cx, cy, this->gc().iradi(),
-                                this->ps().x1(), this->ps().y1(),
-                                this->ps().x2(), this->ps().y2()
-                        );
-                        double d1 = edist(
-                                this->ps().x1(), this->ps().y1(),
-                                std::get<0>(cli), std::get<1>(cli)
-                        );
-                        double d2 = edist(
-                                this->ps().x1(), this->ps().y1(),
-                                std::get<2>(cli), std::get<3>(cli)
-                        );
-                        double dist_o = std::min<double>(d1, d2);
-                        double angl_o = atan2(
-                                this->ps().y4() - this->ps().y3(),
-                                this->ps().x4() - this->ps().x3()
-                        );
-                        // projection
-                        double angl_d = atan2(
-                                this->ps().y1() - this->ps().y2(),
-                                this->ps().x1() - this->ps().x2()
-                        );
-                        angl_d -= angl_o;
-                        dist_o *= cos(angl_d);
-                        this->gc().x(this->gc().x() + dist_o * cos(angl_o));
-                        this->gc().y(this->gc().y() + dist_o * sin(angl_o));
-                        // --- ENDTODO ---
-                        this->gc().sp(-0.01);
-                        this->gc().st(dts);
-                        return;
-                } else {
-                        dts = atan2(
-                                this->ps().y2() - this->ps().y1(),
-                                this->ps().x2() - this->ps().x1()
-                        );
-                        dts *= 1.01; // precision workaround
-                        // backward parking
-                        h = dts + M_PI;
-                        x += -(this->gc().df() + 0.01) * cos(h);
-                        y += -(this->gc().df() + 0.01) * sin(h);
-                        if (this->ps().right())
-                                dts += M_PI / 2;
-                        else
-                                dts -= M_PI / 2;
-                        x += (this->gc().w() / 2 + 0.01) * cos(dts);
-                        y += (this->gc().w() / 2 + 0.01) * sin(dts);
-                }
+                // This is for backward parking only.
+                double entry_width = edist(
+                        this->ps().x1(), this->ps().y1(),
+                        this->ps().x4(), this->ps().y4()
+                );
+                double dist_l =
+                        this->gc().orradi()
+                        - (this->gc().mtr() + this->gc().w() / 2)
+                ;
+                double move1 = dist_l + this->gc().w() / 2;
+                double dist_r = entry_width - this->gc().w() - dist_l;
+                double move2 = sqrt(
+                        pow(this->gc().iradi(), 2)
+                        - pow(this->gc().iradi() - dist_r, 2)
+                );
+                move2 -= this->gc().dr() / 2; // workaround
+                x += move1 * cos(h);
+                y += move1 * sin(h);
+                x += move2 * cos(h + dts);
+                y += move2 * sin(h + dts);
+                h += dts + M_PI;
         }
         while (h > M_PI)
                 h -= 2 * M_PI;
@@ -256,6 +185,10 @@ std::vector<BicycleCar> PSPlanner::possible_goals(
 )
 {
         std::vector<BicycleCar> pi;
+        if (this->cc().sp() > 0)
+                this->cc().sp(1);
+        else
+                this->cc().sp(-1);
         this->cc().sp(this->cc().sp() * dist);
         this->cc().st(this->cc().st() * 1);
         BicycleCar orig_cc(this->cc());
@@ -270,74 +203,64 @@ std::vector<BicycleCar> PSPlanner::possible_goals(
 // find entry
 void PSPlanner::fe()
 {
-        if (this->ps().parallel())
+        this->c_ = 0;
+        if (this->ps().parallel()) {
                 return this->fe_parallel();
-        else
-                return this->fe_perpendicular();
+        } else {
+                this->guess_gc();
+                this->cc() = BicycleCar(this->gc());
+                this->cc().set_max_steer();
+                if (this->ps().right())
+                        this->cc().st(this->cc().st() * -1);
+                this->cc().sp(1);
+        }
 }
 
 void PSPlanner::fe_parallel()
 {
-        // angle for distance from "entry" corner
-        double dist_angl = this->ps().heading() + M_PI;
-        dist_angl += (this->ps().right()) ? - M_PI / 4 : + M_PI / 4;
-        // set bicycle car `bci` basic dimensions and heading
-        BicycleCar bci = BicycleCar(this->gc());
         BicycleCar bco = BicycleCar(this->gc());
-        bci.h(this->ps().heading());
-        // move 0.01 from the "entry" corner
-        bci.x(this->ps().x4() + 0.01 * cos(dist_angl));
-        bci.y(this->ps().y4() + 0.01 * sin(dist_angl));
-        // align with parking "top" of slot (move backward)
-        dist_angl = bci.h() + M_PI;
-        bci.x(bci.x() + bci.df() * cos(dist_angl));
-        bci.y(bci.y() + bci.df() * sin(dist_angl));
-        // align with "entry" to pakring slot (move outside)
-        dist_angl = this->ps().heading();
-        dist_angl += (this->ps().right()) ? + M_PI / 2 : - M_PI / 2;
-        bci.x(bci.x() + bci.w() / 2 * cos(dist_angl));
-        bci.y(bci.y() + bci.w() / 2 * sin(dist_angl));
-        // set default speed, steer
-        bci.st(bci.wb() / bci.mtr());
+        this->cc() = BicycleCar();
+        this->cc().sp(-0.01);
+        this->cc().set_max_steer();
         if (!this->ps().right())
-                bci.st(bci.st() * -1);
-        bci.sp(-0.01);
-        // BFS - init all starts
-        // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
-        double dist_diag = sqrt(pow(bci.w() / 2, 2) + pow(bci.df(), 2));
-        if (this->ps().right())
-                dist_angl = atan2(bci.y() - bci.rfy(), bci.x() - bci.rfx());
-        else
-                dist_angl = atan2(bci.y() - bci.lfy(), bci.x() - bci.lfx());
-        double DIST_ANGL = dist_angl;
-        std::queue<BicycleCar, std::list<BicycleCar>> q;
-        while (
-                (
-                        this->ps().right()
-                        && dist_angl < DIST_ANGL + 3 * M_PI / 4
+                this->cc().st(this->cc().st() * -1);
+        this->cc().h(this->ps().heading());
+        double angl_in_slot = this->ps().heading() - M_PI / 4;
+        if (!this->ps().right())
+                angl_in_slot += M_PI / 2;
+        this->cc().x(
+                this->ps().x4()
+                + this->cc().w()/2 * cos(
+                        this->ps().heading()
+                        + (this->ps().right() ? + M_PI / 2 : - M_PI / 2)
                 )
-                || (
-                        !this->ps().right()
-                        && dist_angl > DIST_ANGL - 3 * M_PI / 4
+                + (this->cc().df() + 0.01) * cos(
+                        this->ps().heading() + M_PI
                 )
-        ) {
-                this->cc() = BicycleCar(bci);
-                if (this->ps().right()) {
-                        this->cc().x(bci.rfx() + dist_diag * cos(dist_angl));
-                        this->cc().y(bci.rfy() + dist_diag * sin(dist_angl));
-                } else {
-                        this->cc().x(bci.lfx() + dist_diag * cos(dist_angl));
-                        this->cc().y(bci.lfy() + dist_diag * sin(dist_angl));
-                }
-                this->cc().h(this->ps().heading() + dist_angl - DIST_ANGL);
-                if (!this->collide()) {
-                        q.push(BicycleCar(this->cc()));
-                }
-                dist_angl += (this->ps().right()) ? + 0.01 : - 0.01;
+        );
+        this->cc().y(
+                this->ps().y4()
+                + this->cc().w()/2 * sin(
+                        this->ps().heading()
+                        + (this->ps().right() ? + M_PI / 2 : - M_PI / 2)
+                )
+                + (this->cc().df() + 0.01) * sin(
+                        this->ps().heading() + M_PI
+                )
+        );
+
+        std::queue<BicycleCar, std::list<BicycleCar>> q;
+        while (!this->collide()) {
+                q.push(this->cc());
+                this->cc().rotate(
+                        this->ps().x4(),
+                        this->ps().y4() - 0.01,
+                        ((this->ps().right()) ? 0.01 : -0.01)
+                );
         }
         // BFS - find entry current car `cc` and corresponding goal car `gc`
         unsigned int iter_cntr = 0;
-        while (!q.empty() && iter_cntr < 9) {
+        while (!q.empty() && iter_cntr < 30) {
                 this->cc() = BicycleCar(q.front());
                 q.pop();
                 while (
@@ -387,18 +310,23 @@ void PSPlanner::fe_perpendicular()
 
 void PSPlanner::fer()
 {
-        if (this->ps().parallel())
+        this->c_ = 0;
+        if (this->ps().parallel()) {
+                this->guess_gc();
+                this->cc() = BicycleCar(this->gc());
+                this->cc().set_max_steer();
+                if (!this->ps().right())
+                        this->cc().st(this->cc().st() * -1);
+                this->cc().sp(0.01);
                 return this->fer_parallel();
-        else
+        } else {
                 return this->fer_perpendicular();
+        }
 }
 
 void PSPlanner::fer_parallel()
 {
-        this->cc().st(this->cc().wb() / this->cc().mtr());
-        if (!this->ps().right())
-                this->cc().st(this->cc().st() * -1);
-        this->cc().sp(0.01);
+        this->cusps_.clear();
         while (!this->left()) {
                 while (!this->collide() && !this->left())
                         this->cc().next();
@@ -409,8 +337,13 @@ void PSPlanner::fer_parallel()
                         this->cc().next();
                         this->cc().st(this->cc().st() * -1);
                         this->c_++;
+                        this->cusps_.push_back(this->cc());
                 }
         }
+        if (this->cc().st() < 0) {
+                this->c_++;
+                this->cusps_.push_back(this->cc());
+        }
 }
 
 void PSPlanner::fer_perpendicular()
@@ -479,7 +412,7 @@ void PSPlanner::fer_perpendicular()
         double delta_3 = delta;
         if (D < 0)
                 delta_use[2] = false;
-        if (delta_use[0] && delta_use[1] && delta_use[22])
+        if (delta_use[0] && delta_use[1] && delta_use[2])
                 delta = std::max(delta_1, std::max(delta_2, delta_3));
         else if (delta_use[0] && delta_use[1])
                 delta = std::max(delta_1, delta_2);
@@ -504,7 +437,7 @@ void PSPlanner::fer_perpendicular()
                 this->cc().sp(-0.01);
         else
                 this->cc().sp(0.01);
-        this->cc().st(this->cc().wb() / this->cc().mtr());
+        this->cc().set_max_steer();
         if (this->ps().right())
                 this->cc().st(this->cc().st() * -1);
         while (!this->left()) {

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index c9278b6aa3f4add585b422bb737b0a74ba43a0d7..4df5c5cc9cecbcfa7be3c4ec17198893040c80ec 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -2,160 +2,3 @@
 #include "wvtest.h"
 
 #include "psp.h"
-
-WVTEST_MAIN("parallel parking slot planner")
-{
-        PSPlanner psp;
-        psp.ps().border(3, 3, 5, 3, 5, 8, 3, 8);
-        psp.gc().mtr(10);
-        psp.gc().wb(2);
-        psp.gc().w(1);
-        psp.gc().l(3);
-        psp.gc().he(1.5);
-        psp.gc().df(2 + 0.5);
-        psp.gc().dr(0.5);
-        psp.guess_gc();
-        psp.cc() = BicycleCar(psp.gc());
-
-        // init orientation
-        WVPASS(!psp.collide());
-        WVPASS(!psp.forward());
-        WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
-        WVPASS(psp.parked());
-        std::vector<std::tuple<double, double>> slot;
-        slot.push_back(std::make_tuple(psp.ps().x1(), psp.ps().y1()));
-        slot.push_back(std::make_tuple(psp.ps().x2(), psp.ps().y2()));
-        slot.push_back(std::make_tuple(psp.ps().x3(), psp.ps().y3()));
-        slot.push_back(std::make_tuple(psp.ps().x4(), psp.ps().y4()));
-        WVPASS(inside(psp.gc().x(), psp.gc().y(), slot));
-        WVPASS(inside(psp.gc().lfx(), psp.gc().lfy(), slot));
-        WVPASS(inside(psp.gc().lrx(), psp.gc().lry(), slot));
-        WVPASS(inside(psp.gc().rrx(), psp.gc().rry(), slot));
-        WVPASS(inside(psp.gc().rfx(), psp.gc().rfy(), slot));
-
-        // init collide
-        PSPlanner tpsp;
-        tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
-        tpsp.gc() = BicycleCar(psp.gc());
-        tpsp.cc() = BicycleCar(tpsp.gc());
-        WVPASS(tpsp.collide());
-        WVPASS(!tpsp.left());
-        tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
-        WVPASS(tpsp.left());
-
-        // entry point found by reverse
-        WVPASS(!psp.left());
-        psp.fer();
-        WVPASS(psp.left());
-        WVPASS(psp.parked());
-
-        // find entry
-        psp.cc() = BicycleCar(psp.gc());
-        WVPASS(!psp.left());
-        psp.fe();
-        WVPASS(psp.left());
-        WVPASS(psp.parked());
-}
-
-WVTEST_MAIN("backward perpendicullar parking slot planner")
-{
-        PSPlanner psp;
-        psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
-        psp.gc().mtr(10);
-        psp.gc().wb(2);
-        psp.gc().w(1);
-        psp.gc().l(3);
-        psp.gc().he(1.5);
-        psp.gc().df(2 + 0.5);
-        psp.gc().dr(0.5);
-        psp.guess_gc();
-        psp.cc() = BicycleCar(psp.gc());
-
-        // init orientation
-        WVPASS(!psp.collide());
-        WVPASS(!psp.forward());
-        WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
-        WVPASS(psp.parked());
-
-        // entry point found by reverse
-        WVPASS(!psp.left());
-        psp.fer();
-        WVPASS(psp.left());
-        WVPASS(psp.parked());
-
-        // find entry
-        psp.cc() = BicycleCar(psp.gc());
-        WVPASS(!psp.left());
-        psp.fe();
-        WVPASS(psp.left());
-        WVPASS(psp.parked());
-}
-
-WVTEST_MAIN("forward perpendicullar parking slot planner")
-{
-        PSPlanner psp;
-        psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
-        psp.gc().x(4);
-        psp.gc().y(4);
-        psp.gc().h(0);
-        psp.gc().mtr(10);
-        psp.gc().wb(2);
-        psp.gc().w(1);
-        psp.gc().l(3);
-        psp.gc().he(1.5);
-        psp.gc().df(2 + 0.5);
-        psp.gc().dr(0.5);
-        psp.cc() = BicycleCar(psp.gc());
-
-        // init orientation
-        WVPASS(!psp.collide());
-        WVPASS(psp.forward());
-        WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
-        WVPASS(psp.parked());
-
-        // entry point found by reverse
-        WVPASS(!psp.left());
-        psp.fer();
-        WVPASS(psp.left());
-        WVPASS(psp.parked());
-
-        // find entry
-        psp.cc() = BicycleCar(psp.gc());
-        WVPASS(!psp.left());
-        psp.fe();
-        WVPASS(psp.left());
-        WVPASS(psp.parked());
-}
-
-WVTEST_MAIN("possible goals test")
-{
-        PSPlanner psp;
-        psp.gc().x(0);
-        psp.gc().y(0);
-        psp.gc().h(0);
-        psp.gc().mtr(10);
-        psp.gc().wb(2);
-        psp.gc().w(1);
-        psp.gc().l(3);
-        psp.gc().he(1.5);
-        psp.gc().df(2 + 0.5);
-        psp.gc().dr(0.5);
-        psp.cc() = BicycleCar(psp.gc());
-        psp.cc().sp(1);
-        psp.cc().st(0);
-        psp.cc().next();
-        WVPASSEQ_DOUBLE(psp.gc().x() + 1, psp.cc().x(), 0.00001);
-        WVPASSEQ_DOUBLE(psp.gc().y(), psp.cc().y(), 0.00001);
-        WVPASSEQ_DOUBLE(psp.gc().h(), psp.cc().h(), 0.00001);
-        WVPASS(psp.possible_goals().size() > 0);
-        double i = 1;
-        for (auto &c: psp.possible_goals()) {
-                WVPASSEQ_DOUBLE(psp.gc().x() + 1 + i, c.x(), 0.00001);
-                WVPASSEQ_DOUBLE(psp.gc().y(), c.y(), 0.00001);
-                WVPASSEQ_DOUBLE(psp.gc().h(), c.h(), 0.00001);
-                i++;
-        }
-        WVPASSEQ_DOUBLE(psp.gc().x() + 1, psp.cc().x(), 0.00001);
-        WVPASSEQ_DOUBLE(psp.gc().y(), psp.cc().y(), 0.00001);
-        WVPASSEQ_DOUBLE(psp.gc().h(), psp.cc().h(), 0.00001);
-}