Add find init pose in forward direction method

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index de4b4d5390ea57e882d5b42e590778851d8fa519..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,

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index d9a98edf2eb9f8258eac134ad06944c1008601b7..9a845d0fbb17f1646b675aea9f6d6445e7a77f19 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -75,6 +75,10 @@ class ParallelSlot {
                         std::vector<CircleObstacle>& co,
                         std::vector<SegmentObstacle>& so
                 );
+                void fipf(
+                        std::vector<CircleObstacle>& co,
+                        std::vector<SegmentObstacle>& so
+                ); // perpendicular forward parking
                 /** _Find Last Not Colliding_ BicycleCar pose
 
                 @param B Find from?