From 241d52dcd1756631c9c61368071c7a064ea7315b Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Wed, 16 Jan 2019 16:34:14 +0100 Subject: [PATCH] Update T2 with adding to `ixy_` data structure --- base/rrtbase.cc | 3 +++ decision_control/rrtplanner.cc | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/base/rrtbase.cc b/base/rrtbase.cc index 59ef374..0cabc23 100644 --- 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 c3b45bb..d5667fe 100644 --- 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)) { -- 2.39.2