#endif
// RRT
-#include "sample.h"
#include "cost.h"
#include "steer.h"
-#include "nn.h"
-#include "nv.h"
#if USE_GL > 0
extern SDL_Window* gw;
delete this->goal_;
}
-RRTBase::RRTBase():
- root_(new RRTNode()),
- goal_(new RRTNode()),
- gen_(std::random_device{}())
+RRTBase::RRTBase()
+ : root_(new RRTNode())
+ , goal_(new RRTNode())
+ , gen_(std::random_device{}())
{
this->nodes_.reserve(NOFNODES);
this->nodes_.push_back(this->root_);
this->add_iy(this->root_);
this->add_ixy(this->root_);
+ float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+ float hrange = (this->HMAX - this->HMIN) / 2;
+ float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+ float vrange = (this->VMAX - this->VMIN) / 2;
+ this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+ this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+ this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
}
-RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
- root_(init),
- goal_(goal),
- gen_(std::random_device{}())
+RRTBase::RRTBase(RRTNode *init, RRTNode *goal)
+ : root_(init)
+ , goal_(goal)
+ , gen_(std::random_device{}())
{
this->nodes_.reserve(NOFNODES);
this->nodes_.push_back(init);
this->add_iy(init);
this->add_ixy(init);
+ float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+ float hrange = (this->HMAX - this->HMIN) / 2;
+ float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+ float vrange = (this->VMAX - this->VMIN) / 2;
+ this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+ this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+ this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
}
// getter
return this->goal_;
}
+std::vector<RRTNode *> &RRTBase::goals()
+{
+ return this->goals_;
+}
+
std::vector<RRTNode *> &RRTBase::nodes()
{
return this->nodes_;
return this->dnodes_;
}
+std::queue<RRTNode *> &RRTBase::firsts()
+{
+ return this->firsts_;
+}
+
PolygonObstacle &RRTBase::frame()
{
return this->frame_;
this->goal_ = node;
}
+void RRTBase::goals(std::vector<RRTNode *> g)
+{
+ this->goals_ = g;
+ std::reverse(this->goals_.begin(), this->goals_.end());
+ RRTNode *pn = this->goals_.front();
+ for (auto n: this->goals_) {
+ if (n != pn) {
+ pn->add_child(n, this->cost(pn ,n));
+ pn = n;
+ }
+ }
+}
+
bool RRTBase::logr(RRTNode *root)
{
std::vector<RRTEdge *> e; // Edges to log
bool RRTBase::add_iy(RRTNode *n)
{
- int i = IYI(n->y());
+ int i = this->YI(n);
if (i < 0)
i = 0;
if (i >= IYSIZE)
bool RRTBase::add_ixy(RRTNode *n)
{
- int ix = IXI(n->x());
+ int ix = this->XI(n);
if (ix < 0)
ix = 0;
if (ix >= IXSIZE)
ix = IXSIZE - 1;
- int iy = IYI(n->y());
+ int iy = this->YI(n);
if (iy < 0)
iy = 0;
if (iy >= IYSIZE)
bool RRTBase::glplot()
{
#if USE_GL > 0
+ float glplwscale = 1.0 / ((this->VMAX) - (this->VMIN));
+ float glplhscale = 1.0 / ((this->HMAX) - (this->HMIN));
glClear(GL_COLOR_BUFFER_BIT);
glLineWidth(1);
glPointSize(1);
glVertex2f(GLVERTEX(this->samples_.back()));
glEnd();
}
- // Plot nodes
+ // Plot nodes (position, orientation
std::vector<RRTNode *> s; // DFS stack
std::vector<RRTNode *> r; // reset visited_
RRTNode *tmp;
glBegin(GL_LINES);
s.push_back(this->root_);
+ while (s.size() > 0) {
+ tmp = s.back();
+ s.pop_back();
+ if (!tmp->visit()) {
+ r.push_back(tmp);
+ for (auto ch: tmp->children()) {
+ s.push_back(ch);
+ glColor3f(0.5, 0.5, 0.5);
+ BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
+ glVertex2f(
+ bc.lfx() * glplwscale,
+ bc.lfy() * glplhscale
+ );
+ glVertex2f(
+ bc.lrx() * glplwscale,
+ bc.lry() * glplhscale
+ );
+ glVertex2f(
+ bc.lrx() * glplwscale,
+ bc.lry() * glplhscale
+ );
+ glVertex2f(
+ bc.rrx() * glplwscale,
+ bc.rry() * glplhscale
+ );
+ glVertex2f(
+ bc.rrx() * glplwscale,
+ bc.rry() * glplhscale
+ );
+ glVertex2f(
+ bc.rfx() * glplwscale,
+ bc.rfy() * glplhscale
+ );
+ }
+ }
+ }
+ glEnd();
+ // Plot nodes
+ glBegin(GL_LINES);
+ s.push_back(this->root_);
while (s.size() > 0) {
tmp = s.back();
s.pop_back();
RRTNode *node,
float (*cost)(RRTNode *, RRTNode* ))
{
- if (IS_NEAR(node, this->goal_)) {
+ if (GOAL_IS_NEAR(node, this->goal_)) {
if (this->goal_found_) {
if (node->ccost() + this->cost(node, this->goal_) <
this->goal_->ccost()) {
return false;
}
this->goal_found_ = true;
+ // Update ccost of goal's parents
+ if (this->goals().size() > 0) {
+ RRTNode *ch = this->goals().back();
+ RRTNode *pn = this->goals().back()->parent();
+ while (pn) {
+ pn->ccost(
+ ch->ccost()
+ - this->cost(pn, ch)
+ );
+ ch = pn;
+ pn = pn->parent();
+ }
+ }
+ return true;
+ }
+ }
+ return false;
+}
+
+bool RRTBase::goal_found(
+ RRTNode *node,
+ RRTNode *goal
+)
+{
+ if (GOAL_IS_NEAR(node, goal)) {
+ if (this->goal_found_) {
+ if (
+ goal->ccost() != -1
+ && node->ccost() + this->cost(node, goal)
+ < goal->ccost()
+ ) {
+ RRTNode *op; // old parent
+ float oc; // old cumulative cost
+ float od; // old direct cost
+ op = goal->parent();
+ oc = goal->ccost();
+ od = goal->dcost();
+ node->add_child(goal,
+ this->cost(node, goal));
+ if (this->collide(node, goal)) {
+ node->children().pop_back();
+ goal->parent(op);
+ goal->ccost(oc);
+ goal->dcost(od);
+ } else {
+ op->rem_child(goal);
+ return true;
+ }
+ } else {
+ return false;
+ }
+ } else {
+ node->add_child(
+ goal,
+ this->cost(node, goal)
+ );
+ if (this->collide(node, goal)) {
+ node->children().pop_back();
+ goal->remove_parent();
+ return false;
+ }
+ this->goal_found_ = true;
+ // Update ccost of goal's children
+ goal->update_ccost();
+ // Update ccost of goals
+ for (auto g: this->goals()) {
+ if (g == goal)
+ break;
+ g->ccost(-1);
+ }
return true;
}
}
ch_cost = dnodes[tmp.ni].c +
this->cost(cusps[tmp.ni], cusps[i]);
steered = this->steer(cusps[tmp.ni], cusps[i]);
+ if (steered.size() <= 0)
+ break;
for (unsigned int j = 0; j < steered.size() - 1; j++)
steered[j]->add_child(steered[j + 1], 1);
if (this->collide(
return false;
std::vector<RRTNode *> cusps;
for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
- if (tmp_cusps[i] != tmp_cusps[(i + 1) % tmp_cusps.size()])
+ if (!IS_NEAR(
+ tmp_cusps[i],
+ tmp_cusps[(i + 1) % tmp_cusps.size()]
+ ))
cusps.push_back(tmp_cusps[i]);
}
std::reverse(cusps.begin(), cusps.end());
}
if (to_del < this->nodes_.size())
this->nodes_.erase(this->nodes_.begin() + to_del);
- iy = IYI(tmp->y());
+ iy = this->YI(tmp);
to_del = this->iy_[iy].size();
#pragma omp parallel for reduction(min: to_del)
for (i = 0; i < this->iy_[iy].size(); i++) {
std::vector<RRTNode *> RRTBase::findt()
{
- return this->findt(this->goal_);
+ RRTNode *goal = this->goal_;
+ for (auto g: this->goals()) {
+ if (goal->parent() == nullptr || g->ccost() < g->ccost())
+ goal = g;
+ }
+ if (goal->parent() == nullptr)
+ this->goal_found(false);
+ return this->findt(goal);
}
std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
return nodes;
}
+int RRTBase::XI(RRTNode *n)
+{
+ float step = (this->HMAX - this->HMIN) / IXSIZE;
+ float index = (int) (floor(n->x() - this->HMIN) / step);
+ if (index < 0) index = 0;
+ if (index >= IXSIZE) index = IXSIZE - 1;
+ return index;
+}
+
+int RRTBase::YI(RRTNode *n)
+{
+ float step = (this->VMAX - this->VMIN) / IYSIZE;
+ float index = (int) (floor(n->y() - this->VMIN) / step);
+ if (index < 0) index = 0;
+ if (index >= IYSIZE) index = IYSIZE - 1;
+ return index;
+}
+
// RRT Framework
+void RRTBase::defaultSamplingInfo()
+{
+ float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+ float hrange = (this->HMAX - this->HMIN) / 2;
+ float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+ float vrange = (this->VMAX - this->VMIN) / 2;
+ this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+ this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+ this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
+}
+
+void RRTBase::setSamplingInfo(SamplingInfo si)
+{
+ this->ndx_ = std::normal_distribution<float>(si.x0, si.x);
+ this->ndy_ = std::normal_distribution<float>(si.y0, si.y);
+ this->ndh_ = std::normal_distribution<float>(si.h0, si.h);
+}
+
RRTNode *RRTBase::sample()
{
- if (this->useSamplingInfo_) {
- float sar = static_cast<float>(rand());
- sar /= static_cast<float>(RAND_MAX / this->samplingInfo_.r);
- sar += this->samplingInfo_.mr;
- float sah = static_cast<float>(rand());
- sah /= static_cast<float>(RAND_MAX / this->samplingInfo_.h);
- sah *= this->samplingInfo_.dh;
- sah += this->samplingInfo_.sh;
- float h = static_cast<float>(rand());
- h /= static_cast<float>(RAND_MAX / this->samplingInfo_.mh);
- h += this->samplingInfo_.mmh;
- h *= this->samplingInfo_.dh;
- h += this->samplingInfo_.sh;
- return new RRTNode(
- this->samplingInfo_.x + sar * cos(sah),
- this->samplingInfo_.y + sar * sin(sah),
- h
- );
- } else {
- return sa1();
- }
+ float x = this->ndx_(this->gen_);
+ float y = this->ndy_(this->gen_);
+ float h = this->ndh_(this->gen_);
+ return new RRTNode(x, y, h);
}
float RRTBase::cost(RRTNode *init, RRTNode *goal)
RRTNode *RRTBase::nn(RRTNode *rs)
{
- return nn4(this->iy_, rs, nullptr);
- //return nn3(this->iy_, rs, nullptr);
+ int iy = this->YI(rs);
+ float iy_step = (this->VMAX - this->VMIN) / IYSIZE;
+ struct mcnn nn;
+ nn.nn = nullptr;
+ nn.mc = 9999;
+ unsigned int i = 0; // vector step
+ unsigned int j = 0; // array step
+ int iyj = 0;
+ while (nn.mc > j * iy_step) {
+ iyj = (int) (iy + j);
+ if (iyj >= IYSIZE)
+ iyj = IYSIZE - 1;
+ #pragma omp parallel for reduction(minn: nn)
+ for (i = 0; i < this->iy_[iyj].size(); i++) {
+ if (EDIST(this->iy_[iyj][i], rs) < nn.mc) {
+ nn.mc = EDIST(this->iy_[iyj][i], rs);
+ nn.nn = this->iy_[iyj][i];
+ }
+ }
+ if (j > 0) {
+ iyj = (int) (iy - j);
+ if (iyj < 0)
+ iyj = 0;
+ #pragma omp parallel for reduction(minn: nn)
+ for (i = 0; i < this->iy_[iyj].size(); i++) {
+ if (EDIST(this->iy_[iyj][i], rs) < nn.mc) {
+ nn.mc = EDIST(this->iy_[iyj][i], rs);
+ nn.nn = this->iy_[iyj][i];
+ }
+ }
+ }
+ j++;
+ }
+ return nn.nn;
}
std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)
{
std::vector<RRTNode *> nvs;
- unsigned int iy = IYI(node->y());
- unsigned int iy_dist = floor(dist / IYSTEP) + 1;
+ unsigned int iy = this->YI(node);
+ float iy_step = (this->VMAX - this->VMIN) / IYSIZE;
+ unsigned int iy_dist = floor(dist / iy_step) + 1;
unsigned int i = 0; // vector index
unsigned int j = 0; // array index
unsigned int jmin = 0; // minimal j index
projects / hubacji1 / iamcar.git / blobdiff