Protect usage of `n` when it is `nullptr`

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

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index f888d401618d8aaa90fc451ccf44a9203bb15ea7..3f903aa6527a8a2a0be99d78de7bf80c6e3b26de 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -145,9 +145,16 @@ void ParallelSlot::fip(
                                 && p->h() < this->slotHeading()
                         ) || (
                                 this->slotSide() == RIGHT
-                                && p->h() > this->slotHeading()
+                                && (
+                                        p->h() > this->slotHeading()
+                                        || p->h() < 0
+                                )
                         ))
                 ) {
+                        if (tmpc.size() == 0) {
+                                i = 0;
+                                p = this->getFP();
+                        }
                         bool end = false;
                         std::vector<RRTEdge *> eds = p->frame();
                         for (auto o: co)
@@ -257,6 +264,10 @@ void ParallelSlot::fipf(
                         && p->h() < this->slotHeading()
                 ))
         ) {
+                if (tmpc.size() == 0) {
+                        i = 0;
+                        p = this->getFPf();
+                }
                 bool end = false;
                 std::vector<RRTEdge *> eds = p->frame();
                 for (auto o: co)
@@ -680,25 +691,47 @@ BicycleCar *ParallelSlot::getFP()
 
 BicycleCar *ParallelSlot::getISPP(BicycleCar *B)
 {
+        // rigt side (for right parking slot)
         float x = this->slot().bnodes().back()->x();
         float y = this->slot().bnodes().back()->y();
-        float y0;
-        if (this->slotSide() == LEFT) // TODO only for backward parking now
-                y0 = B->ccl()->y();
-        else
-                y0 = B->ccr()->y();
+        float x1;
+        float y1;
+        if (this->slotSide() == LEFT) {
+                x1 = B->ccl()->x();
+                y1 = B->ccl()->y();
+        } else {
+                x1 = B->ccr()->x();
+                y1 = B->ccr()->y();
+        }
         float IR = BCAR_IN_RADI;
         float a = 1;
-        float b = -2 * x;
-        float c = pow(x, 2) + pow(y - y0, 2) - pow(IR, 2);
+        float b = (x1 - x) * 2 * cos(B->h()) + (y1 - y) * 2 * sin(B->h());
+        float c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
         float D = pow(b, 2) - 4 * a * c;
-        float x0;
-        if (this->slotSide() == LEFT)
-                x0 = -b - sqrt(D);
-        else
-                x0 = -b + sqrt(D);
-        x0 /= 2 * a;
-        return new BicycleCar(x0, B->y(), B->h());
+        float delta;
+        delta = -b - sqrt(D);
+        delta /= 2 * a;
+        float delta_1 = delta;
+        // left front (for right parking slot)
+        x = this->slot().bnodes().front()->x();
+        y = this->slot().bnodes().front()->y();
+        IR = BCAR_OUT_RADI;
+        a = 1;
+        b = (x1 - x) * 2 * cos(B->h()) + (y1 - y) * 2 * sin(B->h());
+        c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
+        D = pow(b, 2) - 4 * a * c;
+        //delta = -b + sqrt(D);
+        //delta /= 2 * a;
+        float delta_2 = delta;
+        delta = -b - sqrt(D);
+        delta /= 2 * a;
+        float delta_3 = delta;
+        delta = std::max(delta_1, std::max(delta_2, delta_3));
+        return new BicycleCar(
+                B->x() + delta * cos(B->h()),
+                B->y() + delta * sin(B->h()),
+                B->h()
+        );
 }
 
 BicycleCar *ParallelSlot::getFPf()
@@ -742,7 +775,7 @@ BicycleCar *ParallelSlot::getISPPf(BicycleCar *B)
         float c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
         float D = pow(b, 2) - 4 * a * c;
         float delta;
-        delta = -b - sqrt(D); // Use just `(-b - D) / (2a)` formula.
+        delta = -b - sqrt(D); // TODO why this works?
         delta /= 2 * a;
         float delta_1 = delta;
         // left front (for right parking slot)
@@ -794,21 +827,34 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
         RRTNode *n = this->getMidd();
+        if (n == nullptr)
+                return si;
+        BicycleCar *bc = new BicycleCar(n->x(), n->y(), n->h());
+        RRTNode *cc;
+        if (this->slotSide() == LEFT)
+                cc = bc->ccl();
+        else
+                cc = bc->ccr();
         if (this->slotType() == PARALLEL) {
-                if (n != nullptr) {
-                        si.x0 = n->x() + 1.5 * BCAR_LENGTH * cos(n->h());
-                        si.y0 = n->y() + 1.5 * BCAR_LENGTH * sin(n->h());
-                        si.h0 = n->h();
-                } else {
-                        si.x0 = this->slot().bnodes().front()->x();
-                        si.y0 = this->slot().bnodes().front()->y();
-                        si.h0 = this->slotHeading();
-                }
+                BicycleCar *nbc = bc->move(cc, this->slotHeading() - n->h());
+                si.x0 = nbc->x();
+                si.y0 = nbc->y();
+                si.h0 = nbc->h();
                 si.x = BCAR_WIDTH;
                 si.y = BCAR_WIDTH;
                 si.h = M_PI / 8;
         } else {
-                // TODO
+                BicycleCar *nbc;
+                if (this->slotSide() == LEFT)
+                        nbc = bc->move(cc, M_PI/4);
+                else
+                        nbc = bc->move(cc, -M_PI/4);
+                si.x0 = nbc->x();
+                si.y0 = nbc->y();
+                si.h0 = nbc->h();
+                si.x = BCAR_WIDTH;
+                si.y = BCAR_WIDTH;
+                si.h = M_PI / 8;
         }
         return si;
 }