Add RRT*-Connect main loop

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 94acac2f1373eade7c83424a32cdaa83db0fcd37..73c84a55315659c1a2891417a37426f9f56736a9 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -720,6 +720,30 @@ Klemm2015::Klemm2015(RRTNode *init, RRTNode *goal):
 
 bool Klemm2015::next()
 {
+        RRTNode *xn = nullptr;
+        RRTNode *rs;
+#if GOALFIRST > 0
+        if (this->samples().size() == 0)
+                rs = this->goal();
+        else
+                rs = this->sample();
+#else
+        rs = this->sample();
+#endif
+        this->samples().push_back(rs);
+        //std::cerr << "next" << std::endl;
+        if (this->extendstar1(rs, &xn) != 2) {
+        //        if (xn) {
+        //                std::cerr << "- xn: " << xn->x() << ", " << xn->y();
+        //                std::cerr << std::endl;
+        //        } else {
+        //                std::cerr << "- xn: nullptr" << std::endl;
+        //        }
+                this->swap();
+                this->connectstar(xn);
+        } else {
+                this->swap();
+        }
         return this->goal_found();
 }