along with I am car. If not, see <http://www.gnu.org/licenses/>.
*/
+#include <algorithm>
#include <cmath>
#include <omp.h>
+#include <queue>
#include "bcar.h"
#include "rrtbase.h"
+
+#if USE_GL > 0
// OpenGL
#include <GL/gl.h>
#include <GL/glu.h>
#include <SDL2/SDL.h>
+#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;
extern SDL_GLContext gc;
+#endif
+
+Cell::Cell()
+{
+ pthread_mutex_init(&this->m_, NULL);
+}
+
+bool Cell::changed()
+{
+ pthread_mutex_lock(&this->m_);
+ bool ret = this->changed_;
+ pthread_mutex_unlock(&this->m_);
+ return ret;
+}
+
+std::vector<RRTNode *> Cell::nodes()
+{
+ pthread_mutex_lock(&this->m_);
+ std::vector<RRTNode *> ret(this->nodes_);
+ pthread_mutex_unlock(&this->m_);
+ return ret;
+}
+
+void Cell::add_node(RRTNode *n)
+{
+ pthread_mutex_lock(&this->m_);
+ this->nodes_.push_back(n);
+ this->changed_ = true;
+ pthread_mutex_unlock(&this->m_);
+}
RRTBase::~RRTBase()
{
+// Fix heap-use-after-free error when T3 planner is used. If only T2 is used,
+// please uncommend the following code:
+//
for (auto n: this->nodes_)
if (n != this->root_)
delete n;
delete this->goal_;
}
-RRTBase::RRTBase():
- root_(new RRTNode()),
- goal_(new RRTNode())
+RRTBase::RRTBase()
+ : root_(new RRTNode())
+ , goal_(new RRTNode())
+ , gen_(std::random_device{}())
+ , ndx_(HMAX - HMIN, (HMAX - HMIN) / 4)
+ , ndy_(VMAX - VMIN, (VMAX - VMIN) / 4)
+ , ndh_(0, M_PI * 2 / 4)
{
+ this->nodes_.reserve(NOFNODES);
this->nodes_.push_back(this->root_);
this->add_iy(this->root_);
+ this->add_ixy(this->root_);
}
-RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
- root_(init),
- goal_(goal)
+RRTBase::RRTBase(RRTNode *init, RRTNode *goal)
+ : root_(init)
+ , goal_(goal)
+ , gen_(std::random_device{}())
+ , ndx_(HMIN + (HMAX - HMIN) / 2, (HMAX - HMIN) / 4)
+ , ndy_(VMIN + (VMAX - VMIN) / 2, (VMAX - VMIN) / 4)
+ , ndh_(0, M_PI * 2 / 4)
{
+ this->nodes_.reserve(NOFNODES);
this->nodes_.push_back(init);
this->add_iy(init);
+ this->add_ixy(init);
}
+// getter
RRTNode *RRTBase::root()
{
return this->root_;
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_;
+}
+
std::vector<RRTNode *> &RRTBase::samples()
{
return this->samples_;
}
-std::vector<CircleObstacle> *RRTBase::cos()
+std::vector<CircleObstacle> *RRTBase::co()
{
return this->cobstacles_;
}
-std::vector<SegmentObstacle> *RRTBase::sos()
+std::vector<SegmentObstacle> *RRTBase::so()
{
return this->sobstacles_;
}
return this->tlog_;
}
+std::vector<RRTNode *> &RRTBase::slot_cusp()
+{
+ return this->slot_cusp_;
+}
+
bool RRTBase::goal_found()
{
return this->goal_found_;
return dt.count();
}
+std::vector<RRTNode *> RRTBase::traj_cusp()
+{
+ std::vector<RRTNode *> tmp_cusps;
+ for (auto n: this->tlog().back()) {
+ if (sgn(n->s()) == 0) {
+ tmp_cusps.push_back(n);
+ } else if (n->parent() &&
+ sgn(n->s()) != sgn(n->parent()->s())) {
+ tmp_cusps.push_back(n);
+ tmp_cusps.push_back(n->parent());
+ }
+ }
+ 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()])
+ cusps.push_back(tmp_cusps[i]);
+ }
+ return cusps;
+}
+
+// setter
+void RRTBase::root(RRTNode *node)
+{
+ this->root_ = node;
+}
+
+void RRTBase::goal(RRTNode *node)
+{
+ 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
return true;
}
+bool RRTBase::add_ixy(RRTNode *n)
+{
+ int ix = IXI(n->x());
+ if (ix < 0)
+ ix = 0;
+ if (ix >= IXSIZE)
+ ix = IXSIZE - 1;
+ int iy = IYI(n->y());
+ if (iy < 0)
+ iy = 0;
+ if (iy >= IYSIZE)
+ iy = IYSIZE - 1;
+ this->ixy_[ix][iy].add_node(n);
+ return true;
+}
+
+bool RRTBase::goal_found(bool f)
+{
+ this->goal_found_ = f;
+ return f;
+}
+
+void RRTBase::slot_cusp(std::vector<RRTNode *> sc)
+{
+ for (unsigned int i = 0; i < sc.size() - 1; i++)
+ sc[i]->add_child(sc[i + 1], this->cost(sc[i], sc[i + 1]));
+ sc[0]->parent(this->goal());
+ this->slot_cusp_ = sc;
+}
+
+// other
bool RRTBase::glplot()
{
+#if USE_GL > 0
glClear(GL_COLOR_BUFFER_BIT);
glLineWidth(1);
glPointSize(1);
glVertex2f(GLVERTEX(this->goal_));
glEnd();
// Plot last sample
- glPointSize(8);
- glBegin(GL_POINTS);
- glColor3f(0, 1, 0);
- glVertex2f(GLVERTEX(this->samples_.back()));
- glEnd();
+ if (this->samples_.size() > 0) {
+ glPointSize(8);
+ glBegin(GL_POINTS);
+ glColor3f(0, 1, 0);
+ glVertex2f(GLVERTEX(this->samples_.back()));
+ glEnd();
+ }
// Plot nodes
std::vector<RRTNode *> s; // DFS stack
std::vector<RRTNode *> r; // reset visited_
}
}
glEnd();
+ // Plot nodes (from goal)
+ glBegin(GL_LINES);
+ s.push_back(this->goal_);
+ 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);
+ glVertex2f(GLVERTEX(tmp));
+ glVertex2f(GLVERTEX(ch));
+ }
+ }
+ }
+ glEnd();
+ std::vector<RRTNode *> cusps;
// Plot last trajectory
if (this->tlog().size() > 0) {
glLineWidth(2);
glColor3f(0, 0, 1);
glVertex2f(GLVERTEX(n));
glVertex2f(GLVERTEX(n->parent()));
+ if (sgn(n->s()) != sgn(n->parent()->s()))
+ cusps.push_back(n);
}
}
glEnd();
}
+ // Plot cusps
+ glPointSize(8);
+ glBegin(GL_POINTS);
+ for (auto n: cusps) {
+ glColor3f(0, 0, 1);
+ glVertex2f(GLVERTEX(n));
+ }
+ glEnd();
SDL_GL_SwapWindow(gw);
for (auto n: r)
n->visit(false);
+#endif
return true;
}
RRTNode *node,
float (*cost)(RRTNode *, RRTNode* ))
{
- 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) {
+ if (IS_NEAR(node, this->goal_)) {
if (this->goal_found_) {
- if (node->ccost() + (*cost)(node, this->goal_) <
+ if (node->ccost() + this->cost(node, this->goal_) <
this->goal_->ccost()) {
RRTNode *op; // old parent
float oc; // old cumulative cost
oc = this->goal_->ccost();
od = this->goal_->dcost();
node->add_child(this->goal_,
- (*cost)(node, this->goal_));
+ this->cost(node, this->goal_));
if (this->collide(node, this->goal_)) {
node->children().pop_back();
this->goal_->parent(op);
} else {
node->add_child(
this->goal_,
- (*cost)(node, this->goal_));
+ this->cost(node, this->goal_));
if (this->collide(node, this->goal_)) {
node->children().pop_back();
this->goal_->remove_parent();
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 (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;
}
}
if ((*this->cobstacles_)[i].collide(tmp)) {
col = true;
}
- // TODO collide with car frame
+ for (auto &e: bcframe) {
+ if ((*this->cobstacles_)[i].collide(e)) {
+ col = true;
+ }
+ }
}
if (col) {
for (auto e: bcframe) {
return false;
}
+class RRTNodeDijkstra {
+ public:
+ RRTNodeDijkstra(int i):
+ ni(i),
+ pi(0),
+ c(9999),
+ v(false)
+ {};
+ RRTNodeDijkstra(int i, float c):
+ ni(i),
+ pi(0),
+ c(c),
+ v(false)
+ {};
+ RRTNodeDijkstra(int i, int p, float c):
+ ni(i),
+ pi(p),
+ c(c),
+ v(false)
+ {};
+ unsigned int ni;
+ unsigned int pi;
+ float c;
+ bool v;
+ bool vi()
+ {
+ if (this->v)
+ return true;
+ this->v = true;
+ return false;
+ };
+};
+
+class RRTNodeDijkstraComparator {
+ public:
+ int operator() (
+ const RRTNodeDijkstra& n1,
+ const RRTNodeDijkstra& n2)
+ {
+ return n1.c > n2.c;
+ }
+};
+
+bool RRTBase::optp_dijkstra(
+ std::vector<RRTNode *> &cusps,
+ std::vector<int> &npi)
+{
+ std::vector<RRTNodeDijkstra> dnodes;
+ for (unsigned int i = 0; i < cusps.size(); i++)
+ if (i > 0)
+ dnodes.push_back(RRTNodeDijkstra(
+ i,
+ i - 1,
+ cusps[i]->ccost()));
+ else
+ dnodes.push_back(RRTNodeDijkstra(
+ i,
+ cusps[i]->ccost()));
+ dnodes[0].vi();
+ std::priority_queue<
+ RRTNodeDijkstra,
+ std::vector<RRTNodeDijkstra>,
+ RRTNodeDijkstraComparator> pq;
+ RRTNodeDijkstra tmp = dnodes[0];
+ pq.push(tmp);
+ float ch_cost = 9999;
+ std::vector<RRTNode *> steered;
+ while (!pq.empty()) {
+ tmp = pq.top();
+ pq.pop();
+ for (unsigned int i = tmp.ni + 1; i < cusps.size(); i++) {
+ ch_cost = dnodes[tmp.ni].c +
+ this->cost(cusps[tmp.ni], cusps[i]);
+ steered = this->steer(cusps[tmp.ni], cusps[i]);
+ for (unsigned int j = 0; j < steered.size() - 1; j++)
+ steered[j]->add_child(steered[j + 1], 1);
+ if (this->collide(
+ steered[0],
+ steered[steered.size() - 1])) {
+ for (auto n: steered)
+ delete n;
+ continue;
+ }
+ if (ch_cost < dnodes[i].c) {
+ dnodes[i].c = ch_cost;
+ dnodes[i].pi = tmp.ni;
+ if (!dnodes[i].vi())
+ pq.push(dnodes[i]);
+ }
+ for (auto n: steered)
+ delete n;
+ }
+ }
+ unsigned int tmpi = 0;
+ for (auto n: dnodes) {
+ if (n.v && n.ni > tmpi)
+ tmpi = n.ni;
+ }
+ while (tmpi > 0) {
+ npi.push_back(tmpi);
+ tmpi = dnodes[tmpi].pi;
+ }
+ npi.push_back(tmpi);
+ std::reverse(npi.begin(), npi.end());
+ return true;
+}
+
+bool RRTBase::optp_rrp(
+ std::vector<RRTNode *> &cusps,
+ std::vector<int> &npi)
+{
+ std::vector<RRTNode *> steered;
+ std::vector<int> candidates;
+ RRTNode *x_j = nullptr;
+ RRTNode *x_i = nullptr;
+ int j = cusps.size() - 1;
+ int i_min = 0;
+ float c_min = 0;
+ float cost = 0;
+ float dx = 0;
+ float dy = 0;
+ float ed = 0;
+ float th_i = 0;
+ float th_j = 0;
+ while (j > 0) {
+ npi.push_back(j);
+ steered.clear();
+ candidates.clear();
+ x_j = cusps[j];
+ for (int i = 0; i < j; i++) {
+ steered = this->steer(cusps[i], x_j);
+ for (unsigned int k = 0; k < steered.size() - 1; k++)
+ steered[k]->add_child(steered[k + 1], 1);
+ if (!this->collide(
+ steered[0],
+ steered[steered.size() - 1]))
+ candidates.push_back(i);
+ }
+ if (candidates.size() <= 0)
+ return false;
+ i_min = candidates[0];
+ x_i = cusps[i_min];
+ c_min = 9999;
+ for (auto c: candidates) {
+ x_i = cusps[c];
+ dx = x_j->x() - x_i->x();
+ dy = x_j->y() - x_i->y();
+ ed = EDIST(x_i, x_j);
+ th_i = (cos(x_i->h()) * dx + sin(x_i->h()) * dy) / ed;
+ th_j = (cos(x_j->h()) * dx + sin(x_j->h()) * dy) / ed;
+ cost = th_i + th_j;
+ if (cost < c_min) {
+ i_min = c;
+ c_min = cost;
+ }
+ }
+ j = i_min;
+
+ }
+ npi.push_back(j);
+ std::reverse(npi.begin(), npi.end());
+ return true;
+}
+
+bool RRTBase::optp_smart(
+ std::vector<RRTNode *> &cusps,
+ std::vector<int> &npi)
+{
+ std::vector<RRTNode *> steered;
+ int li = cusps.size() - 1;
+ int ai = li - 1;
+ npi.push_back(li);
+ while (ai > 1) {
+ steered = this->steer(cusps[ai - 1], cusps[li]);
+ for (unsigned int j = 0; j < steered.size() - 1; j++)
+ steered[j]->add_child(steered[j + 1], 1);
+ if (this->collide(steered[0], steered[steered.size() - 1])) {
+ npi.push_back(ai);
+ li = ai;
+ }
+ ai--;
+ for (auto n: steered)
+ delete n;
+ }
+ npi.push_back(0);
+ std::reverse(npi.begin(), npi.end());
+ return true;
+}
+
+bool RRTBase::opt_path()
+{
+ if (this->tlog().size() == 0)
+ return false;
+ float oc = this->tlog().back().front()->ccost();
+ std::vector<RRTNode *> tmp_cusps;
+ for (auto n: this->tlog().back()) {
+ if (sgn(n->s()) == 0) {
+ tmp_cusps.push_back(n);
+ } else if (n->parent() &&
+ sgn(n->s()) != sgn(n->parent()->s())) {
+ tmp_cusps.push_back(n);
+ tmp_cusps.push_back(n->parent());
+ }
+ //tmp_cusps.push_back(n);
+ }
+ if (tmp_cusps.size() < 2)
+ 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()])
+ cusps.push_back(tmp_cusps[i]);
+ }
+ std::reverse(cusps.begin(), cusps.end());
+ std::vector<int> npi; // new path indexes
+ if (!this->optp_dijkstra(cusps, npi))
+ return false;
+ RRTNode *pn = cusps[npi[0]];
+ RRTNode *tmp = nullptr;
+ bool en_add = true;
+ for (unsigned int i = 0; i < npi.size() - 1; i++) {
+ pn = cusps[npi[i]];
+ for (auto ns: this->steer(cusps[npi[i]], cusps[npi[i + 1]])) {
+ if (!en_add) {
+ delete ns;
+ } else if (IS_NEAR(cusps[npi[i]], ns)) {
+ tmp = ns;
+ while (tmp && tmp != cusps[npi[i]]) {
+ pn = tmp->parent();
+ delete tmp;
+ tmp = pn;
+ }
+ pn = cusps[npi[i]];
+ } else if (IS_NEAR(ns, cusps[npi[i + 1]])) {
+ delete ns;
+ cusps[npi[i + 1]]->parent()->rem_child(
+ cusps[npi[i + 1]]);
+ pn->add_child(
+ cusps[npi[i + 1]],
+ this->cost(pn, cusps[npi[i + 1]]));
+ en_add = false;
+ } else if (IS_NEAR(pn, ns)) {
+ delete ns;
+ } else {
+ this->nodes().push_back(ns);
+ this->add_iy(ns);
+ this->add_ixy(ns);
+ pn->add_child(ns, this->cost(pn, ns));
+ pn = ns;
+ }
+ }
+ }
+ this->root()->update_ccost();
+ if (this->tlog().back().front()->ccost() < oc)
+ return true;
+ return false;
+}
+
bool RRTBase::rebase(RRTNode *nr)
{
- if (this->goal_ == nr || this->root_ == nr)
+ if (!nr || this->goal_ == nr || this->root_ == nr)
return false;
std::vector<RRTNode *> s; // DFS stack
RRTNode *tmp;
}
if (to_del < this->nodes_.size())
this->nodes_.erase(this->nodes_.begin() + to_del);
-#if NNVERSION > 1
iy = IYI(tmp->y());
to_del = this->iy_[iy].size();
#pragma omp parallel for reduction(min: to_del)
}
if (to_del < this->iy_[iy].size())
this->iy_[iy].erase(this->iy_[iy].begin() + to_del);
-#endif
this->dnodes().push_back(tmp);
}
this->root_ = nr;
std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
{
std::vector<RRTNode *> nodes;
- if (!n->parent())
+ if (!n || !n->parent())
return nodes;
- RRTNode *tmp = n;
- while (tmp) {
- nodes.push_back(tmp);
- tmp = tmp->parent();
+ while (n) {
+ nodes.push_back(n);
+ n = n->parent();
}
return nodes;
}
+
+// RRT Framework
+RRTNode *RRTBase::sample()
+{
+ if (this->useSamplingInfo_ && this->nodes().size() % 2 == 0) {
+ float x = static_cast<float>(rand());
+ x /= static_cast<float>(RAND_MAX / this->samplingInfo_.x);
+ x -= this->samplingInfo_.x / 2;
+ x += this->samplingInfo_.x0;
+ float y = static_cast<float>(rand());
+ y /= static_cast<float>(RAND_MAX / this->samplingInfo_.y);
+ y -= this->samplingInfo_.y / 2;
+ y += this->samplingInfo_.y0;
+ float h = static_cast<float>(rand());
+ h /= static_cast<float>(RAND_MAX / this->samplingInfo_.h);
+ h -= this->samplingInfo_.h / 2;
+ h += this->samplingInfo_.h0;
+ return new RRTNode(x, y, h);
+ } else {
+ 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)
+{
+ return co2(init, goal);
+}
+
+RRTNode *RRTBase::nn(RRTNode *rs)
+{
+ return nn4(this->iy_, rs, nullptr);
+ //return nn3(this->iy_, rs, nullptr);
+}
+
+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 i = 0; // vector index
+ unsigned int j = 0; // array index
+ unsigned int jmin = 0; // minimal j index
+ unsigned int jmax = 0; // maximal j index
+ jmin = iy - iy_dist;
+ jmin = (jmin > 0) ? jmin : 0;
+ jmax = iy + iy_dist + 1;
+ jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
+ #pragma omp parallel for reduction(merge: nvs)
+ for (j = jmin; j < jmax; j++) {
+ #pragma omp parallel for reduction(merge: nvs)
+ for (i = 0; i < this->iy_[j].size(); i++) {
+ if (this->cost(this->iy_[j][i], node) < dist) {
+ nvs.push_back(this->iy_[j][i]);
+ }
+ }
+ }
+ return nvs;
+}
+
+std::vector<RRTNode *> RRTBase::steer(RRTNode *init, RRTNode *goal)
+{
+ return st3(init, goal);
+}
+
+std::vector<RRTNode *> RRTBase::steer(RRTNode *init, RRTNode *goal, float step)
+{
+ return st3(init, goal, step);
+}
projects / hubacji1 / iamcar.git / blobdiff