Add init pose for forward perpendicular parking

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 22669b1e94246f7e08ad4a31e9171599f95771e2..1c32eb0dc8a88186e1a61eab9fda94eaaa32703e 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -640,6 +640,34 @@ BicycleCar *ParallelSlot::getISPP(BicycleCar *B)
         return new BicycleCar(x0, B->y(), B->h());
 }
 
+BicycleCar *ParallelSlot::getFPf()
+{
+        this->setAll();
+        float x = this->slot().bnodes().front()->x();
+        float y = this->slot().bnodes().front()->y();
+        float h = this->slotHeading();
+        float nx;
+        float ny;
+        if (this->slotSide() == LEFT) {
+                h += M_PI / 2;
+                nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2
+                        * cos(h);
+                ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2
+                        * sin(h);
+                x = nx + (BCAR_DIAG_RRADI) * cos(h - M_PI / 2);
+                y = ny + (BCAR_DIAG_RRADI) * sin(h - M_PI / 2);
+        } else {
+                h -= M_PI / 2;
+                nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2
+                        * cos(h);
+                ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2
+                        * sin(h);
+                x = nx + (BCAR_DIAG_RRADI) * cos(h + M_PI / 2);
+                y = ny + (BCAR_DIAG_RRADI) * sin(h + M_PI / 2);
+        }
+        return new BicycleCar(x, y, h);
+}
+
 bool ParallelSlot::isInside(BicycleCar *c)
 {
         bool inside = true;

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 2704519f08473d7863e002f77bfe973d24888b07..ef539cd314b7a0d05e19a63786561a5a26e4951a 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -111,6 +111,7 @@ class ParallelSlot {
                 BicycleCar *getEPC();
                 /** Return start pose for fipr method */
                 BicycleCar *getFP();
+                BicycleCar *getFPf(); // parked forward, only for perpendicular
                 /** In slot perpendicular pose getter
 
                 This method returns a pose of perpendicular parking slot from