Add inside slot position for perp. forward parking

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 1c32eb0dc8a88186e1a61eab9fda94eaaa32703e..de4b4d5390ea57e882d5b42e590778851d8fa519 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -668,6 +668,48 @@ BicycleCar *ParallelSlot::getFPf()
         return new BicycleCar(x, y, h);
 }
 
+BicycleCar *ParallelSlot::getISPPf(BicycleCar *B)
+{
+        float x = this->slot().bnodes().front()->x();
+        float y = this->slot().bnodes().front()->y();
+        float y0;
+        if (this->slotSide() == LEFT)
+                y0 = B->ccl()->y();
+        else
+                y0 = B->ccr()->y();
+        float IR = BCAR_IN_RADI;
+        float a = 1;
+        float b = -2 * x;
+        float c = pow(x, 2) + pow(y - y0, 2) - pow(IR, 2);
+        float D = pow(b, 2) - 4 * a * c;
+        float x0;
+        if (this->slotSide() == LEFT)
+                x0 = -b - sqrt(D);
+        else
+                x0 = -b + sqrt(D);
+        x0 /= 2 * a;
+        float x0_1 = x0;
+        // left front
+        x = this->slot().bnodes().back()->x();
+        y = this->slot().bnodes().back()->y();
+        IR = BCAR_OUT_RADI;
+        a = 1;
+        b = -2 * x;
+        c = pow(x, 2) + pow(y - y0, 2) - pow(IR, 2);
+        D = pow(b, 2) - 4 * a * c;
+        if (this->slotSide() == LEFT)
+                x0 = -b + sqrt(D);
+        else
+                x0 = -b - sqrt(D);
+        x0 /= 2 * a;
+        float x0_2 = x0;
+        if (this->slotSide() == LEFT)
+                x0 = std::max(x0_1, x0_2);
+        else
+                x0 = std::min(x0_1, x0_2);
+        return new BicycleCar(x0, B->y(), B->h());
+}
+
 bool ParallelSlot::isInside(BicycleCar *c)
 {
         bool inside = true;

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index ef539cd314b7a0d05e19a63786561a5a26e4951a..d9a98edf2eb9f8258eac134ad06944c1008601b7 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -121,6 +121,7 @@ class ParallelSlot {
                 @param B The pose to start from.
                 */
                 BicycleCar *getISPP(BicycleCar *B);
+                BicycleCar *getISPPf(BicycleCar *B); // perp. forward parking
                 /** Return true if car is inside slot */
                 bool isInside(BicycleCar *c);
                 /** Return values to set sampling function of RRT */