Add find init pose in forward direction method

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

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 1c32eb0dc8a88186e1a61eab9fda94eaaa32703e..e4a61ebdbd88215c56b950be72d809c01b0ed54b 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -217,6 +217,59 @@ createcuspandfinish:
         std::swap(q, empty);
 }
 
+void ParallelSlot::fipf(
+        std::vector<CircleObstacle>& co,
+        std::vector<SegmentObstacle>& so
+)
+{
+        this->setAll();
+        std::vector<RRTNode *> tmpc;
+        BicycleCar *tmpf = this->getFPf();
+        BicycleCar *tmpb = this->getISPPf(tmpf);
+        RRTNode *cc;
+        if (this->slotSide() == LEFT)
+                cc = tmpb->ccl();
+        else
+                cc = tmpb->ccr();
+        if (this->slotSide() == LEFT)
+                this->DH(-1 * 0.5 / tmpb->out_radi());
+        else
+                this->DH(1 * 0.5 / tmpb->out_radi());
+        BicycleCar *p;
+        int i = 1;
+        p = tmpb->move(cc, i * this->DH());
+        while (
+                !this->slot().collide(p->frame())
+                && ((
+                        this->slotSide() == LEFT
+                        && p->h() > this->slotHeading()
+                ) || (
+                        this->slotSide() == RIGHT
+                        && p->h() < this->slotHeading()
+                ))
+        ) {
+                bool end = false;
+                std::vector<RRTEdge *> eds = p->frame();
+                for (auto o: co)
+                        if (o.collide(eds))
+                                end = true;
+                for (auto o: so)
+                        if (o.collide(eds))
+                                end = true;
+                for (auto e: eds)
+                        delete e;
+                if (end)
+                        break;
+                this->goals_.push_back(p);
+                tmpc.push_back(p);
+                i += 1;
+                p = tmpb->move(cc, i * this->DH());
+        }
+        if (tmpc.size() > 0)
+                this->cusp().push_back(tmpc);
+        return;
+}
+
 BicycleCar *ParallelSlot::flnc(
         BicycleCar *B,
         std::vector<CircleObstacle>& co,
@@ -668,6 +721,48 @@ BicycleCar *ParallelSlot::getFPf()
         return new BicycleCar(x, y, h);
 }
 
+BicycleCar *ParallelSlot::getISPPf(BicycleCar *B)
+{
+        float x = this->slot().bnodes().front()->x();
+        float y = this->slot().bnodes().front()->y();
+        float y0;
+        if (this->slotSide() == LEFT)
+                y0 = B->ccl()->y();
+        else
+                y0 = 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 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;
+        float x0_1 = x0;
+        // left front
+        x = this->slot().bnodes().back()->x();
+        y = this->slot().bnodes().back()->y();
+        IR = BCAR_OUT_RADI;
+        a = 1;
+        b = -2 * x;
+        c = pow(x, 2) + pow(y - y0, 2) - pow(IR, 2);
+        D = pow(b, 2) - 4 * a * c;
+        if (this->slotSide() == LEFT)
+                x0 = -b + sqrt(D);
+        else
+                x0 = -b - sqrt(D);
+        x0 /= 2 * a;
+        float x0_2 = x0;
+        if (this->slotSide() == LEFT)
+                x0 = std::max(x0_1, x0_2);
+        else
+                x0 = std::min(x0_1, x0_2);
+        return new BicycleCar(x0, B->y(), B->h());
+}
+
 bool ParallelSlot::isInside(BicycleCar *c)
 {
         bool inside = true;