Update RRT sampling with right side parking

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

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 99cb1f136011394ec5627c673ee36f419d6bad55..3516726ace5bdc55c86c16b4c593f63a3a5b2eb7 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -473,14 +473,20 @@ bool ParallelSlot::isInside(BicycleCar *c)
 struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
-        BicycleCar *CC = new BicycleCar(
-                this->fposecenter()->x(),
-                this->fposecenter()->y() - 0.01,
-                this->slotHeading()
-        );
+        BicycleCar *CC = this->getEPC();
         si.x = this->slot().bnodes()[0]->x();
         si.y = this->slot().bnodes()[0]->y();
+        if (this->slotSide() == RIGHT) {
+                si.dx = 1;
+                si.dy = 1;
+                si.dh = 1;
+        } else {
+                si.dx = -1;
+                si.dy = -1;
+                si.dh = -1;
+        }
         si.r = CC->diag_radi();
+        si.sh = this->slotHeading();
         si.h = this->slotHeading() - acos(EDIST( // TODO generalize
                 this->slot().bnodes()[0],
                 this->slot().bnodes()[1]