From 624bcc315bc330b2a9c41326d3f55dfa2e663b66 Mon Sep 17 00:00:00 2001
From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Wed, 4 Jul 2018 15:28:59 +0200
Subject: [PATCH] Add RRT testing planner `T1`

---
 CHANGELOG.md                   |   2 +
 base/main.cc                   |   1 +
 decision_control/rrtplanner.cc | 103 +++++++++++++++++++++++++++++++++
 incl/rrtplanner.h              |  22 +++++++
 4 files changed, 128 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 6d51555..f4eaeb6 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -32,6 +32,8 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Testing parking scenarios.
 - Plot python script.
 - RRT planner based on [Kuwata2008] paper.
+- RRT testing planner `T1`. First node of steer add as in RRT\* (connect and
+  rewire procedures) then the rest stick to the first node without rewiring.
 
 ### Changed
 - Adding JSON ouput for edges, samples.
diff --git a/base/main.cc b/base/main.cc
index 9a9b54b..22cc01a 100644
--- a/base/main.cc
+++ b/base/main.cc
@@ -31,6 +31,7 @@ int main()
         LaValle1998 p(
         //Kuwata2008 p(
         //Karaman2011 p(
+        //T1 p(
                         new RRTNode(
                                 jvi["init"][0].asFloat(),
                                 jvi["init"][1].asFloat(),
diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index ac74a51..bbf63cb 100644
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -209,3 +209,106 @@ bool Karaman2011::next()
         }
         return this->goal_found();
 }
+
+T1::T1(RRTNode *init, RRTNode *goal):
+        RRTBase(init, goal),
+        nn(nn1),
+        nv(nv1),
+        sample(sa1),
+        steer(st1),
+        cost(co1)
+{
+        srand(static_cast<unsigned>(time(0)));
+}
+
+bool T1::next()
+{
+        RRTNode *rs;
+        if (this->samples().size() == 0)
+                rs = this->goal();
+        else
+                rs = this->sample();
+        this->samples().push_back(rs);
+        RRTNode *nn = this->nn(this->root(), rs, this->cost);
+        RRTNode *pn = nn;
+        std::vector<RRTNode *> nvs;
+        bool connected;
+        RRTNode *op; // old parent
+        float od; // old direct cost
+        float oc; // old cumulative cost
+        std::vector<RRTNode *> steered = this->steer(nn, rs);
+        // RRT* for first node
+        RRTNode *ns = steered[0];
+        {
+                nvs = this->nv(this->root(), ns, this->cost, MIN(
+                                        GAMMA_RRTSTAR(this->nodes().size()),
+                                        0.2)); // TODO const
+                this->nodes().push_back(ns);
+                connected = false;
+                pn->add_child(ns, this->cost(pn, ns));
+                if (this->collide(pn, ns)) {
+                        pn->children().pop_back();
+                } else {
+                        connected = true;
+                }
+                // connect
+                for (auto nv: nvs) {
+                        if (!connected || (nv->ccost() + this->cost(nv, ns) <
+                                        ns->ccost())) {
+                                op = ns->parent();
+                                od = ns->dcost();
+                                oc = ns->ccost();
+                                nv->add_child(ns, this->cost(nv, ns));
+                                if (this->collide(nv, ns)) {
+                                        nv->children().pop_back();
+                                        ns->parent(op);
+                                        ns->dcost(od);
+                                        ns->ccost(oc);
+                                } else if (connected) {
+                                        op->children().pop_back();
+                                } else {
+                                        connected = true;
+                                }
+                        }
+                }
+                if (!connected)
+                        return false;
+                // rewire
+                for (auto nv: nvs) {
+                        if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
+                                op = nv->parent();
+                                od = nv->dcost();
+                                oc = nv->ccost();
+                                ns->add_child(nv, this->cost(ns, nv));
+                                if (this->collide(ns, nv)) {
+                                        ns->children().pop_back();
+                                        nv->parent(op);
+                                        nv->dcost(od);
+                                        nv->ccost(oc);
+                                } else {
+                                        op->rem_child(nv);
+                                }
+                        }
+                }
+                pn = ns;
+                if (this->goal_found(pn, this->cost)) {
+                        this->tlog(this->findt());
+                }
+        }
+        unsigned int i = 0;
+        for (i = 1; i < steered.size(); i++) {
+                ns = steered[i];
+                this->nodes().push_back(ns);
+                pn->add_child(ns, this->cost(pn, ns));
+                if (this->collide(pn, ns)) {
+                        pn->children().pop_back();
+                        break;
+                }
+                pn = ns;
+                if (this->goal_found(pn, this->cost)) {
+                        this->tlog(this->findt());
+                        break;
+                }
+        }
+        return this->goal_found();
+}
diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index 3b271a8..f474a76 100644
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -79,4 +79,26 @@ class Karaman2011: public RRTBase {
                 bool next();
 };
 
+class T1: public RRTBase {
+        public:
+                T1(RRTNode *init, RRTNode *goal);
+
+                // RRT framework
+                RRTNode *(*nn)(
+                                RRTNode *root,
+                                RRTNode *node,
+                                float (*cost)(RRTNode *, RRTNode *));
+                std::vector<RRTNode *> (*nv)(
+                                RRTNode *root,
+                                RRTNode *node,
+                                float (*cost)(RRTNode *, RRTNode *),
+                                float dist);
+                RRTNode *(*sample)();
+                std::vector<RRTNode *> (*steer)(
+                                RRTNode *init,
+                                RRTNode *goal);
+                float (*cost)(RRTNode *init, RRTNode *goal);
+                bool next();
+};
+
 #endif
-- 
2.39.2