Remove unused vector, use `npi` for parent node

[hubacji1/iamcar.git] / decision_control / rrtplanner.cc

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 744a662484eac47636312de41fec58732ad3aaba..2b93dbcde0ce6d2132a767b983e03687bad80bdf 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -696,9 +696,8 @@ bool T2::opt_path()
                 tmpi = dnodes[tmpi].pi;
         }
         npi.push_back(tmpi);
-        std::vector<RRTNode *> npn; // new path nodes
         std::reverse(npi.begin(), npi.end());
-        RRTNode *pn = cusps[0];
+        RRTNode *pn = cusps[npi[0]];
         for (unsigned int i = 0; i < npi.size() - 1; i++) {
                 for (auto ns: this->steer(cusps[npi[i]], cusps[npi[i + 1]])) {
                         pn->add_child(ns, this->cost(pn, ns));