Use different sampling info for USE_SLOTPLANNER

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 411af2ecba74ae68ad130c5b36059ece4b0bd895..93ea4c7dfde109722e3ce93798d3cace9bdcecb8 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -541,6 +541,7 @@ bool ParallelSlot::isInside(BicycleCar *c)
 struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
+#ifdef USE_SLOTPLANNER
         BicycleCar *CC = this->getEPC();
         si.x = this->slot().bnodes()[0]->x();
         si.y = this->slot().bnodes()[0]->y();
@@ -553,7 +554,6 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
                 si.dy = -1;
                 si.dh = -1;
         }
-        si.r = CC->diag_radi();
         si.sh = this->slotHeading();
         if (this->slotType() == PARALLEL) {
                 si.h = this->slotHeading() - acos(EDIST(
@@ -563,5 +563,26 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
         } else {
                 si.h = M_PI /2;
         }
+#else
+        si.x = this->slot().bnodes()[3]->x() - this->slot().bnodes()[0]->x();
+        si.x /= 2;
+        si.x += this->slot().bnodes()[0]->x();
+        si.y = this->slot().bnodes()[3]->y() - this->slot().bnodes()[0]->y();
+        si.y /= 2;
+        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 = EDIST(this->slot().bnodes()[0], this->slot().bnodes()[3]) / 2;
+        si.r *= 2;
+        si.sh = this->slotHeading();
+        si.h = M_PI / 2;
+#endif
         return si;
 }