Find entry point by reverse approach

See Vorobieva2015 for more info about reverse approach.

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 20635407e9eb89cb589fb573096546702dd91d0d..0adb01341f5350e51b472d77211f6ea7ca467118 100644 (file)
--- 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 85eb8a276ac4077412e3a7c053df834a4cdec31e..4499ac688d35f5ea17260de57fdd1c2fe2fb50d8 100644 (file)
--- 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 682881d6764fc9284a231bdb3ce248b2718cacc7..db8278ea3b02feb2e8fbdbc2ef1d027a3d47662a 100644 (file)
--- 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 aea98311800cf7fcbc7390db60e06305ea1f01d2..759a051e6705e83144db211be47c642d8b95966c 100644 (file)
--- 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 b2cea9ee263b94c9b912cefcbc5ad143a5139011..2750a239cc5ca63d84704bf25608e2ddcc0758ca 100644 (file)
--- 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],