Merge branch 'release/0.7.0'

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

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index d37a8913fe682d0c2fe38726de96a9b369e806db..2ad47445a716baa5dcfa38fcb8697499329ea16e 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -142,7 +142,17 @@ void ParallelSlot::fip(
                         !this->slot().collide(p->frame())
                         && ((
                                 this->slotSide() == LEFT
-                                && p->h() < this->slotHeading()
+                                && (
+                                        p->h() < this->slotHeading()
+                                        || (
+                                                sgn(p->h()) != sgn(
+                                                        this->slotHeading()
+                                                )
+                                                && p->h()
+                                                < this->slotHeading()
+                                                + 2 * M_PI
+                                        )
+                                )
                         ) || (
                                 this->slotSide() == RIGHT
                                 && (
@@ -827,21 +837,34 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
         RRTNode *n = this->getMidd();
+        if (n == nullptr)
+                return si;
+        BicycleCar *bc = new BicycleCar(n->x(), n->y(), n->h());
+        RRTNode *cc;
+        if (this->slotSide() == LEFT)
+                cc = bc->ccl();
+        else
+                cc = bc->ccr();
         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;
 }