Fix final pose getter

[hubacji1/iamcar.git] / decision_control / slotplanner.cc

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index ed1446de490c20a83eab26a9392b45d6ebe59f76..46a05166d90b5437c28bd05fed2518de68d604e3 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -470,8 +470,8 @@ BicycleCar *ParallelSlot::getFP()
                                 * cos(h + M_PI);
                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
                                 * sin(h + M_PI);
-                        x = nx + (BCAR_WIDTH + 0.01) * cos(h + M_PI / 2);
-                        y = ny + (BCAR_WIDTH + 0.01) * sin(h + M_PI / 2);
+                        x = nx + (BCAR_WIDTH / 2 + 0.01) * cos(h + M_PI / 2);
+                        y = ny + (BCAR_WIDTH / 2 + 0.01) * sin(h + M_PI / 2);
                 } else {
                         h += M_PI / 2;
                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2