Add perpendicular slot planner proposal

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index f871d0f4b7042e848056e5199a5983da0c72be90..d0faffab6ec11b17d7160a50e9429a0ec9fe568a 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -101,6 +101,79 @@ void ParallelSlot::fip(
         std::vector<SegmentObstacle>& so
 )
 {
+        if (this->slotType() == PERPENDICULAR) {
+                // TODO different slot headings
+                // this is jus for slot heading = pi / 2
+                this->DH(0.01 / BCAR_TURNING_RADIUS);
+                BicycleCar *perc = nullptr;
+                RRTNode *cc = nullptr;
+                BicycleCar *p = nullptr;
+                int i = 0;
+                float x;
+                float h;
+                float y;
+                // parking backward
+                x = this->slot().bnodes()[3]->x();
+                y = this->slot().bnodes()[3]->y();
+                h = 0;
+                // -> top
+                x -= BCAR_DIST_FRONT;
+                y = this->slot().bnodes()[3]->y();
+                y += BCAR_OUT_RRADI - BCAR_TURNING_RADIUS;
+                if (perc)
+                        delete perc;
+                perc = new BicycleCar(x, y, h);
+                if (cc)
+                        delete cc;
+                cc = perc->ccl();
+                if (p)
+                        delete p;
+                p = perc->move(cc, i * this->DH());
+                while (p->x() < 0) {
+                        delete p;
+                        p = perc->move(cc, i * this->DH());
+                        i++;
+                }
+                // -> bottom
+                // (reset for parking backward)
+                x = this->slot().bnodes()[0]->x();
+                y = this->slot().bnodes()[0]->y();
+                h = 0;
+                // -> bottom
+                x -= BCAR_DIST_FRONT;
+                // get y from quadratic equation
+                float tmpD = pow(-2 * this->slot().bnodes()[0]->y(), 2);
+                tmpD -= 4 * (
+                        pow(x - this->slot().bnodes()[0]->x(), 2) +
+                        pow(this->slot().bnodes()[0]->y(), 2) -
+                        pow(BCAR_IN_RADI, 2)
+                );
+                y = 2 * this->slot().bnodes()[0]->y();
+                y += sqrt(tmpD);
+                y /= 2;
+                y -= BCAR_TURNING_RADIUS;
+                // -- end of quadratic equation
+                if (perc)
+                        delete perc;
+                perc = new BicycleCar(x, y, h);
+                if (cc)
+                        delete cc;
+                cc = perc->ccl();
+                if (p)
+                        delete p;
+                p = perc->move(cc, i * this->DH());
+                while (p->x() < 0) {
+                        delete p;
+                        p = perc->move(cc, i * this->DH());
+                        i++;
+                }
+                // store nodes
+                std::vector<RRTNode *> cusp;
+                cusp.push_back(new RRTNode(p->x(), p->y(), p->h()));
+                cusp.push_back(new RRTNode(x, y, h));
+                this->cusp().push_back(cusp);
+                return;
+        }
         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
         // RRTNode.s() works as iteration level
         std::queue<BicycleCar *, std::list<BicycleCar *>> q;