Add and use entry point getters in fip

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index b878eedbc1182a450cdc6e5ef300feba608cdb64..99cb1f136011394ec5627c673ee36f419d6bad55 100644 (file)
--- 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 0be18453da67e660b9111e9c9d3f731626856dda..d4f7ba06d6c34d328c29535ed2e885c94367372d 100644 (file)
--- 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 */