Add RRT testing planner `T1`

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 6d51555377fd65b04bb5e75b46fe84438c91ce98..f4eaeb68b38c97b2faf9076b5c13f9f02308eac9 100644 (file)
--- 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 9a9b54b7489a2d41f2a758200a92769d4269f2a3..22cc01a5e3047f431fe3f227504e076ab9a021a3 100644 (file)
--- 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 ac74a51f3715faaf155f7086eda25e03ac6088fb..bbf63cb10a1edfa5fc3e808c18fd16eb524f1da8 100644 (file)
--- 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 3b271a88ca08bc49ad24123804aa2eeb8bfba833..f474a763880da64f237115951b4d321b3d13bcd0 100644 (file)
--- 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