From 4348fb92b51877644f6ce17e63a69019f9edf221 Mon Sep 17 00:00:00 2001
From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Wed, 4 Jul 2018 13:27:41 +0200
Subject: [PATCH] Add RRT planner based on [Kuwata2008] paper

---
 CHANGELOG.md                   |  1 +
 base/main.cc                   |  1 +
 decision_control/rrtplanner.cc | 72 ++++++++++++++++++++++++++++++++++
 incl/rrtplanner.h              | 17 ++++++++
 4 files changed, 91 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3f40e8f..6d51555 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -31,6 +31,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
   [Coulter1992] paper for steering.
 - Testing parking scenarios.
 - Plot python script.
+- RRT planner based on [Kuwata2008] paper.
 
 ### Changed
 - Adding JSON ouput for edges, samples.
diff --git a/base/main.cc b/base/main.cc
index e0c778a..9a9b54b 100644
--- a/base/main.cc
+++ b/base/main.cc
@@ -29,6 +29,7 @@ int main()
         std::string encoding = jvi.get("encoding", "UTF-8" ).asString();
 
         LaValle1998 p(
+        //Kuwata2008 p(
         //Karaman2011 p(
                         new RRTNode(
                                 jvi["init"][0].asFloat(),
diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 9bf01ef..ac74a51 100644
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -24,6 +24,9 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "rrtplanner.h"
 #include "cost.h"
 
+#define KUWATA2008_CCOST co3
+#define KUWATA2008_DCOST co1
+
 LaValle1998::LaValle1998(RRTNode *init, RRTNode *goal):
         RRTBase(init, goal),
         nn(nn1),
@@ -56,6 +59,75 @@ bool LaValle1998::next()
         return this->goal_found();
 }
 
+Kuwata2008::Kuwata2008(RRTNode *init, RRTNode *goal):
+        RRTBase(init, goal),
+        nn(nn1),
+        sample(sa1),
+        steer(st1),
+        cost(KUWATA2008_DCOST)
+{
+        srand(static_cast<unsigned>(time(0)));
+}
+
+bool Kuwata2008::next()
+{
+        RRTNode *rs;
+        if (this->samples().size() == 0) {
+                rs = this->goal();
+        } else {
+                rs = this->sample();
+        }
+        this->samples().push_back(rs);
+        float heur = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
+        if (this->goal_found()) {
+                if (heur < 0.7)
+                        this->cost = &KUWATA2008_CCOST;
+                else
+                        this->cost = &KUWATA2008_DCOST;
+        } else {
+                if (heur < 0.3)
+                        this->cost = &KUWATA2008_CCOST;
+                else
+                        this->cost = &KUWATA2008_DCOST;
+        }
+        RRTNode *nn = this->nn(this->root(), rs, this->cost);
+        RRTNode *pn = nn;
+        std::vector<RRTNode *> newly_added;
+        for (auto ns: this->steer(nn, rs)) {
+                this->nodes().push_back(ns);
+                pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
+                if (this->collide(pn, ns)) {
+                        pn->children().pop_back();
+                        break;
+                }
+                pn = ns;
+                newly_added.push_back(pn);
+                if (this->goal_found(pn, &KUWATA2008_DCOST)) {
+                        this->tlog(this->findt());
+                        break;
+                }
+        }
+        if (this->samples().size() > 1) {
+                for (auto na: newly_added) {
+                        pn = na;
+                        for (auto ns: this->steer(na, this->goal())) {
+                                this->nodes().push_back(ns);
+                                pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
+                                if (this->collide(pn, ns)) {
+                                        pn->children().pop_back();
+                                        break;
+                                }
+                                pn = ns;
+                                if (this->goal_found(pn, &KUWATA2008_DCOST)) {
+                                        this->tlog(this->findt());
+                                        break;
+                                }
+                        }
+                }
+        }
+        return this->goal_found();
+}
+
 Karaman2011::Karaman2011(RRTNode *init, RRTNode *goal):
         RRTBase(init, goal),
         nn(nn1),
diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index f3b8a02..3b271a8 100644
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -40,6 +40,23 @@ class LaValle1998: public RRTBase {
                 bool next();
 };
 
+class Kuwata2008: public RRTBase {
+        public:
+                Kuwata2008(RRTNode *init, RRTNode *goal);
+
+                // RRT framework
+                RRTNode *(*nn)(
+                                RRTNode *root,
+                                RRTNode *node,
+                                float (*cost)(RRTNode *, RRTNode *));
+                RRTNode *(*sample)();
+                std::vector<RRTNode *> (*steer)(
+                                RRTNode *init,
+                                RRTNode *goal);
+                float (*cost)(RRTNode *init, RRTNode *goal);
+                bool next();
+};
+
 class Karaman2011: public RRTBase {
         public:
                 Karaman2011(RRTNode *init, RRTNode *goal);
-- 
2.39.2