From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 5 Dec 2018 12:39:48 +0000 (+0100)
Subject: Add RRT*-Connect main loop
X-Git-Tag: v0.4.0~16^2~8
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/53d0ef4087ac2e3d744bc7be7045df4313290144

Add RRT*-Connect main loop
---

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 94acac2..73c84a5 100644
--- 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();
 }