Merge branch 'feature/dijkstra-path-opt'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index bcf98fd8ef022183663f8ef3268dd3019ad60076..4aac6efaa58ccb1c3aef2a4b3a3bfc8d8be904b0 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -13,6 +13,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Time restriction for algorithm.
 - RRT node types (currently `cusp` and `connected`).
 - Compound extensions.
+- Dijkstra algorithm for path optimization (`rrtext3.cc`).
 
 [cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 74c03ee47e1c85d0109cdc8bf53564e4babf54bf..d9ca70847639646bd9aedee7fa911d8b0e575840 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/api)
 
 add_library(rrts SHARED
         src/rrts.cc
+        src/rrtext3.cc
         src/rrtext2.cc
         src/rrtext1.cc
         src/reeds_shepp.cpp

diff --git a/README.md b/README.md
index 96cd47163bc1564e2a8aa4bcf875f73e333d5a78..ca075904914e1edbc2c57febad6a9f157d27ed2a 100644 (file)
--- a/README.md
+++ b/README.md
@@ -30,6 +30,7 @@ and upgrades to RRT, *extensions* are declared in `rrtext.h` and implemented in
 
 ## Implemented extensions
 There is a list of implemented extensions and what they include:
+- `rrtext3.cc`: Dijkstra algorithm for path optimization,
 - `rrtext2.cc`: [cute c2][] for collision detection,
 - `rrtext1.cc`: different cost for building and searching.
 

diff --git a/api/rrtext.h b/api/rrtext.h
index 170f8e8b6bbb3580b218b541bd6f04048375b26b..db4a36fccc11c3c76820932831840780556c5317 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -6,6 +6,24 @@
 // ext2
 #include "cute_c2.h"
 
+/*! \brief Use Dijkstra algorithm to find the shorter path.
+*/
+class RRTExt3 : public virtual RRTS {
+        private:
+                std::vector<RRTNode *> orig_path_;
+                double orig_path_cost_;
+        public:
+                std::vector<RRTNode *> path();
+
+                // getter, setter
+                std::vector<RRTNode *> &orig_path()
+                {
+                        return this->orig_path_;
+                };
+                double &orig_path_cost() { return this->orig_path_cost_; }
+                void orig_path_cost(double c) { this->orig_path_cost_ = c; }
+};
+
 /*! \brief Use cute_c2 for collision detection.
 
 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h

diff --git a/api/rrts.h b/api/rrts.h
index 1d870f74c49e7bc491a967721283bd1d84222da5..79436901f57239c482de2be8e329e8511b49d145 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -91,7 +91,7 @@ class RRTS {
                 the current iteration should be the last one.
                 */
                 bool should_stop();
-
+        protected:
                 // RRT procedures
                 std::tuple<bool, unsigned int, unsigned int>
                 collide(std::vector<std::tuple<double, double>> &poly);
@@ -119,9 +119,15 @@ class RRTS {
                 bool connect();
                 void rewire();
         public:
+                /*! \brief Initialize RRT algorithm if needed.
+                */
+                virtual void init();
+                /*! \brief Deinitialize RRT algorithm if needed.
+                */
+                virtual void deinit();
                 /*! \brief Return path found by RRT*.
                 */
-                std::vector<RRTNode *> path();
+                virtual std::vector<RRTNode *> path();
                 /*! \brief Run next RRT* iteration.
                 */
                 bool next();

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
new file mode 100644 (file)
index 0000000..f8c694c
--- /dev/null
+++ b/src/rrtext3.cc
@@ -0,0 +1,107 @@
+#include <queue>
+#include "rrtext.h"
+
+std::vector<RRTNode *> RRTExt3::path()
+{
+        if (this->orig_path().size() == 0) {
+                this->orig_path_ = RRTS::path();
+                if (this->orig_path().size() == 0)
+                        return this->orig_path();
+                else
+                        this->orig_path_cost(cc(*this->orig_path().back()));
+        }
+        class DijkstraNode : public RRTNode {
+                public:
+                        DijkstraNode *s = nullptr;
+                        RRTNode *n = nullptr;
+                        unsigned int i = 0;
+                        double cc = 9999;
+                        bool v = false;
+                        bool vi()
+                        {
+                                if (this->v)
+                                        return true;
+                                this->v = true;
+                                return false;
+                        }
+                        using RRTNode::RRTNode;
+                        // override
+                        DijkstraNode *p_ = nullptr;
+                        DijkstraNode *p() const { return this->p_; }
+                        void p(DijkstraNode *p) { this->p_ = p; }
+        };
+        class DijkstraNodeComparator {
+                public:
+                        int operator() (
+                                const DijkstraNode &n1,
+                                const DijkstraNode &n2
+                        )
+                        {
+                                return n1.cc > n2.cc;
+                        }
+        };
+        std::vector<DijkstraNode> dn;
+        dn.reserve(this->orig_path().size());
+        unsigned int dncnt = 0;
+        for (auto n: this->orig_path()) {
+                if (
+                        n->t(RRTNodeType::cusp)
+                        || n->t(RRTNodeType::connected)
+                ) {
+                        dn.push_back(DijkstraNode(*n));
+                        dn.back().cc = cc(*n);
+                        dn.back().s = &dn.back();
+                        dn.back().n = n;
+                        dn.back().i = dncnt++;
+                }
+        }
+        dn.push_back(DijkstraNode(*this->orig_path().back()));
+        dn.back().cc = cc(*this->orig_path().back());
+        dn.back().s = &dn.back();
+        dn.back().n = this->orig_path().back();
+        dn.back().i = dncnt++;
+        std::priority_queue<
+                DijkstraNode,
+                std::vector<DijkstraNode>,
+                DijkstraNodeComparator
+        > pq;
+        dn.front().vi();
+        pq.push(dn.front());
+        while (!pq.empty()) {
+                DijkstraNode f = pq.top();
+                pq.pop();
+                for (unsigned int i = f.i + 1; i < dncnt; i++) {
+                        double cost = f.cc + this->cost_search(f, dn[i]);
+                        this->steer(f, dn[i]);
+                        if (this->steered().size() == 0)
+                                break; // TODO why not continue?
+                        if (std::get<0>(this->collide_steered_from(f)))
+                                continue;
+                        if (cost < dn[i].cc) {
+                                dn[i].cc = cost;
+                                dn[i].p(f.s);
+                                if (!dn[i].vi())
+                                        pq.push(dn[i]);
+                        }
+                }
+        }
+        DijkstraNode *ln = nullptr;
+        for (auto &n: dn) {
+                if (n.v)
+                        ln = &n;
+        }
+        if (ln == nullptr || ln->p() == nullptr)
+                return this->orig_path();
+        while (ln->p() != nullptr) {
+                RRTNode *t = ln->n;
+                RRTNode *f = ln->p()->n;
+                this->steer(*f, *t);
+                if (std::get<0>(this->collide_steered_from(*f)))
+                        return this->orig_path();
+                this->join_steered(f);
+                t->p(&this->nodes().back());
+                t->c(this->cost_build(this->nodes().back(), *t));
+                ln = ln->p();
+        }
+        return RRTS::path();
+}

diff --git a/src/rrts.cc b/src/rrts.cc
index 46dad56965500cfc050481a86f481335da0c4c60..c3fd155377f67f632f59b05f84909ba044fb4ddf 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -242,6 +242,14 @@ void RRTS::rewire()
 }
 
 // API
+void RRTS::init()
+{
+}
+
+void RRTS::deinit()
+{
+}
+
 std::vector<RRTNode *> RRTS::path()
 {
         std::vector<RRTNode *> path;