Merge branch 'feature/dijkstra-optimize-path'

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 466d27407ea7ed0061d0fba510884e7a7abbecea..98d5d4b29a0d572c20839541519ed20665fda660 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -18,6 +18,7 @@ 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"
@@ -500,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;
                                 }
                         }
@@ -537,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;
                                         }
                                 }
@@ -591,26 +588,119 @@ 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)
+                {};
+                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()
 {
-        std::vector<RRTNode *> cusps;
         if (this->tlog().size() == 0)
                 return false;
+        std::vector<RRTNode *> tmp_cusps;
         for (auto n: this->tlog().back()) {
-                if (n->parent() && sgn(n->s()) != sgn(n->parent()->s()))
-                                cusps.push_back(n);
+                if (n->parent() && sgn(n->s()) != sgn(n->parent()->s())) {
+                                tmp_cusps.push_back(n);
+                                tmp_cusps.push_back(n->parent());
+                }
+        }
+        std::vector<RRTNode *> cusps;
+        for (unsigned int i = 0; i < tmp_cusps.size() - 1; i++) {
+                if (tmp_cusps[i] != tmp_cusps[i + 1])
+                        cusps.push_back(tmp_cusps[i]);
         }
         cusps.push_back(this->root());
         std::reverse(cusps.begin(), cusps.end());
         cusps.push_back(this->goal());
-        int li = cusps.size() - 1;
-        int i = li - 1;
-        while (i >= 0) {
-                if (this->opt_part(cusps[i], cusps[li]))
-                        i--;
-                else
-                        li = i--;
+        // Begin of Dijkstra
+        std::vector<RRTNodeDijkstra> dnodes;
+        for (unsigned int i = 0; i < cusps.size(); i++)
+                dnodes.push_back(RRTNodeDijkstra(i));
+        dnodes[0].c = 0;
+        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 (this->collide(
+                                                steered[0],
+                                                steered[steered.size() - 1]))
+                                ch_cost = 9999 + 1;
+                        if (ch_cost < dnodes[i].c) {
+                                dnodes[i].c = ch_cost;
+                                dnodes[i].pi = tmp.ni;
+                                if (!dnodes[i].vi())
+                                        pq.push(dnodes[i]);
+                        }
+                }
+        }
+        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::vector<RRTNode *> npn; // new path nodes
+        std::reverse(npi.begin(), npi.end());
+        RRTNode *pn = cusps[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->goal(), this->cost(pn, this->goal()));
+        // End of Dijkstra
         return true;
 }