Change sampling info

Sampling info based on init pose found by `fip()`.

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index d37a8913fe682d0c2fe38726de96a9b369e806db..8ac19e8e33bbde5ae265bcaec4991d72a215bc54 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -827,21 +827,34 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
         RRTNode *n = this->getMidd();
+        BicycleCar *bc = new BicycleCar(n->x(), n->y(), n->h());
+        RRTNode *cc;
+        if (this->slotSide() == LEFT)
+                cc = bc->ccl();
+        else
+                cc = bc->ccr();
+        if (n == nullptr)
+                return si;
         if (this->slotType() == PARALLEL) {
-                if (n != nullptr) {
-                        si.x0 = n->x() + 1.5 * BCAR_LENGTH * cos(n->h());
-                        si.y0 = n->y() + 1.5 * BCAR_LENGTH * sin(n->h());
-                        si.h0 = n->h();
-                } else {
-                        si.x0 = this->slot().bnodes().front()->x();
-                        si.y0 = this->slot().bnodes().front()->y();
-                        si.h0 = this->slotHeading();
-                }
+                BicycleCar *nbc = bc->move(cc, this->slotHeading() - n->h());
+                si.x0 = nbc->x();
+                si.y0 = nbc->y();
+                si.h0 = nbc->h();
                 si.x = BCAR_WIDTH;
                 si.y = BCAR_WIDTH;
                 si.h = M_PI / 8;
         } else {
-                // TODO
+                BicycleCar *nbc;
+                if (this->slotSide() == LEFT)
+                        nbc = bc->move(cc, M_PI/4);
+                else
+                        nbc = bc->move(cc, -M_PI/4);
+                si.x0 = nbc->x();
+                si.y0 = nbc->y();
+                si.h0 = nbc->h();
+                si.x = BCAR_WIDTH;
+                si.y = BCAR_WIDTH;
+                si.h = M_PI / 8;
         }
         return si;
 }