Limit node distance, #cusps for RRT* grow in T2

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 55bcd6501e3cd4aab282236847cbe091c51944a3..c51c4e3a083bd9d3aa77bf83be5118ba76046947 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -455,10 +455,17 @@ bool T2::next()
         std::vector<RRTNode *> nvs;
         std::vector<RRTNode *> newly_added;
         bool en_add = true;
+        int cusps = 0;
         for (auto ns: this->steer(nn, rs)) {
                 if (!en_add) {
                         delete ns;
+                } else if (IS_NEAR(pn, ns)) {
+                        delete ns;
+                } else if (cusps > 0) {
+                        en_add = false;
                 } else {
+                        if (sgn(pn->s()) != sgn(ns->s()))
+                                cusps++;
 #if NVVERSION>1
                         nvs = this->nv(
                                         this->iy_,
@@ -501,10 +508,17 @@ bool T2::next()
         for (auto na: newly_added) {
                 pn = na;
                 en_add = true;
+                cusps = 0;
                 for (auto ns: this->steer(na, this->goal())) {
                         if (!en_add) {
                                 delete ns;
+                        } else if (IS_NEAR(pn, ns)) {
+                                delete ns;
+                        } else if (cusps > 0) {
+                                en_add = false;
                         } else {
+                                if (sgn(pn->s()) != sgn(ns->s()))
+                                        cusps++;
                                 this->nodes().push_back(ns);
                                 this->add_iy(ns);
                                 pn->add_child(ns, this->cost(pn, ns));