Update T2 with adding to `ixy_` data structure

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 59ef374efbb9698a490992e4d008380b84ef3111..0cabc238b81ab76b4e16ac2791e8749c21f27513 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -90,6 +90,7 @@ RRTBase::RRTBase():
         this->nodes_.reserve(20000);
         this->nodes_.push_back(this->root_);
         this->add_iy(this->root_);
+        this->add_ixy(this->root_);
 }
 
 RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
@@ -99,6 +100,7 @@ RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
         this->nodes_.reserve(20000);
         this->nodes_.push_back(init);
         this->add_iy(init);
+        this->add_ixy(init);
 }
 
 // getter
@@ -768,6 +770,7 @@ bool RRTBase::opt_path()
                         } else {
                                 this->nodes().push_back(ns);
                                 this->add_iy(ns);
+                                this->add_ixy(ns);
                                 pn->add_child(ns, this->cost(pn, ns));
                                 pn = ns;
                         }

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index c3b45bbba576784a0c278f47184062ec19c88df1..d5667fe06c45f50fefb564120553f69fcf910faa 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -442,6 +442,7 @@ bool T2::next()
                                 delete ns;
                                 en_add = false;
                         } else {
+                                this->add_ixy(ns);
                                 // rewire
                                 this->rewire(nvs, ns);
                                 pn = ns;
@@ -480,6 +481,7 @@ bool T2::next()
                                 delete ns;
                                 en_add = false;
                         } else {
+                                this->add_ixy(ns);
                                 // rewire
                                 this->rewire(nvs, ns);
                                 pn = ns;
@@ -518,6 +520,7 @@ bool T2::next()
                                         delete ns;
                                         en_add = false;
                                 } else {
+                                        this->add_ixy(ns);
                                         this->ocost(ns);
                                         pn = ns;
                                         if (this->goal_found(pn, CO)) {
@@ -550,6 +553,7 @@ bool T2::next()
                                         delete ns;
                                         en_add = false;
                                 } else {
+                                        this->add_ixy(ns);
                                         this->ocost(ns);
                                         pn = ns;
                                         if (this->goal_found(pn, CO)) {