Update slotplanner init pose

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index b208c39a0c3b6629df929073458d520b44594a51..5e719a0e4871026102e5328ee74f2b6c70e5cd6d 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -106,26 +106,38 @@ void ParallelSlot::fip()
         // for now just copy fpose()
         bool left = false; // right parking slot
         float di = -1;
-        BicycleCar *CC = new BicycleCar(
-                this->fposecenter()->x(),
-                this->fposecenter()->y() - 0.01,
-                this->slotHeading()
-        );
-        BicycleCar *B = new BicycleCar(
-                CC->x() - CC->width() / 2,
-                CC->y() - (CC->length() + CC->wheelbase()) / 2,
-                this->slotHeading()
-        );
-        if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
+        // 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);
+        // 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;
-                delete B;
-                B = new BicycleCar(
-                        CC->x() + CC->width() / 2,
-                        CC->y() - (CC->length() + CC->wheelbase()) / 2,
-                        this->slotHeading()
-                );
+                // 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());
         this->DH(di * 0.01 / CC->out_radi());
         BicycleCar *c;
         int i = 0;