Merge branch 'fix/perpendicular-parking'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 86d60a0717b099de70c599e89af4a5b09b6e806a..a05e84cd2b9bf506c7f9031dc2a8dea843e87351 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,9 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Heading overflow/underflow.
 - Negative discriminant.
 - Parallel parking slot entry find.
+- 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.
 
 ## 0.2.0 - 2019-08-05
 ### Deleted

diff --git a/src/psp.cc b/src/psp.cc
index 894893704085e552ea67bb32e868be3603110645..7d00cc6ad9a0d142e905d45c00ddabb717f7f760 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -21,16 +21,18 @@ bool PSPlanner::collide()
 
 bool PSPlanner::forward()
 {
-        double heading = this->ps().heading();
+        if (this->ps().parallel())
+                return false;
+        double heading = atan2(
+                this->ps().y2() - this->ps().y1(),
+                this->ps().x2() - this->ps().x1()
+        );
         while (heading < 0) heading += 2 * M_PI;
-        if (!this->ps().parallel())
-                heading -= M_PI / 2;
         double h = this->gc().h();
         while (h < 0) h += 2 * M_PI;
-        if (-0.00001 < heading - h && heading - h < 0.00001)
+        if (std::abs(heading - h) < M_PI / 4)
                 return true;
-        else
-                return false;
+        return false;
 }
 
 void PSPlanner::guess_gc()
@@ -41,20 +43,41 @@ void PSPlanner::guess_gc()
         double dts = + M_PI / 2; // direction to slot
         if (this->ps().right())
                 dts = - M_PI / 2;
+        dts *= 0.99; // precision workaround
         if (this->ps().parallel()) {
                 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 {
-                x += (this->ps().x4() - this->ps().x1()) / 2;
-                x += (this->gc().df() + 0.01) * cos(h + dts);
-                y += (this->ps().y4() - this->ps().y1()) / 2;
-                y += (this->gc().df() + 0.01) * sin(h + dts);
-                if (this->ps().right())
-                        h += M_PI / 2;
-                else
-                        h -= M_PI / 2;
+                dts = atan2(
+                        this->ps().y2() - this->ps().y1(),
+                        this->ps().x2() - this->ps().x1()
+                );
+                dts *= 1.01; // precision workaround
+                if (std::abs(dts - this->ps().heading()) < M_PI / 2) {
+                        x = this->ps().x4();
+                        y = this->ps().y4();
+                        h = dts;
+                        x += (this->gc().dr() + 0.01) * cos(h);
+                        y += (this->gc().dr() + 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);
+                } else {
+                        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);
+                }
         }
         while (h > M_PI)
                 h -= 2 * M_PI;
@@ -118,13 +141,8 @@ std::vector<BicycleCar> PSPlanner::possible_inits(
 )
 {
         std::vector<BicycleCar> pi;
-        if (!this->ps().parallel()) {
-                this->cc().sp(this->cc().sp() * -dist);
-                this->cc().st(this->cc().st() * -1);
-        } else {
-                this->cc().sp(this->cc().sp() * dist);
-                this->cc().st(this->cc().st() * 1);
-        }
+        this->cc().sp(this->cc().sp() * dist);
+        this->cc().st(this->cc().st() * 1);
         BicycleCar orig_cc(this->cc());
         for (unsigned int i = 0; i < cnt; i++) {
                 this->cc().next();
@@ -243,7 +261,13 @@ void PSPlanner::fe_perpendicular()
         //
         //      Another approach could be testing angles from the
         //      beginning of the escape parkig slot maneuver.
-        return fer_perpendicular();
+        if (this->forward())
+                this->cc().sp(-0.01);
+        else
+                this->cc().sp(0.01);
+        while (!this->left())
+                this->cc().next();
+        return;
 }
 
 void PSPlanner::fer()
@@ -361,9 +385,9 @@ void PSPlanner::fer_perpendicular()
         this->cc().h(cc_h);
         // get current car `cc` out of slot
         if (this->forward())
-                this->cc().sp(-0.1);
+                this->cc().sp(-0.01);
         else
-                this->cc().sp(0.1);
+                this->cc().sp(0.01);
         this->cc().st(this->cc().wb() / this->cc().mtr());
         if (this->ps().right())
                 this->cc().st(this->cc().st() * -1);

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 7a343cd8ca982c5a0bc23179e100222859963f4c..e663b75b22381be88941c136d9cb706a5e6f0466 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -19,7 +19,7 @@ WVTEST_MAIN("parallel parking slot planner")
 
         // init orientation
         WVPASS(!psp.collide());
-        WVPASS(psp.forward());
+        WVPASS(!psp.forward());
         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
         WVPASS(psp.parked());
         std::vector<std::tuple<double, double>> slot;