Update fip with right side parking

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 6207645b2f33c81e8c29d7767a5e102d0c90bc1e..b878eedbc1182a450cdc6e5ef300feba608cdb64 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -100,11 +100,7 @@ void ParallelSlot::fip()
         // RRTNode.s() works as iteration level
         std::queue<BicycleCar *, std::list<BicycleCar *>> q;
         std::queue<BicycleCar *, std::list<BicycleCar *>> empty;
-
-        // TODO add init nodes
-        // for now just copy fpose()
-        bool left = false; // right parking slot
-        float di = -1;
+        int di = -1;
         // new pose for parallel parking to right slot
         float tnx;
         float tny;
@@ -122,20 +118,15 @@ void ParallelSlot::fip()
         // move left by car width / 2
         tnx = CC->x() + CC->width() / 2 * cos(CC->h() + M_PI / 2);
         tny = CC->y() + CC->width() / 2 * sin(CC->h() + M_PI / 2);
-        // move down
-        nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
-        ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
         if (this->slotSide() == LEFT) {
-                std::cerr << "left PS" << std::endl;
-                left = true;
                 di = 1;
                 // move right by car width / 2
                 tnx = CC->x() + CC->width() / 2 * cos(CC->h() - M_PI / 2);
                 tny = CC->y() + CC->width() / 2 * sin(CC->h() - M_PI / 2);
-                // move down
-                nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
-                ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
         }
+        // move down
+        nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
+        ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
         BicycleCar *B = new BicycleCar(nx, ny, CC->h());
         this->DH(di * 0.01 / CC->out_radi());
         BicycleCar *c;
@@ -147,7 +138,6 @@ void ParallelSlot::fip()
                 i += 1;
         }
         delete c; // not in q and collide
-        // BFS
         while (!q.empty()) {
                 c = q.front();
                 q.pop();
@@ -206,28 +196,35 @@ void ParallelSlot::fipr(BicycleCar *B)
 
 BicycleCar *ParallelSlot::flnc(BicycleCar *B)
 {
-        // TODO find last not colliding
-        // for now just copy flast()
         RRTNode *cc;
-        if (int(B->s()) % 2 == 0) {
-                cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
+        if (this->slotSide() == LEFT) {
+                if (int(B->s()) % 2 == 0)
+                        cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
+                else
+                        cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
         } else {
-                cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
+                if (int(B->s()) % 2 == 0)
+                        cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
+                else
+                        cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
         }
-        float di = -1;
-        if (this->slotSide() == LEFT)
-                di = 1;
         BicycleCar *p;
         int i = 1;
-        p = B->move(cc, i * di * this->DH());
-        while (!this->slot().collide(p->frame())) {
+        p = B->move(cc, i * this->DH());
+        while (
+                !this->slot().collide(p->frame())
+                && std::abs(this->slotHeading() - p->h()) < M_PI / 2
+        ) {
                 delete p;
                 i += 10;
                 p = B->move(cc, i * this->DH());
         }
         i -= 10;
-        p = B->move(cc, i * di * this->DH());
-        while (!this->slot().collide(p->frame())) {
+        p = B->move(cc, i * this->DH());
+        while (
+                !this->slot().collide(p->frame())
+                && std::abs(this->slotHeading() - p->h()) < M_PI / 2
+        ) {
                 if (this->isInside(p)) {
                         i += 1;
                         break;