Include obstacles to slot planner

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index f945bbcb1b93590b880e3eb7bbdad1cdc9270eff..f871d0f4b7042e848056e5199a5983da0c72be90 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -96,7 +96,10 @@ void ParallelSlot::setAll()
 }
 
 // other
-void ParallelSlot::fip()
+void ParallelSlot::fip(
+        std::vector<CircleObstacle>& co,
+        std::vector<SegmentObstacle>& so
+)
 {
         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
         // RRTNode.s() works as iteration level
@@ -112,7 +115,18 @@ void ParallelSlot::fip()
         int i = 0;
         c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
         while (!this->slot().collide(c->frame())) {
-                q.push(c);
+                bool end = false;
+                std::vector<RRTEdge *> eds = c->frame();
+                for (auto o: co)
+                        if (o.collide(eds))
+                                end = true;
+                for (auto o: so)
+                        if (o.collide(eds))
+                                end = true;
+                for (auto e: eds)
+                        delete e;
+                if (!end)
+                        q.push(c);
                 c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
                 i += 1;
         }
@@ -123,7 +137,7 @@ void ParallelSlot::fip()
                 if (this->isInside(c)) {
                         goto createcuspandfinish;
                 } else if (c->s() < 9) {
-                        BicycleCar *cc = this->flnc(c);
+                        BicycleCar *cc = this->flnc(c, co, so);
                         cc->s(c->s() + 1);
                         cc->bcparent(c);
                         q.push(cc);
@@ -213,7 +227,11 @@ void ParallelSlot::fipr(BicycleCar *B)
         this->cusp().push_back(cusp);
 }
 
-BicycleCar *ParallelSlot::flnc(BicycleCar *B)
+BicycleCar *ParallelSlot::flnc(
+        BicycleCar *B,
+        std::vector<CircleObstacle>& co,
+        std::vector<SegmentObstacle>& so
+)
 {
         RRTNode *cc;
         if (this->slotSide() == LEFT) {
@@ -237,6 +255,18 @@ BicycleCar *ParallelSlot::flnc(BicycleCar *B)
                 delete p;
                 i += 10;
                 p = B->move(cc, i * this->DH());
+                bool end = false;
+                std::vector<RRTEdge *> eds = p->frame();
+                for (auto o: co)
+                        if (o.collide(eds))
+                                end = true;
+                for (auto o: so)
+                        if (o.collide(eds))
+                                end = true;
+                for (auto e: eds)
+                        delete e;
+                if (end)
+                        break;
         }
         i -= 10;
         p = B->move(cc, i * this->DH());
@@ -251,6 +281,18 @@ BicycleCar *ParallelSlot::flnc(BicycleCar *B)
                 delete p;
                 i += 1;
                 p = B->move(cc, i * this->DH());
+                bool end = false;
+                std::vector<RRTEdge *> eds = p->frame();
+                for (auto o: co)
+                        if (o.collide(eds))
+                                end = true;
+                for (auto o: so)
+                        if (o.collide(eds))
+                                end = true;
+                for (auto e: eds)
+                        delete e;
+                if (end)
+                        break;
         }
         delete p;
         return B->move(cc, (i - 1) * this->DH());

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 11dc5f1b940147f95e26ddb5df642e5e446ac77f..d7c0395e601e09a9fa87ee44b77419609df37157 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -69,7 +69,10 @@ class ParallelSlot {
 
                 // other
                 /** BFS to _Find Init Pose_. */
-                void fip();
+                void fip(
+                        std::vector<CircleObstacle>& co,
+                        std::vector<SegmentObstacle>& so
+                );
                 /** _Find Init Pose by Reverse_ approach, see Vorobieva2015
 
                 @param B Last pose of vehicle when it is parked.
@@ -85,7 +88,11 @@ class ParallelSlot {
 
                 @param B Find from?
                 */
-                BicycleCar *flnc(BicycleCar *B);
+                BicycleCar *flnc(
+                        BicycleCar *B,
+                        std::vector<CircleObstacle>& co,
+                        std::vector<SegmentObstacle>& so
+                );
                 RRTNode *fposecenter();
                 /** Recursive function to find out the moves */
                 bool flast(