From f55e2af91933f97e5078c155f71a2d92e188b484 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 27 May 2019 16:13:15 +0200
Subject: [PATCH] Add find init pose in forward direction method

---
 decision_control/slotplanner.cc | 53 +++++++++++++++++++++++++++++++++
 incl/slotplanner.h              |  4 +++
 2 files changed, 57 insertions(+)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index de4b4d5..e4a61eb 100644
--- 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 d9a98ed..9a845d0 100644
--- 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?
-- 
2.39.2