Move path optimization procedure to RRT base

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 5da6fe8ad5a5c7da52a52a50b42f4cbb1d36040d..0bb7442f31e802db14a6986e3c363b6f1147ea00 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -15,14 +15,19 @@ You should have received a copy of the GNU General Public License
 along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
+#include <algorithm>
 #include <cmath>
 #include <omp.h>
+#include <queue>
 #include "bcar.h"
 #include "rrtbase.h"
 // OpenGL
 #include <GL/gl.h>
 #include <GL/glu.h>
 #include <SDL2/SDL.h>
+// RRT
+#include "cost.h"
+#include "steer.h"
 
 extern SDL_Window* gw;
 extern SDL_GLContext gc;
@@ -429,6 +434,144 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
         return false;
 }
 
+class RRTNodeDijkstra {
+        public:
+                RRTNodeDijkstra(int i):
+                        ni(i),
+                        pi(0),
+                        c(9999),
+                        v(false)
+                {};
+                RRTNodeDijkstra(int i, float c):
+                        ni(i),
+                        pi(0),
+                        c(c),
+                        v(false)
+                {};
+                RRTNodeDijkstra(int i, int p, float c):
+                        ni(i),
+                        pi(p),
+                        c(c),
+                        v(false)
+                {};
+                unsigned int ni;
+                unsigned int pi;
+                float c;
+                bool v;
+                bool vi()
+                {
+                        if (this->v)
+                                return true;
+                        this->v = true;
+                        return false;
+                };
+};
+
+class RRTNodeDijkstraComparator {
+        public:
+                int operator() (
+                                const RRTNodeDijkstra& n1,
+                                const RRTNodeDijkstra& n2)
+                {
+                        return n1.c > n2.c;
+                }
+};
+
+bool RRTBase::opt_path()
+{
+        if (this->tlog().size() == 0)
+                return false;
+        float oc = this->tlog().back().front()->ccost();
+        std::vector<RRTNode *> tmp_cusps;
+        for (auto n: this->tlog().back()) {
+                if (sgn(n->s()) == 0) {
+                        tmp_cusps.push_back(n);
+                } else if (n->parent() &&
+                                sgn(n->s()) != sgn(n->parent()->s())) {
+                        tmp_cusps.push_back(n);
+                        tmp_cusps.push_back(n->parent());
+                }
+        }
+        if (tmp_cusps.size() < 2)
+                return false;
+        std::vector<RRTNode *> cusps;
+        for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
+                if (tmp_cusps[i] != tmp_cusps[i + 1])
+                        cusps.push_back(tmp_cusps[i]);
+        }
+        std::reverse(cusps.begin(), cusps.end());
+        // Begin of Dijkstra
+        std::vector<RRTNodeDijkstra> dnodes;
+        for (unsigned int i = 0; i < cusps.size(); i++)
+                if (i > 0)
+                        dnodes.push_back(RRTNodeDijkstra(
+                                                i,
+                                                i - 1,
+                                                cusps[i]->ccost()));
+                else
+                        dnodes.push_back(RRTNodeDijkstra(
+                                                i,
+                                                cusps[i]->ccost()));
+        dnodes[0].vi();
+        std::priority_queue<
+                RRTNodeDijkstra,
+                std::vector<RRTNodeDijkstra>,
+                RRTNodeDijkstraComparator> pq;
+        RRTNodeDijkstra tmp = dnodes[0];
+        pq.push(tmp);
+        float ch_cost = 9999;
+        std::vector<RRTNode *> steered;
+        while (!pq.empty() && tmp.ni != cusps.size() - 1) {
+                tmp = pq.top();
+                pq.pop();
+                for (unsigned int i = tmp.ni + 1; i < cusps.size(); i++) {
+                        ch_cost = dnodes[tmp.ni].c +
+                                CO(cusps[tmp.ni], cusps[i]);
+                        steered = ST(cusps[tmp.ni], cusps[i]);
+                        for (unsigned int j = 0; j < steered.size() - 1; j++)
+                                steered[j]->add_child(
+                                                steered[j + 1],
+                                                CO(
+                                                        steered[j],
+                                                        steered[j + 1]));
+                        if (i != tmp.ni + 1 && this->collide( // TODO
+                                                steered[0],
+                                                steered[steered.size() - 1]))
+                                continue;
+                        if (ch_cost < dnodes[i].c) {
+                                dnodes[i].c = ch_cost;
+                                dnodes[i].pi = tmp.ni;
+                                if (!dnodes[i].vi())
+                                        pq.push(dnodes[i]);
+                        }
+                }
+        }
+        if (tmp.ni != cusps.size() - 1)
+                return false;
+        std::vector<int> npi; // new path indexes
+        int tmpi = tmp.ni;
+        while (tmpi > 0) {
+                npi.push_back(tmpi);
+                tmpi = dnodes[tmpi].pi;
+        }
+        npi.push_back(tmpi);
+        std::reverse(npi.begin(), npi.end());
+        RRTNode *pn = cusps[npi[0]];
+        for (unsigned int i = 0; i < npi.size() - 1; i++) {
+                for (auto ns: ST(cusps[npi[i]], cusps[npi[i + 1]])) {
+                        pn->add_child(ns, CO(pn, ns));
+                        pn = ns;
+                }
+        }
+        pn->add_child(
+                        this->tlog().back().front(),
+                        CO(pn, this->tlog().back().front()));
+        // End of Dijkstra
+        if (this->tlog().back().front()->ccost() < oc)
+                return true;
+        return false;
+}
+
 bool RRTBase::rebase(RRTNode *nr)
 {
         if (!nr || this->goal_ == nr || this->root_ == nr)

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 747d00886d98f760e5e142dcde4eaaad07ffc29c..0cecd74ebfa698062aad8bd1f0e404e868a98516 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -15,10 +15,8 @@ You should have received a copy of the GNU General Public License
 along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include <algorithm>
 #include <cstdlib>
 #include <ctime>
-#include <queue>
 #include "compile.h"
 #include "nn.h"
 #include "nv.h"
@@ -590,144 +588,6 @@ float T2::goal_cost()
         return this->goal()->ccost();
 }
 
-class RRTNodeDijkstra {
-        public:
-                RRTNodeDijkstra(int i):
-                        ni(i),
-                        pi(0),
-                        c(9999),
-                        v(false)
-                {};
-                RRTNodeDijkstra(int i, float c):
-                        ni(i),
-                        pi(0),
-                        c(c),
-                        v(false)
-                {};
-                RRTNodeDijkstra(int i, int p, float c):
-                        ni(i),
-                        pi(p),
-                        c(c),
-                        v(false)
-                {};
-                unsigned int ni;
-                unsigned int pi;
-                float c;
-                bool v;
-                bool vi()
-                {
-                        if (this->v)
-                                return true;
-                        this->v = true;
-                        return false;
-                };
-};
-
-class RRTNodeDijkstraComparator {
-        public:
-                int operator() (
-                                const RRTNodeDijkstra& n1,
-                                const RRTNodeDijkstra& n2)
-                {
-                        return n1.c > n2.c;
-                }
-};
-
-bool T2::opt_path()
-{
-        if (this->tlog().size() == 0)
-                return false;
-        float oc = this->tlog().back().front()->ccost();
-        std::vector<RRTNode *> tmp_cusps;
-        for (auto n: this->tlog().back()) {
-                if (sgn(n->s()) == 0) {
-                        tmp_cusps.push_back(n);
-                } else if (n->parent() &&
-                                sgn(n->s()) != sgn(n->parent()->s())) {
-                        tmp_cusps.push_back(n);
-                        tmp_cusps.push_back(n->parent());
-                }
-        }
-        if (tmp_cusps.size() < 2)
-                return false;
-        std::vector<RRTNode *> cusps;
-        for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
-                if (tmp_cusps[i] != tmp_cusps[i + 1])
-                        cusps.push_back(tmp_cusps[i]);
-        }
-        std::reverse(cusps.begin(), cusps.end());
-        // Begin of Dijkstra
-        std::vector<RRTNodeDijkstra> dnodes;
-        for (unsigned int i = 0; i < cusps.size(); i++)
-                if (i > 0)
-                        dnodes.push_back(RRTNodeDijkstra(
-                                                i,
-                                                i - 1,
-                                                cusps[i]->ccost()));
-                else
-                        dnodes.push_back(RRTNodeDijkstra(
-                                                i,
-                                                cusps[i]->ccost()));
-        dnodes[0].vi();
-        std::priority_queue<
-                RRTNodeDijkstra,
-                std::vector<RRTNodeDijkstra>,
-                RRTNodeDijkstraComparator> pq;
-        RRTNodeDijkstra tmp = dnodes[0];
-        pq.push(tmp);
-        float ch_cost = 9999;
-        std::vector<RRTNode *> steered;
-        while (!pq.empty() && tmp.ni != cusps.size() - 1) {
-                tmp = pq.top();
-                pq.pop();
-                for (unsigned int i = tmp.ni + 1; i < cusps.size(); i++) {
-                        ch_cost = dnodes[tmp.ni].c +
-                                this->cost(cusps[tmp.ni], cusps[i]);
-                        steered = this->steer(cusps[tmp.ni], cusps[i]);
-                        for (unsigned int j = 0; j < steered.size() - 1; j++)
-                                steered[j]->add_child(
-                                                steered[j + 1],
-                                                this->cost(
-                                                        steered[j],
-                                                        steered[j + 1]));
-                        if (i != tmp.ni + 1 && this->collide( // TODO
-                                                steered[0],
-                                                steered[steered.size() - 1]))
-                                continue;
-                        if (ch_cost < dnodes[i].c) {
-                                dnodes[i].c = ch_cost;
-                                dnodes[i].pi = tmp.ni;
-                                if (!dnodes[i].vi())
-                                        pq.push(dnodes[i]);
-                        }
-                }
-        }
-        if (tmp.ni != cusps.size() - 1)
-                return false;
-        std::vector<int> npi; // new path indexes
-        int tmpi = tmp.ni;
-        while (tmpi > 0) {
-                npi.push_back(tmpi);
-                tmpi = dnodes[tmpi].pi;
-        }
-        npi.push_back(tmpi);
-        std::reverse(npi.begin(), npi.end());
-        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));
-                        pn = ns;
-                }
-        }
-        pn->add_child(
-                        this->tlog().back().front(),
-                        this->cost(pn, this->tlog().back().front()));
-        // End of Dijkstra
-        if (this->tlog().back().front()->ccost() < oc)
-                return true;
-        return false;
-}
-
 bool T2::opt_part(RRTNode *init, RRTNode *goal)
 {
         std::vector<RRTNode *> steered;

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index f95e7e4fd046a6f57244c68f680a7c8c32913f04..dfb8aef38a8606f76fb2318d8ad708aa2a4cb377 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -92,6 +92,7 @@ class RRTBase {
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *));
                 bool collide(RRTNode *init, RRTNode *goal);
+                bool opt_path();
                 bool rebase(RRTNode *nr);
                 std::vector<RRTNode *> findt();
                 std::vector<RRTNode *> findt(RRTNode *n);

diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index 433ee3a4753c9b72eb3ce82088b70b26f44d8aa9..cd40f8591b973496d5820ccdc8352f0fc306a1ef 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -139,7 +139,6 @@ class T2: public Karaman2011 {
 
                 bool next();
                 float goal_cost();
-                bool opt_path();
                 bool opt_part(RRTNode *init, RRTNode *goal);
 };