Add RRT planner based on [Kuwata2008] paper

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3f40e8f5466e9a38361b8df5ee3d21071ad77cf5..6d51555377fd65b04bb5e75b46fe84438c91ce98 100644 (file)
--- 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 e0c778aef94344308d9de33ed307bad009f305dd..9a9b54b7489a2d41f2a758200a92769d4269f2a3 100644 (file)
--- 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 9bf01ef3d6aaf19f1e3db0515272486f7fabdf6c..ac74a51f3715faaf155f7086eda25e03ac6088fb 100644 (file)
--- 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 f3b8a0297b6be1dcf3a1a1f2b5190c9db3c24ceb..3b271a88ca08bc49ad24123804aa2eeb8bfba833 100644 (file)
--- 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);