Add perpendicular reverse parking proporsal

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index d0a01df99df3d41317b8cd9641447042ffcdffa9..ed1446de490c20a83eab26a9392b45d6ebe59f76 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -151,6 +151,16 @@ void ParallelSlot::fipr(BicycleCar *B)
         int di = 1;
         if (this->slotSide() == LEFT)
                 di = -1;
+        if (this->slotType() == PERPENDICULAR) {
+                cusp.push_back(new RRTNode(
+                        B->x() - di * B->length(),
+                        B->y(),
+                        B->h()
+                ));
+                std::reverse(cusp.begin(), cusp.end());
+                this->cusp().push_back(cusp);
+                return;
+        }
         this->DH(di * 0.01 / B->out_radi());
         BicycleCar *c;
         c = this->flncr(B);