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 7a9eb7482f60a83a5014ca82c6657d15125d4469..3f903aa6527a8a2a0be99d78de7bf80c6e3b26de 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -151,6 +151,10 @@ void ParallelSlot::fip(
                                 )
                         ))
                 ) {
+                        if (tmpc.size() == 0) {
+                                i = 0;
+                                p = this->getFP();
+                        }
                         bool end = false;
                         std::vector<RRTEdge *> eds = p->frame();
                         for (auto o: co)
@@ -260,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)
@@ -819,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;
 }