Update is inside slot check

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 5e719a0e4871026102e5328ee74f2b6c70e5cd6d..6b378cb883dedcde3d03b546330587529a69aa50 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -152,9 +152,7 @@ void ParallelSlot::fip()
         while (!q.empty()) {
                 c = q.front();
                 q.pop();
-                if (this->DH() > 0 && c->rfx() <= 0 && c->rrx() <= 0) {
-                        goto createcuspandfinish;
-                } else if (this->DH() < 0 && c->lfx() >= 0 && c->lrx() >= 0) {
+                if (this->isInside(c)) {
                         goto createcuspandfinish;
                 } else if (c->s() < 9) {
                         BicycleCar *cc = this->flnc(c);
@@ -389,6 +387,29 @@ void ParallelSlot::fpose()
         }
 }
 
+bool ParallelSlot::isInside(BicycleCar *c)
+{
+        bool inside = true;
+        RRTNode *tmpn;
+        tmpn = new RRTNode(c->lfx(), c->lfy(), 0);
+        if (!this->slot().collide(tmpn))
+                inside = false;
+        delete tmpn;
+        tmpn = new RRTNode(c->lrx(), c->lry(), 0);
+        if (!this->slot().collide(tmpn))
+                inside = false;
+        delete tmpn;
+        tmpn = new RRTNode(c->rrx(), c->rry(), 0);
+        if (!this->slot().collide(tmpn))
+                inside = false;
+        delete tmpn;
+        tmpn = new RRTNode(c->rfx(), c->rfy(), 0);
+        if (!this->slot().collide(tmpn))
+                inside = false;
+        delete tmpn;
+        return inside;
+}
+
 struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 8ff643128a0df1465235ce040e4a41d9ee1f744f..8bc80ee11cb46b8fbe6fc878247858fe11b731cc 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -93,6 +93,8 @@ class ParallelSlot {
                 );
                 /** Test possible init poses */
                 void fpose();
+                /** Return true if car is inside slot */
+                bool isInside(BicycleCar *c);
                 /** Return values to set sampling function of RRT */
                 struct SamplingInfo getSamplingInfo();
 };