Fix final pose for backward angular parking

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index df09cfb0c2e5322bccc4b9b058e03ea3f63b4674..f888d401618d8aaa90fc451ccf44a9203bb15ea7 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -645,6 +645,7 @@ BicycleCar *ParallelSlot::getFP()
         float x = this->slot().bnodes()[0]->x();
         float y = this->slot().bnodes()[0]->y();
         float h = this->slotHeading();
+        float ph = this->poseHeading();
         float nx;
         float ny;
         if (this->slotType() == PARALLEL) {
@@ -659,24 +660,22 @@ BicycleCar *ParallelSlot::getFP()
                 y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
         } else {
                 if (this->slotSide() == LEFT) {
-                        h -= M_PI / 2;
                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
-                                * cos(h + M_PI);
+                                * cos(ph + M_PI);
                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
-                                * sin(h + M_PI);
-                        x = nx + (BCAR_DIAG_RRADI) * cos(h + M_PI / 2);
-                        y = ny + (BCAR_DIAG_RRADI) * sin(h + M_PI / 2);
+                                * sin(ph + M_PI);
+                        x = nx + (BCAR_DIAG_RRADI) * cos(h);
+                        y = ny + (BCAR_DIAG_RRADI) * sin(h);
                 } else {
-                        h += M_PI / 2;
                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
-                                * cos(h - M_PI);
+                                * cos(ph + M_PI);
                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
-                                * sin(h - M_PI);
-                        x = nx + (BCAR_DIAG_RRADI) * cos(h - M_PI / 2);
-                        y = ny + (BCAR_DIAG_RRADI) * sin(h - M_PI / 2);
+                                * sin(ph + M_PI);
+                        x = nx + (BCAR_DIAG_RRADI) * cos(h);
+                        y = ny + (BCAR_DIAG_RRADI) * sin(h);
                 }
         }
-        return new BicycleCar(x, y, h);
+        return new BicycleCar(x, y, ph);
 }
 
 BicycleCar *ParallelSlot::getISPP(BicycleCar *B)