Use firsts FIFO queue in T2 planner

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index b0cb089f75c201c194c18c6521ac02ec3ee5c9de..49080d18a2dbe2ddbc5731814de2bb14a640f19a 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -400,14 +400,12 @@ bool T1::next()
 bool T2::next()
 {
         RRTNode *rs;
-#if GOALFIRST > 0
-        if (this->samples().size() == 0)
-                rs = this->goal();
-        else
+        if (this->firsts().size() > 0) {
+                rs = this->firsts().front();
+                this->firsts().pop();
+        } else {
                 rs = this->sample();
-#else
-        rs = this->sample();
-#endif
+        }
         this->samples().push_back(rs);
         RRTNode *nn = this->nn(rs);
         if (!nn)