From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 11 Mar 2019 14:30:09 +0000 (+0100)
Subject: Add and use entry point getters in fip
X-Git-Tag: v0.6.0~13^2~5
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/716a31076938cbe2c510b1126bbcfb09023d0b83

Add and use entry point getters in fip
---

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index b878eed..99cb1f1 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -101,33 +101,10 @@ void ParallelSlot::fip()
         std::queue<BicycleCar *, std::list<BicycleCar *>> q;
         std::queue<BicycleCar *, std::list<BicycleCar *>> empty;
         int di = -1;
-        // new pose for parallel parking to right slot
-        float tnx;
-        float tny;
-        float nx;
-        float ny;
-        // temporary tnx is angle
-        tnx = this->slotHeading() + M_PI;
-        if (this->slotSide() == RIGHT)
-                tnx -= M_PI / 4;
-        else
-                tnx += M_PI / 4;
-        nx = this->fposecenter()->x() + 0.01 * cos(tnx);
-        ny = this->fposecenter()->y() + 0.01 * sin(tnx);
-        BicycleCar *CC = new BicycleCar(nx, ny, this->slotHeading());
-        // move left by car width / 2
-        tnx = CC->x() + CC->width() / 2 * cos(CC->h() + M_PI / 2);
-        tny = CC->y() + CC->width() / 2 * sin(CC->h() + M_PI / 2);
-        if (this->slotSide() == LEFT) {
+        if (this->slotSide() == LEFT)
                 di = 1;
-                // move right by car width / 2
-                tnx = CC->x() + CC->width() / 2 * cos(CC->h() - M_PI / 2);
-                tny = CC->y() + CC->width() / 2 * sin(CC->h() - M_PI / 2);
-        }
-        // move down
-        nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
-        ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
-        BicycleCar *B = new BicycleCar(nx, ny, CC->h());
+        BicycleCar *CC = this->getEPC();
+        BicycleCar *B = this->getEP();
         this->DH(di * 0.01 / CC->out_radi());
         BicycleCar *c;
         int i = 0;
@@ -413,6 +390,44 @@ void ParallelSlot::fpose()
         }
 }
 
+BicycleCar *ParallelSlot::getEP()
+{
+        // new pose for parallel parking to right slot
+        float tnx;
+        float tny;
+        float nx;
+        float ny;
+        BicycleCar *CC = this->getEPC();
+        // move left by car width / 2
+        tnx = CC->x() + CC->width() / 2 * cos(CC->h() + M_PI / 2);
+        tny = CC->y() + CC->width() / 2 * sin(CC->h() + M_PI / 2);
+        if (this->slotSide() == LEFT) {
+                // move right by car width / 2
+                tnx = CC->x() + CC->width() / 2 * cos(CC->h() - M_PI / 2);
+                tny = CC->y() + CC->width() / 2 * sin(CC->h() - M_PI / 2);
+        }
+        // move down
+        nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
+        ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
+        return new BicycleCar(nx, ny, CC->h());
+}
+
+BicycleCar *ParallelSlot::getEPC()
+{
+        // new pose for parallel parking to right slot
+        float ta;
+        float nx;
+        float ny;
+        ta = this->slotHeading() + M_PI;
+        if (this->slotSide() == RIGHT)
+                ta -= M_PI / 4;
+        else
+                ta += M_PI / 4;
+        nx = this->fposecenter()->x() + 0.01 * cos(ta);
+        ny = this->fposecenter()->y() + 0.01 * sin(ta);
+        return new BicycleCar(nx, ny, this->slotHeading());
+}
+
 BicycleCar *ParallelSlot::getFP()
 {
         float x = this->slot().bnodes()[3]->x();
diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 0be1845..d4f7ba0 100644
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -93,6 +93,10 @@ class ParallelSlot {
                 );
                 /** Test possible init poses */
                 void fpose();
+                /** Return start pose for fip method */
+                BicycleCar *getEP();
+                /** Return center of rotation for start pose for fip method */
+                BicycleCar *getEPC();
                 /** Return start pose for fipr method */
                 BicycleCar *getFP();
                 /** Return true if car is inside slot */