Merge branch 'feature/perpendicular-slotplanner'

[hubacji1/iamcar.git] / decision_control / slotplanner.cc

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 4b80a17f876447841cf19c6e41ea6d5312612468..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);
@@ -408,9 +418,15 @@ BicycleCar *ParallelSlot::getEP()
                 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->slotType() == PARALLEL) {
+                // move down
+                nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
+                ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
+        } else {
+                // move down
+                nx = tnx + (CC->length() - CC->wheelbase()) / 2 * cos(CC->h());
+                ny = tny + (CC->length() - CC->wheelbase()) / 2 * sin(CC->h());
+        }
         return new BicycleCar(nx, ny, CC->h());
 }
 
@@ -437,15 +453,35 @@ BicycleCar *ParallelSlot::getFP()
         float h = this->slotHeading();
         float nx;
         float ny;
-        if (this->slotSide() == LEFT) {
-                nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
-                ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
+        if (this->slotType() == PARALLEL) {
+                if (this->slotSide() == LEFT) {
+                        nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
+                        ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
+                } else {
+                        nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
+                        ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
+                }
+                x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
+                y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
         } else {
-                nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
-                ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
+                if (this->slotSide() == LEFT) {
+                        h -= M_PI / 2;
+                        nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
+                                * cos(h + M_PI);
+                        ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
+                                * sin(h + M_PI);
+                        x = nx + (BCAR_WIDTH + 0.01) * cos(h + M_PI / 2);
+                        y = ny + (BCAR_WIDTH + 0.01) * sin(h + M_PI / 2);
+                } else {
+                        h += M_PI / 2;
+                        nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
+                                * cos(h - M_PI);
+                        ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
+                                * sin(h - M_PI);
+                        x = nx + (BCAR_WIDTH / 2 + 0.01) * cos(h - M_PI / 2);
+                        y = ny + (BCAR_WIDTH / 2 + 0.01) * sin(h - M_PI / 2);
+                }
         }
-        x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
-        y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
         return new BicycleCar(x, y, h);
 }
 
@@ -489,9 +525,13 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
         }
         si.r = CC->diag_radi();
         si.sh = this->slotHeading();
-        si.h = this->slotHeading() - acos(EDIST( // TODO generalize
-                this->slot().bnodes()[0],
-                this->slot().bnodes()[1]
-        ) / BCAR_LENGTH);
+        if (this->slotType() == PARALLEL) {
+                si.h = this->slotHeading() - acos(EDIST(
+                        this->slot().bnodes()[0],
+                        this->slot().bnodes()[1]
+                ) / BCAR_LENGTH);
+        } else {
+                si.h = M_PI /2;
+        }
         return si;
 }