From 0e2d3e024348b7f4d1a6b980ffdb3f0a34a3a7a3 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Wed, 3 Apr 2019 09:13:24 +0200 Subject: [PATCH] Fix final pose getter --- decision_control/slotplanner.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc index ed1446d..46a0516 100644 --- 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 -- 2.39.2