Run path optimization in each iteration

diff --git a/base/main.cc b/base/main.cc
index b0e559368ee5ca9975db9d69e617b6af5d53e049..42975aee82df71f54a8cf35a92967db7c1e14303 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -99,6 +99,8 @@ int main()
         while (run_planner) {
                 p.next();
                 p.tend();
+                if (p.opt_path())
+                        p.tlog(p.findt());
 #ifdef USE_GL
                 p.glplot();
 #endif

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index b299bd9576e47b0bf176c5fb741cd4b7ad3149c3..98d5d4b29a0d572c20839541519ed20665fda660 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -501,8 +501,6 @@ bool T2::next()
                                 if (this->goal_found(pn, this->cost)) {
                                         this->goal_cost();
                                         this->tlog(this->findt());
-                                        this->opt_path();
-                                        this->tlog(this->findt());
                                         en_add = false;
                                 }
                         }
@@ -538,8 +536,6 @@ bool T2::next()
                                         if (this->goal_found(pn, this->cost)) {
                                                 this->goal_cost();
                                                 this->tlog(this->findt());
-                                                this->opt_path();
-                                                this->tlog(this->findt());
                                                 en_add = false;
                                         }
                                 }