Update fipr with right side parking

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 4e2fc378ff0b99fc4c9ec85e5051dee660b2a251..6207645b2f33c81e8c29d7767a5e102d0c90bc1e 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -177,11 +177,8 @@ createcuspandfinish:
 
 void ParallelSlot::fipr(BicycleCar *B)
 {
-        // TODO for right parallel parking also
-        // it's only for lpar scenario now
         std::vector<RRTNode *> cusp;
         cusp.push_back(new RRTNode(B->x(), B->y(), B->h()));
-        // just copied from fip()
         int di = 1;
         if (this->slotSide() == LEFT)
                 di = -1;
@@ -189,7 +186,13 @@ void ParallelSlot::fipr(BicycleCar *B)
         BicycleCar *c;
         c = this->flncr(B);
         c->s(B->s() + 1);
-        while (this->slot().collide(new RRTNode(c->lfx(), c->lfy(), 0))) {
+        while ((
+                this->slotSide() == LEFT
+                && this->slot().collide(new RRTNode(c->lfx(), c->lfy(), 0))
+        ) || (
+                this->slotSide() == RIGHT
+                && this->slot().collide(new RRTNode(c->rfx(), c->rfy(), 0))
+        )) {
                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
                 BicycleCar *cc = this->flncr(c);
                 cc->s(c->s() + 1);
@@ -239,19 +242,38 @@ BicycleCar *ParallelSlot::flnc(BicycleCar *B)
 
 BicycleCar *ParallelSlot::flncr(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();
-        else
-                cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
+        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 {
+                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();
+        }
         BicycleCar *p;
         int i = 1;
         p = B->move(cc, i * this->DH());
         while (
                 !this->slot().collide(p->frame())
-                && this->slot().collide(new RRTNode(p->lfx(), p->lfy(), 0))
+                && ((
+                        this->slotSide() == LEFT
+                        && this->slot().collide(new RRTNode(
+                                p->lfx(),
+                                p->lfy(),
+                                0
+                        ))
+                ) || (
+                        this->slotSide() == RIGHT
+                        && this->slot().collide(new RRTNode(
+                                p->rfx(),
+                                p->rfy(),
+                                0
+                        ))
+                ))
         ) {
                 delete p;
                 i += 10;
@@ -260,15 +282,28 @@ BicycleCar *ParallelSlot::flncr(BicycleCar *B)
         i -= 10;
         p = B->move(cc, i * this->DH());
         while (!this->slot().collide(p->frame())) {
-                if(!this->slot().collide(new RRTNode(p->lfx(), p->lfy(), 0))) {
+                if(
+                        this->slotSide() == LEFT
+                        && !this->slot().collide(new RRTNode(
+                                p->lfx(),
+                                p->lfy(),
+                                0
+                        ))
+                ) {
+                        i += 1;
+                        break;
+                }
+                if(
+                        this->slotSide() == RIGHT
+                        && !this->slot().collide(new RRTNode(
+                                p->rfx(),
+                                p->rfy(),
+                                0
+                        ))
+                ) {
                         i += 1;
                         break;
                 }
-                //if (this->DH() < 0 && p->lfx() >= 0) {
-                //        i += 1;
-                //        break;
-                //}
-                delete p;
                 i += 1;
                 p = B->move(cc, i * this->DH());
         }