Add Extend* procedure

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 421350458790000a22378b7febb1eca01264e44b..72b682568032dfd9d7d6e00176c41540280dc071 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -755,9 +755,70 @@ bool Klemm2015::next()
         return this->goal_found();
 }
 
-int Klemm2015::extendstar()
+int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
 {
-        return 0;
+        int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
+        char tree = this->root()->tree();
+#if NNVERSION>2
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost, tree);
+#elif NNVERSION>1
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost);
+#else
+        RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
+#endif
+        std::vector<RRTNode *> nvs;
+        std::vector<RRTNode *> steered = this->steer(nn, rs);
+        RRTNode *ns = steered[1];
+        ns->tree(tree);
+#if NVVERSION>2
+       nvs = this->nv(
+               this->iy_,
+               ns,
+               this->cost,
+               MIN(
+                       GAMMA_RRTSTAR(
+                               this->nodes().size()),
+                       0.2),
+               '0'); // TODO const
+#elif NVVERSION>1
+       nvs = this->nv(
+               this->iy_,
+               ns,
+               this->cost,
+               MIN(
+                       GAMMA_RRTSTAR(
+                               this->nodes().size()),
+                       0.2)); // TODO const
+#else
+       nvs = this->nv(
+               this->root(),
+               ns,
+               this->cost,
+               MIN(
+                       GAMMA_RRTSTAR(
+                               this->nodes().size()),
+                       0.2)); // TODO const
+#endif
+       this->nodes().push_back(ns);
+       this->add_iy(ns);
+       // connect
+       if (!this->connect(nn, ns, nvs)) {
+               this->iy_[IYI(ns->y())].pop_back();
+                ret = 2;
+       } else {
+                // rewire
+                this->rewire(nvs, ns);
+                if (this->goal_found(ns, this->cost)) {
+                        this->tlog(this->findt());
+                        ret = 1;
+                }
+       }
+        for (auto n: steered) {
+                if (n != steered[1])
+                        delete n;
+        }
+        *xn = ns;
+        return ret;
 }
 
 int Klemm2015::connectstar(RRTNode *x)

diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index 1b49cedfb3b0b15466272ad35ff4ff029a98866b..fd7ee5b67d7db2dc862c4e427506e1d70030f6a3 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -184,7 +184,7 @@ class Klemm2015: public Karaman2011 {
                 RRTNode *orig_root_ = nullptr;
                 RRTNode *orig_goal_ = nullptr;
         protected:
-                int extendstar();
+                int extendstar1(RRTNode *rs, RRTNode **xn);
                 int connectstar(RRTNode *x);
                 void swap();
         public: