From da7f14b31324a5f5ffb8ef69a993b9138868fa85 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Sun, 10 Mar 2019 17:51:38 +0100
Subject: [PATCH] Update is inside slot check

---
 decision_control/slotplanner.cc | 27 ++++++++++++++++++++++++---
 incl/slotplanner.h              |  2 ++
 2 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 5e719a0..6b378cb 100644
--- 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 8ff6431..8bc80ee 100644
--- 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();
 };
-- 
2.39.2