From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Mon, 24 Sep 2018 13:35:52 +0000 (+0200)
Subject: Limit node distance, #cusps for RRT* grow in T2
X-Git-Tag: v0.2.0~20^2~2
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/81ad58627c918ba5583891a516c70e9eb40007de

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

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 55bcd65..c51c4e3 100644
--- 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));