Add RRTNode, RRTBase class

diff --git a/CHANGELOG.md b/CHANGELOG.md
index c2e36d58310f7ddba873a277816bde2dafa5128a..04524a6f2b532cd762f787a0f3ca65f68e43950f 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -10,3 +10,5 @@ The format is based on [Keep a Changelog][] and this project adheres to
 ## Unreleased
 ### Added
 - Changelog, license, readme.
+- Class for RRT nodes.
+- Base class for RRT planners.

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
new file mode 100644 (file)
index 0000000..f75ef99
--- /dev/null
+++ b/base/rrtbase.cc
@@ -0,0 +1,105 @@
+/*
+This file is part of I am car.
+
+I am car is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+I am car is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with I am car. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <cmath>
+#include "rrtbase.h"
+
+RRTBase::RRTBase():
+        root_(new RRTNode()),
+        goal_(new RRTNode())
+{}
+
+RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
+        root_(init),
+        goal_(goal)
+{}
+
+RRTNode *RRTBase::root()
+{
+        return this->root_;
+}
+
+RRTNode *RRTBase::goal()
+{
+        return this->goal_;
+}
+
+std::vector<RRTNode *> &RRTBase::nodes()
+{
+        return this->nodes_;
+}
+
+std::vector<RRTNode *> &RRTBase::samples()
+{
+        return this->samples_;
+}
+
+std::vector<std::vector<RRTNode *>> &RRTBase::tlog()
+{
+        return this->tlog_;
+}
+
+bool RRTBase::goal_found()
+{
+        return this->goal_found_;
+}
+
+float RRTBase::elapsed()
+{
+        std::chrono::duration<float> dt;
+        dt = std::chrono::duration_cast<std::chrono::duration<float>>(
+                        this->tend_ - this->tstart_);
+        return dt.count();
+}
+
+bool RRTBase::tlog(std::vector<RRTNode *> t)
+{
+        this->tlog_.push_back(t);
+        return true;
+}
+
+void RRTBase::tstart()
+{
+        this->tstart_ = std::chrono::high_resolution_clock::now();
+}
+
+void RRTBase::tend()
+{
+        this->tend_ = std::chrono::high_resolution_clock::now();
+}
+
+bool RRTBase::goal_found(
+                RRTNode *node,
+                float (*cost)(RRTNode *, RRTNode* ))
+{
+        if (this->goal_found_)
+                return false;
+        float xx = pow(node->x() - this->goal_->x(), 2);
+        float yy = pow(node->y() - this->goal_->y(), 2);
+        float dh = std::abs(node->h() - this->goal_->h());
+        if (pow(xx + yy, 0.5) < this->GOAL_FOUND_DISTANCE &&
+                        dh < this->GOAL_FOUND_ANGLE) {
+                node->add_child(this->goal_, (*cost)(node, this->goal_));
+                //if (this->collide(node, this->goal_)) {
+                //        node->children().pop_back();
+                //        return false;
+                //}
+                this->goal_found_ = true;
+                return true;
+        }
+        return false;
+}

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
new file mode 100644 (file)
index 0000000..97881b6
--- /dev/null
+++ b/base/rrtnode.cc
@@ -0,0 +1,124 @@
+/*
+This file is part of I am car.
+
+I am car is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+I am car is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with I am car. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "rrtnode.h"
+RRTNode::RRTNode():
+        x_(0),
+        y_(0),
+        h_(0)
+{}
+
+RRTNode::RRTNode(float x, float y):
+        x_(x),
+        y_(y),
+        h_(0)
+{}
+
+RRTNode::RRTNode(float x, float y, float h):
+        x_(x),
+        y_(y),
+        h_(h)
+{}
+
+// getter
+float RRTNode::x() const
+{
+        return this->x_;
+}
+
+float RRTNode::y() const
+{
+        return this->y_;
+}
+
+float RRTNode::h() const
+{
+        return this->h_;
+}
+
+float RRTNode::ccost() const
+{
+        return this->ccost_;
+}
+
+float RRTNode::dcost() const
+{
+        return this->dcost_;
+}
+
+std::vector<RRTNode *> &RRTNode::children()
+{
+        return this->children_;
+}
+
+RRTNode *RRTNode::parent() const
+{
+        return this->parent_;
+}
+
+bool RRTNode::visited()
+{
+        return this->visited_;
+}
+
+// setter
+bool RRTNode::add_child(RRTNode *node, float cost)
+{
+        node->dcost(cost);
+        node->ccost(this->ccost() + cost);
+        node->parent(this);
+        this->children_.push_back(node);
+        return true;
+}
+
+float RRTNode::ccost(float cost)
+{
+        return this->ccost_ = cost;
+}
+
+float RRTNode::dcost(float cost)
+{
+        return this->dcost_ = cost;
+}
+
+bool RRTNode::parent(RRTNode *parent)
+{
+        if (parent == nullptr)
+                return false;
+        this->parent_ = parent;
+        return true;
+}
+
+bool RRTNode::visit(bool v)
+{
+        return this->visited_ = v;
+}
+
+// other
+bool RRTNode::comp_ccost(RRTNode *n1, RRTNode *n2)
+{
+        return n1->ccost() < n2->ccost();
+}
+
+bool RRTNode::visit()
+{
+        if (this->visited_) {
+                return true;
+        }
+        this->visited_ = true;
+        return false;
+}

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
new file mode 100644 (file)
index 0000000..8eec172
--- /dev/null
+++ b/incl/rrtbase.h
@@ -0,0 +1,67 @@
+/*
+This file is part of I am car.
+
+I am car is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+I am car is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with I am car. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef RRTBASE_H
+#define RRTBASE_H
+
+#include <chrono>
+#include <cmath>
+#include <vector>
+#include "rrtnode.h"
+
+class RRTBase {
+        private:
+                RRTNode *root_ = nullptr;
+                RRTNode *goal_ = nullptr;
+
+                std::vector<RRTNode *> nodes_;
+                std::vector<RRTNode *> samples_;
+
+                bool goal_found_ = false;
+                std::chrono::high_resolution_clock::time_point tstart_;
+                std::chrono::high_resolution_clock::time_point tend_;
+
+                std::vector<std::vector<RRTNode *>> tlog_; // trajectories
+        public:
+                const float GOAL_FOUND_DISTANCE = 1;
+                const float GOAL_FOUND_ANGLE = 2 * M_PI;
+                const float TMAX = 100; // computation time limit
+
+                RRTBase();
+                RRTBase(RRTNode *init, RRTNode *goal);
+
+                // getter
+                RRTNode *root();
+                RRTNode *goal();
+                std::vector<RRTNode *> &nodes();
+                std::vector<RRTNode *> &samples();
+                std::vector<std::vector<RRTNode *>> &tlog();
+                bool goal_found();
+                float elapsed();
+
+                // setter
+                bool tlog(std::vector<RRTNode *> t);
+                void tstart();
+                void tend();
+
+                // other
+                bool goal_found(
+                                RRTNode *node,
+                                float (*cost)(RRTNode *, RRTNode *));
+};
+
+#endif

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
new file mode 100644 (file)
index 0000000..27dfeda
--- /dev/null
+++ b/incl/rrtnode.h
@@ -0,0 +1,68 @@
+/*
+This file is part of I am car.
+
+I am car is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+I am car is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with I am car. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef RRTNODE_H
+#define RRTNODE_H
+
+#include <vector>
+
+class RRTNode {
+        private:
+                float x_ = 0;
+                float y_ = 0;
+                float h_ = 0;
+
+                float dcost_ = 0; // direct cost (of edge) to parent
+                float ccost_ = 0; // cumulative cost from root
+
+                RRTNode *parent_ = nullptr;
+                std::vector<RRTNode *> children_;
+
+                bool visited_ = false; // for DFS
+        public:
+                RRTNode();
+                RRTNode(float x, float y);
+                RRTNode(float x, float y, float h);
+
+                // getter
+                float x() const;
+                float y() const;
+                float h() const;
+
+                float ccost() const;
+                float dcost() const;
+
+                std::vector<RRTNode *> &children();
+                RRTNode *parent() const;
+
+                bool visited();
+
+                // setter
+                bool add_child(RRTNode *node, float cost);
+
+                float ccost(float cost);
+                float dcost(float cost);
+
+                bool parent(RRTNode *parent);
+                bool visit(bool v);
+
+                // other
+                static bool comp_ccost(RRTNode *n1, RRTNode *n2);
+                bool visit();
+};
+
+#endif