Log only if trajectory cost is better

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

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 98d5d4b29a0d572c20839541519ed20665fda660..1047fb8a077e64c62134b07dd91bb1c9b0ec4460 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -629,6 +629,7 @@ bool T2::opt_path()
 {
         if (this->tlog().size() == 0)
                 return false;
+        float oc = this->goal()->ccost();
         std::vector<RRTNode *> tmp_cusps;
         for (auto n: this->tlog().back()) {
                 if (n->parent() && sgn(n->s()) != sgn(n->parent()->s())) {
@@ -701,7 +702,9 @@ bool T2::opt_path()
         }
         pn->add_child(this->goal(), this->cost(pn, this->goal()));
         // End of Dijkstra
-        return true;
+        if (this->goal()->ccost() < oc)
+                return true;
+        return false;
 }
 
 bool T2::opt_part(RRTNode *init, RRTNode *goal)