From 413bfc4ab4d6f6561ff213177e7af161fdc88bfb Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 27 May 2019 16:42:30 +0200 Subject: [PATCH 1/1] Use `poseHeading()` in `getFP()` in `SlotPlanner` --- decision_control/slotplanner.cc | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) 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) -- 2.39.2