From: Jiri Hubacek Date: Fri, 29 Jun 2018 18:33:45 +0000 (+0200) Subject: Add RRTNode, RRTBase class X-Git-Tag: v0.1.0~33^2~3 X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/840c6ce735ae53307deb7066a4c9d443dd564efd Add RRTNode, RRTBase class --- diff --git a/CHANGELOG.md b/CHANGELOG.md index c2e36d5..04524a6 100644 --- 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 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 . +*/ + +#include +#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 &RRTBase::nodes() +{ + return this->nodes_; +} + +std::vector &RRTBase::samples() +{ + return this->samples_; +} + +std::vector> &RRTBase::tlog() +{ + return this->tlog_; +} + +bool RRTBase::goal_found() +{ + return this->goal_found_; +} + +float RRTBase::elapsed() +{ + std::chrono::duration dt; + dt = std::chrono::duration_cast>( + this->tend_ - this->tstart_); + return dt.count(); +} + +bool RRTBase::tlog(std::vector 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 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 . +*/ + +#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::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 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 . +*/ + +#ifndef RRTBASE_H +#define RRTBASE_H + +#include +#include +#include +#include "rrtnode.h" + +class RRTBase { + private: + RRTNode *root_ = nullptr; + RRTNode *goal_ = nullptr; + + std::vector nodes_; + std::vector samples_; + + bool goal_found_ = false; + std::chrono::high_resolution_clock::time_point tstart_; + std::chrono::high_resolution_clock::time_point tend_; + + std::vector> 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 &nodes(); + std::vector &samples(); + std::vector> &tlog(); + bool goal_found(); + float elapsed(); + + // setter + bool tlog(std::vector 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 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 . +*/ + +#ifndef RRTNODE_H +#define RRTNODE_H + +#include + +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 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 &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