#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 "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()
{
// 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;
-// for (auto n: this->dnodes_)
-// if (n != this->root_ && n != this->goal_)
-// delete n;
-// for (auto s: this->samples_)
-// if (s != this->goal_)
-// delete s;
-// for (auto edges: this->rlog_)
-// for (auto e: edges)
-// delete e;
-// delete this->root_;
-// delete this->goal_;
-}
-
-RRTBase::RRTBase():
- root_(new RRTNode()),
- goal_(new RRTNode())
-{
- this->nodes_.reserve(20000);
+ for (auto n: this->nodes_)
+ if (n != this->root_)
+ delete n;
+ for (auto n: this->dnodes_)
+ if (n != this->root_ && n != this->goal_)
+ delete n;
+ for (auto s: this->samples_)
+ if (s != this->goal_)
+ delete s;
+ for (auto edges: this->rlog_)
+ for (auto e: edges)
+ delete e;
+ delete this->root_;
+ delete this->goal_;
+}
+
+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(20000);
+ this->nodes_.reserve(NOFNODES);
this->nodes_.push_back(init);
this->add_iy(init);
+ this->add_ixy(init);
}
// 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_;
+}
+
std::vector<RRTNode *> &RRTBase::samples()
{
return this->samples_;
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->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 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);
SDL_GL_SwapWindow(gw);
for (auto n: r)
n->visit(false);
+#endif
return true;
}
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;
}
}
} else {
this->nodes().push_back(ns);
this->add_iy(ns);
+ this->add_ixy(ns);
pn->add_child(ns, this->cost(pn, ns));
pn = ns;
}
// RRT Framework
RRTNode *RRTBase::sample()
{
- return sa1();
+ 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)
projects / hubacji1 / iamcar.git / blobdiff