Add FP as first node in slot cusps

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 7a9eb7482f60a83a5014ca82c6657d15125d4469..d37a8913fe682d0c2fe38726de96a9b369e806db 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -151,6 +151,10 @@ void ParallelSlot::fip(
                                 )
                         ))
                 ) {
+                        if (tmpc.size() == 0) {
+                                i = 0;
+                                p = this->getFP();
+                        }
                         bool end = false;
                         std::vector<RRTEdge *> eds = p->frame();
                         for (auto o: co)
@@ -260,6 +264,10 @@ void ParallelSlot::fipf(
                         && p->h() < this->slotHeading()
                 ))
         ) {
+                if (tmpc.size() == 0) {
+                        i = 0;
+                        p = this->getFPf();
+                }
                 bool end = false;
                 std::vector<RRTEdge *> eds = p->frame();
                 for (auto o: co)