Add perpendicular reverse parking proporsal

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

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index b878eedbc1182a450cdc6e5ef300feba608cdb64..ed1446de490c20a83eab26a9392b45d6ebe59f76 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -53,37 +53,11 @@ float ParallelSlot::slotHeading()
 
 SlotSide ParallelSlot::slotSide()
 {
-        if (!this->slotSide_) {
-                float y0 = this->slot().bnodes()[0]->y();
-                float x0 = this->slot().bnodes()[0]->x();
-                float y1 = this->slot().bnodes()[1]->y();
-                float x1 = this->slot().bnodes()[1]->x();
-                float y3 = this->slot().bnodes()[3]->y();
-                float x3 = this->slot().bnodes()[3]->x();
-                if (sgn((x1 - x3) * (y0 - y3) - (y1 - y3) * (x0 - x3)) < 0)
-                        this->slotSide_ = LEFT;
-                else
-                        this->slotSide_ = RIGHT;
-        }
         return this->slotSide_;
 }
 
 SlotType ParallelSlot::slotType()
 {
-        if (!this->slotType_) {
-                float d1 = EDIST(
-                        this->slot().bnodes()[0],
-                        this->slot().bnodes()[1]
-                );
-                float d2 = EDIST(
-                        this->slot().bnodes()[1],
-                        this->slot().bnodes()[2]
-                );
-                if (d1 > d2)
-                        this->slotType_ = PERPENDICULAR;
-                else
-                        this->slotType_ = PARALLEL;
-        }
         return this->slotType_;
 }
 
@@ -93,6 +67,34 @@ void ParallelSlot::DH(float dh)
         this->DH_ = dh;
 }
 
+void ParallelSlot::setAll()
+{
+        // slot side
+        float y0 = this->slot().bnodes()[0]->y();
+        float x0 = this->slot().bnodes()[0]->x();
+        float y1 = this->slot().bnodes()[1]->y();
+        float x1 = this->slot().bnodes()[1]->x();
+        float y3 = this->slot().bnodes()[3]->y();
+        float x3 = this->slot().bnodes()[3]->x();
+        if (sgn((x1 - x3) * (y0 - y3) - (y1 - y3) * (x0 - x3)) < 0)
+                this->slotSide_ = LEFT;
+        else
+                this->slotSide_ = RIGHT;
+        // slot type
+        float d1 = EDIST(
+                this->slot().bnodes()[0],
+                this->slot().bnodes()[1]
+        );
+        float d2 = EDIST(
+                this->slot().bnodes()[1],
+                this->slot().bnodes()[2]
+        );
+        if (d1 > d2)
+                this->slotType_ = PERPENDICULAR;
+        else
+                this->slotType_ = PARALLEL;
+}
+
 // other
 void ParallelSlot::fip()
 {
@@ -101,33 +103,10 @@ void ParallelSlot::fip()
         std::queue<BicycleCar *, std::list<BicycleCar *>> q;
         std::queue<BicycleCar *, std::list<BicycleCar *>> empty;
         int di = -1;
-        // new pose for parallel parking to right slot
-        float tnx;
-        float tny;
-        float nx;
-        float ny;
-        // temporary tnx is angle
-        tnx = this->slotHeading() + M_PI;
-        if (this->slotSide() == RIGHT)
-                tnx -= M_PI / 4;
-        else
-                tnx += M_PI / 4;
-        nx = this->fposecenter()->x() + 0.01 * cos(tnx);
-        ny = this->fposecenter()->y() + 0.01 * sin(tnx);
-        BicycleCar *CC = new BicycleCar(nx, ny, this->slotHeading());
-        // 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);
-        if (this->slotSide() == LEFT) {
+        if (this->slotSide() == LEFT)
                 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());
-        BicycleCar *B = new BicycleCar(nx, ny, CC->h());
+        BicycleCar *CC = this->getEPC();
+        BicycleCar *B = this->getEP();
         this->DH(di * 0.01 / CC->out_radi());
         BicycleCar *c;
         int i = 0;
@@ -172,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);
@@ -413,6 +402,50 @@ void ParallelSlot::fpose()
         }
 }
 
+BicycleCar *ParallelSlot::getEP()
+{
+        // new pose for parallel parking to right slot
+        float tnx;
+        float tny;
+        float nx;
+        float ny;
+        BicycleCar *CC = this->getEPC();
+        // 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);
+        if (this->slotSide() == LEFT) {
+                // 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);
+        }
+        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());
+}
+
+BicycleCar *ParallelSlot::getEPC()
+{
+        // new pose for parallel parking to right slot
+        float ta;
+        float nx;
+        float ny;
+        ta = this->slotHeading() + M_PI;
+        if (this->slotSide() == RIGHT)
+                ta -= M_PI / 4;
+        else
+                ta += M_PI / 4;
+        nx = this->fposecenter()->x() + 0.01 * cos(ta);
+        ny = this->fposecenter()->y() + 0.01 * sin(ta);
+        return new BicycleCar(nx, ny, this->slotHeading());
+}
+
 BicycleCar *ParallelSlot::getFP()
 {
         float x = this->slot().bnodes()[3]->x();
@@ -420,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);
 }
 
@@ -458,17 +511,27 @@ bool ParallelSlot::isInside(BicycleCar *c)
 struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
-        BicycleCar *CC = new BicycleCar(
-                this->fposecenter()->x(),
-                this->fposecenter()->y() - 0.01,
-                this->slotHeading()
-        );
+        BicycleCar *CC = this->getEPC();
         si.x = this->slot().bnodes()[0]->x();
         si.y = this->slot().bnodes()[0]->y();
+        if (this->slotSide() == RIGHT) {
+                si.dx = 1;
+                si.dy = 1;
+                si.dh = 1;
+        } else {
+                si.dx = -1;
+                si.dy = -1;
+                si.dh = -1;
+        }
         si.r = CC->diag_radi();
-        si.h = this->slotHeading() - acos(EDIST( // TODO generalize
-                this->slot().bnodes()[0],
-                this->slot().bnodes()[1]
-        ) / BCAR_LENGTH);
+        si.sh = this->slotHeading();
+        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;
 }