From 457ed723b743f31b7d615a0b8b5c5ee339f31e1b Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 6 Mar 2019 10:02:16 +0100
Subject: [PATCH] Find entry point by reverse approach

See Vorobieva2015 for more info about reverse approach.
---
 CHANGELOG.md                    |  1 +
 base/main.cc                    |  5 +++
 decision_control/slotplanner.cc | 63 +++++++++++++++++++++++++++++++++
 incl/slotplanner.h              | 10 ++++++
 lpar.json                       |  1 +
 5 files changed, 80 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 2063540..0adb013 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Right parallel parking slot planner.
 - Shrinking length of parallel parking slot test.
 - SlotPlanner *Find Init Pose* `fip()` method based on BFS.
+- Reverse approach for finding "entry pose". See Vorobieva2015 for more info.
 
 ### Changed
 - Test results plot.
diff --git a/base/main.cc b/base/main.cc
index 85eb8a2..4499ac6 100644
--- a/base/main.cc
+++ b/base/main.cc
@@ -169,6 +169,11 @@ int main()
         }
         if (ps.slot().bnodes().size() > 0)
                 ps.fpose();
+                //ps.fipr(new BicycleCar(
+                //        p.goal()->x(),
+                //        p.goal()->y(),
+                //        p.goal()->h()
+                //));
         TEND();
         jvo["ppse"] = ELAPSED;
         TPRINT("ParallelSlot");
diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 682881d..db8278e 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -135,6 +135,29 @@ createcuspandfinish:
         std::swap(q, empty);
 }
 
+void ParallelSlot::fipr(BicycleCar *B)
+{
+        // TODO for right parallel parking also
+        // it's only for lpar scenario now
+
+        std::vector<RRTNode *> cusp;
+        cusp.push_back(new RRTNode(B->x(), B->y(), B->h()));
+        // just copied from fip()
+        this->DH(-0.01 / B->out_radi());
+        BicycleCar *c;
+        c = this->flncr(B);
+        while (c->lfx() < 0) {
+                cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
+                BicycleCar *cc = this->flncr(c);
+                cc->s(c->s() + 1);
+                delete c;
+                c = cc;
+        }
+        cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
+        std::reverse(cusp.begin(), cusp.end());
+        this->cusp().push_back(cusp);
+}
+
 BicycleCar *ParallelSlot::flnc(BicycleCar *B)
 {
         // TODO find last not colliding
@@ -179,6 +202,46 @@ BicycleCar *ParallelSlot::flnc(BicycleCar *B)
         return B->move(cc, (i - 1) * this->DH());
 }
 
+BicycleCar *ParallelSlot::flncr(BicycleCar *B)
+{
+        // TODO find last not colliding
+        // for now just copy flast()
+        RRTNode *cc;
+        if (int(B->s()) % 2 == 0)
+                cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
+        else
+                cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
+        BicycleCar *p;
+        int i = 1;
+        p = B->move(cc, i * this->DH());
+        while (!this->slot().collide(p->frame())
+                        && (
+                                (this->DH() > 0 && p->x() >= 0)
+                                || (this->DH() < 0 && p->lfx() <= 0)
+                        )) {
+                delete p;
+                i += 10;
+                p = B->move(cc, i * this->DH());
+        }
+        i -= 10;
+        p = B->move(cc, i * this->DH());
+        while (!this->slot().collide(p->frame())) {
+                if (this->DH() > 0 && p->x() <= 0) {
+                        i += 1;
+                        break;
+                }
+                if (this->DH() < 0 && p->lfx() >= 0) {
+                        i += 1;
+                        break;
+                }
+                delete p;
+                i += 1;
+                p = B->move(cc, i * this->DH());
+        }
+        delete p;
+        return B->move(cc, (i - 1) * this->DH());
+}
+
 RRTNode *ParallelSlot::fposecenter()
 {
         return this->slot().bnodes().front();
diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index aea9831..759a051 100644
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -60,6 +60,16 @@ class ParallelSlot {
                 // other
                 /** BFS to _Find Init Pose_. */
                 void fip();
+                /** _Find Init Pose by Reverse_ approach, see Vorobieva2015
+
+                @param B Last pose of vehicle when it is parked.
+                */
+                void fipr(BicycleCar *B);
+                /** _Find Last Not Colliding for Reverse_ BicycleCar pose
+
+                @param B Find from?
+                */
+                BicycleCar *flncr(BicycleCar *B);
                 /** _Find Last Not Colliding_ BicycleCar pose
 
                 @param B Find from?
diff --git a/lpar.json b/lpar.json
index b2cea9e..2750a23 100644
--- a/lpar.json
+++ b/lpar.json
@@ -1,6 +1,7 @@
 {
         "init": [1.1, 6.5, 1.5707963267948966],
         "goal": [-1.1, 14.055, 1.5707963267948966],
+        "rgoal": [-0.8225, 13.665, 1.5707963267948966],
         "slot": {
                 "polygon": [
                         [0, 17.56],
-- 
2.39.2