Revert "Prevent running `findt()` twice"

This reverts commit 360ad9eb7f9b2fc8261f7fafa464c3ae0e5ccda5.

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index d5667fe06c45f50fefb564120553f69fcf910faa..0267e71de4298e9517492f72bc81ee1b4ccbb73f 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -449,6 +449,7 @@ bool T2::next()
                                 newly_added.push_back(pn);
                                 if (this->goal_found(pn, CO)) {
                                         this->goal_cost();
+                                        this->tlog(this->findt());
                                         en_add = false;
                                 }
                         }
@@ -488,6 +489,7 @@ bool T2::next()
                                 newly_added.push_back(pn);
                                 if (this->goal_found(pn, CO)) {
                                         this->goal_cost();
+                                        this->tlog(this->findt());
                                         en_add = false;
                                 }
                         }
@@ -525,6 +527,7 @@ bool T2::next()
                                         pn = ns;
                                         if (this->goal_found(pn, CO)) {
                                                 this->goal_cost();
+                                                this->tlog(this->findt());
                                                 en_add = false;
                                         }
                                 }
@@ -558,6 +561,7 @@ bool T2::next()
                                         pn = ns;
                                         if (this->goal_found(pn, CO)) {
                                                 this->goal_cost();
+                                                this->tlog(this->findt());
                                                 en_add = false;
                                         }
                                 }