Use `poseHeading()` in `getFP()` in `SlotPlanner`

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 7482dbe86119e1b04e2ead8deae1dcd163a70671..5026bbe5f9e3eabfa8dd4febd521379fd5059208 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -708,26 +708,26 @@ BicycleCar *ParallelSlot::getFPf()
         float x = this->slot().bnodes().front()->x();
         float y = this->slot().bnodes().front()->y();
         float h = this->slotHeading();
+        float ph = this->poseHeading();
+        ph += M_PI;
+        while (ph > M_PI)
+                ph -= 2 * M_PI;
+        while (ph <= -M_PI)
+                ph += 2 * M_PI;
         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);
+                nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * cos(ph);
+                ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * sin(ph);
+                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);
-                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);
+                nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * cos(ph);
+                ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * sin(ph);
+                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::getISPPf(BicycleCar *B)