From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 27 May 2019 14:42:30 +0000 (+0200)
Subject: Use `poseHeading()` in `getFP()` in `SlotPlanner`
X-Git-Tag: v0.7.0~18^2~8
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/413bfc4ab4d6f6561ff213177e7af161fdc88bfb

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

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 7482dbe..5026bbe 100644
--- 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)