}
void
-RRTS::recompute_cc(RRTNode* g)
+RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
{
- this->path_.clear();
+ assert(this->_path.size() == 0);
while (g != nullptr) {
- this->path_.push_back(g);
+ this->_path.push_back(g);
g = g->p();
}
- std::reverse(this->path_.begin(), this->path_.end());
- for (unsigned int i = 1; i < this->path_.size(); i++) {
- this->path_[i]->c(this->cost_build(*this->path_[i - 1],
- *this->path_[i]));
+ std::reverse(this->_path.begin(), this->_path.end());
+ for (unsigned int i = 1; i < this->_path.size(); i++) {
+ this->_path[i]->c(this->cost_build(
+ *this->_path[i - 1],
+ *this->_path[i]));
}
+ this->_path.clear();
}
void
RRTS::recompute_path_cc()
{
- this->recompute_cc(&this->goal_);
+ this->recompute_cc_for_predecessors_and(&this->_goal);
}
double
RRTS::min_gamma_eta() const
{
- double ns = this->nodes_.size();
+ double ns = this->_nodes.size();
double gamma = pow(log(ns) / ns, 1.0 / 3.0);
- return std::min(gamma, this->eta_);
+ return std::min(gamma, this->eta());
}
bool
void
RRTS::join_steered(RRTNode* f)
{
- while (this->steered_.size() > 0) {
- this->store(this->steered_.front());
- RRTNode* t = &this->nodes_.back();
+ while (this->_steered.size() > 0) {
+ this->store(this->_steered.front());
+ RRTNode* t = &this->_nodes.back();
t->p(*f);
t->c(this->cost_build(*f, *t));
t->cusp(*f);
- this->steered_.erase(this->steered_.begin());
+ this->_steered.erase(this->_steered.begin());
f = t;
}
}
-RRTNode&
-RRTS::nn()
-{
- return *this->nn_;
-}
-
bool
RRTS::connect()
{
- RRTNode* f = this->nn_;
- RRTNode* t = &this->steered_.front();
+ RRTNode* f = this->_nn;
+ RRTNode* t = &this->_steered.front();
#if USE_RRTS
double cost = f->cc() + this->cost_build(*f, *t);
for (auto n: this->nv_) {
}
}
// Check if it's possible to drive from *f to *t. If not, then fallback
- // to *f = nn_. This could be also solved by additional steer from *f to
+ // to *f = _nn. This could be also solved by additional steer from *f to
// *t instead of the following code.
- this->bc_.set_pose(*f);
- if (!this->bc_.drivable(*t)) {
- f = this->nn_;
+ this->set_bc_pose_to(*f);
+ if (!this->_bc.drivable(*t)) {
+ f = this->_nn;
}
#endif
- this->store(this->steered_.front());
- t = &this->nodes_.back();
+ this->store(this->_steered.front());
+ t = &this->_nodes.back();
t->p(*f);
t->c(this->cost_build(*f, *t));
t->cusp(*f);
- this->steered_.erase(this->steered_.begin());
+ this->_steered.erase(this->_steered.begin());
return true;
}
void
RRTS::rewire()
{
- RRTNode *f = &this->nodes_.back();
- for (auto n: this->nv_) {
+ RRTNode *f = &this->_nodes.back();
+ for (auto n: this->_nv) {
double fc = f->cc() + this->cost_build(*f, *n);
- this->bc_.set_pose(*f);
- bool drivable = this->bc_.drivable(*n);
+ this->set_bc_pose_to(*f);
+ bool drivable = this->_bc.drivable(*n);
if (drivable && fc < n->cc()) {
n->p(*f);
n->c(this->cost_build(*f, *n));
bool
RRTS::goal_drivable_from(RRTNode const& f)
{
- this->bc_.set_pose(f);
- return this->bc_.drivable(this->goal_);
+ this->set_bc_pose_to(f);
+ return this->_bc.drivable(this->_goal);
}
void
RRTS::store(RRTNode n)
{
- this->nodes_.push_back(n);
+ this->_nodes.push_back(n);
}
double
void
RRTS::find_nn(RRTNode const& t)
{
- this->nn_ = &this->nodes_.front();
- this->cost_ = this->cost_search(*this->nn_, t);
- for (auto& f: this->nodes_) {
- if (this->cost_search(f, t) < this->cost_) {
- this->nn_ = &f;
- this->cost_ = this->cost_search(f, t);
+ this->_nn = &this->_nodes.front();
+ this->_cost = this->cost_search(*this->_nn, t);
+ for (auto& f: this->_nodes) {
+ if (this->cost_search(f, t) < this->_cost) {
+ this->_nn = &f;
+ this->_cost = this->cost_search(f, t);
}
}
}
void
RRTS::find_nv(RRTNode const& t)
{
- this->nv_.clear();
- this->cost_ = this->min_gamma_eta();
- for (auto& f: this->nodes_) {
- if (this->cost_search(f, t) < this->cost_) {
- this->nv_.push_back(&f);
+ this->_nv.clear();
+ this->_cost = this->min_gamma_eta();
+ for (auto& f: this->_nodes) {
+ if (this->cost_search(f, t) < this->_cost) {
+ this->_nv.push_back(&f);
}
}
}
void
RRTS::compute_path()
{
- this->path_.clear();
- RRTNode *g = &this->goal_;
+ this->_path.clear();
+ RRTNode *g = &this->_goal;
if (g->p() == nullptr) {
return;
}
- while (g != nullptr && this->path_.size() < 10000) {
+ while (g != nullptr && this->_path.size() < 10000) {
/* FIXME in ext13
*
- * There shouldn't be this->path_.size() < 10000 condition.
+ * There shouldn't be this->_path.size() < 10000 condition.
* However, the RRTS::compute_path() called from
- * RRTExt13::compute_path tends to re-allocate this->path_
+ * RRTExt13::compute_path tends to re-allocate this->_path
* infinitely. There's probably node->p() = &node somewhere...
*/
- this->path_.push_back(g);
+ this->_path.push_back(g);
g = g->p();
}
- std::reverse(this->path_.begin(), this->path_.end());
+ std::reverse(this->_path.begin(), this->_path.end());
}
-RRTS::RRTS() : goal_(0.0, 0.0, 0.0, 0.0), gen_(std::random_device{}())
+RRTS::RRTS() : _goal(0.0, 0.0, 0.0, 0.0), _gen(std::random_device{}())
{
- this->nodes_.reserve(4000000);
- this->steered_.reserve(1000);
- this->path_.reserve(10000);
- this->nv_.reserve(1000);
+ this->_nodes.reserve(4000000);
+ this->_steered.reserve(1000);
+ this->_path.reserve(10000);
+ this->_nv.reserve(1000);
this->store(RRTNode()); // root
}
-BicycleCar &
-RRTS::bc()
+void
+RRTS::set_bc_pose_to(Pose const& p)
+{
+ this->_bc.set_pose_to(p);
+}
+
+RRTGoal const&
+RRTS::goal(void) const
{
- return this->bc_;
+ return this->_goal;
}
void
-RRTS::set_imax_reset(unsigned int i)
+RRTS::goal(double x, double y, double b, double e)
+{
+ this->_goal = RRTGoal(x, y, b, e);
+}
+
+unsigned int
+RRTS::icnt(void) const
{
- this->_imax = i;
+ return this->_icnt;
}
void
-RRTS::set_goal(double x, double y, double b, double e)
+RRTS::icnt(unsigned int i)
+{
+ this->_icnt = i;
+}
+
+unsigned int
+RRTS::icnt_max(void) const
{
- this->goal_ = RRTGoal(x, y, b, e);
+ return this->_icnt_max;
}
void
-RRTS::set_start(double x, double y, double h)
+RRTS::icnt_max(unsigned int i)
{
- this->nodes_.front().x(x);
- this->nodes_.front().y(y);
- this->nodes_.front().h(h);
+ this->_icnt_max = i;
}
-std::vector<Pose>
-RRTS::get_path() const
+void
+RRTS::tstart(void)
{
- std::vector<Pose> path;
- for (auto n: this->path_) {
- path.push_back(Pose(n->x(), n->y(), n->h()));
- }
- return path;
+ this->_ter.start();
}
double
-RRTS::get_path_cost() const
+RRTS::scnt() const
{
- return this->goal_.cc();
+ return this->_ter.scnt();
}
-unsigned int
-RRTS::icnt() const
+void
+RRTS::start(double x, double y, double h)
{
- return this->icnt_;
+ this->_nodes.front().x(x);
+ this->_nodes.front().y(y);
+ this->_nodes.front().h(h);
}
-void
-RRTS::icnt(unsigned int i)
+std::vector<Pose>
+RRTS::path() const
+{
+ std::vector<Pose> path;
+ for (auto n: this->_path) {
+ path.push_back(Pose(n->x(), n->y(), n->h()));
+ }
+ return path;
+}
+
+double
+RRTS::path_cost() const
{
- this->icnt_ = i;
+ return this->_goal.cc();
}
double
-RRTS::scnt() const
+RRTS::last_path_cost(void) const
{
- return this->ter_.scnt();
+ if (this->_last_path.size() == 0) {
+ return 999.9;
+ } else {
+ return this->_last_path.back().cc();
+ }
}
double
RRTS::eta() const
{
- return this->eta_;
+ return this->_eta;
}
void
RRTS::eta(double e)
{
- this->eta_ = e;
+ this->_eta = e;
}
Json::Value
{
Json::Value jvo;
unsigned int i = 0;
- for (auto n: this->path_) {
+ for (auto n: this->_path) { // TODO because path() has just x, y, h
jvo["path"][i][0] = n->x();
jvo["path"][i][1] = n->y();
jvo["path"][i][2] = n->h();
jvo["path"][i][4] = n->st();
i++;
}
- jvo["goal_cc"] = this->goal_.cc();
- jvo["time"] = this->time_;
+ jvo["goal_cc"] = this->_goal.cc();
+ jvo["time"] = this->scnt();
return jvo;
}
{
assert(jvi["init"] != Json::nullValue);
assert(jvi["goal"] != Json::nullValue);
- this->nodes_.front().x(jvi["init"][0].asDouble());
- this->nodes_.front().y(jvi["init"][1].asDouble());
- this->nodes_.front().h(jvi["init"][2].asDouble());
+ this->start(
+ jvi["init"][0].asDouble(),
+ jvi["init"][1].asDouble(),
+ jvi["init"][2].asDouble());
if (jvi["goal"].size() == 4) {
- this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+ this->goal(
+ jvi["goal"][0].asDouble(),
jvi["goal"][1].asDouble(),
jvi["goal"][2].asDouble(),
jvi["goal"][3].asDouble());
} else {
- this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+ this->goal(
+ jvi["goal"][0].asDouble(),
jvi["goal"][1].asDouble(),
jvi["goal"][2].asDouble(),
jvi["goal"][2].asDouble());
bool
RRTS::next()
{
- if (this->icnt_ == 0) {
- this->ter_.start();
+ if (this->icnt() == 0) {
+ this->tstart();
}
- this->icnt_ += 1;
auto rs = this->sample();
#if 1 // anytime RRTs
{
- double d1 = this->cost_search(this->nodes_.front(), rs);
- double d2 = this->cost_search(rs, this->goal_);
- if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
- rs = this->last_path_[rand() % this->last_path_.size()];
+ double d1 = this->cost_search(this->_nodes.front(), rs);
+ double d2 = this->cost_search(rs, this->_goal);
+ if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
+ rs = this->_last_path[rand() % this->_last_path.size()];
}
}
#endif
this->find_nn(rs);
- this->steer(this->nn(), rs);
+ this->steer(*this->_nn, rs);
if (this->collide_steered()) {
return this->should_continue();
}
#if USE_RRTS
- this->find_nv(this->steered_.front());
+ this->find_nv(this->_steered.front());
#endif
if (!this->connect()) {
return this->should_continue();
#if USE_RRTS
this->rewire();
#endif
- unsigned int ss = this->steered_.size();
- this->join_steered(&this->nodes_.back());
- RRTNode* just_added = &this->nodes_.back();
+ unsigned int ss = this->_steered.size();
+ this->join_steered(&this->_nodes.back());
+ RRTNode* just_added = &this->_nodes.back();
bool gf = false;
while (ss > 0 && just_added->p() != nullptr) {
- this->steer(*just_added, this->goal_);
+ this->steer(*just_added, this->_goal);
if (this->collide_steered()) {
ss--;
just_added = just_added->p();
continue;
}
this->join_steered(just_added);
- bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
- bool gd = this->goal_drivable_from(this->nodes_.back());
+ bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
+ bool gd = this->goal_drivable_from(this->_nodes.back());
if (gn && gd) {
- double nc = this->cost_build(this->nodes_.back(),
- this->goal_);
- double ncc = this->nodes_.back().cc() + nc;
- if (this->goal_.p() == nullptr
- || ncc < this->goal_.cc()) {
- this->goal_.p(this->nodes_.back());
- this->goal_.c(nc);
+ double nc = this->cost_build(
+ this->_nodes.back(),
+ this->_goal);
+ double ncc = this->_nodes.back().cc() + nc;
+ if (this->_goal.p() == nullptr
+ || ncc < this->_goal.cc()) {
+ this->_goal.p(this->_nodes.back());
+ this->_goal.c(nc);
gf = true;
}
}
if (gf) {
this->compute_path();
}
- this->time_ = this->ter_.scnt();
+ this->_time = this->scnt();
+ this->icnt(this->icnt() + 1);
return this->should_continue();
}
void
RRTS::reset()
{
- if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
- this->last_goal_cc_ = this->goal_.cc();
- this->last_path_.clear();
- for (auto n: this->path_) {
- this->last_path_.push_back(*n);
+ if (this->path_cost() != 0.0
+ && this->path_cost() < this->last_path_cost()) {
+ this->_last_path.clear();
+ for (auto n: this->_path) {
+ this->_last_path.push_back(*n);
+ // FIXME _last_path nodes' pointers to parents
}
}
- this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
- this->goal_.e());
- this->path_.clear();
- this->steered_.clear();
- this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());
- this->nv_.clear();
- this->nn_ = nullptr;
- this->bc_.x(0);
- this->bc_.y(0);
- this->bc_.h(0);
+ this->_goal = RRTGoal(
+ this->_goal.x(),
+ this->_goal.y(),
+ this->_goal.b(),
+ this->_goal.e());
+ this->_path.clear();
+ this->_steered.clear();
+ this->_nodes.erase(this->_nodes.begin() + 1, this->_nodes.end());
+ this->_nv.clear();
+ this->_nn = nullptr;
+ this->_bc = BicycleCar();
}
} // namespace rrts
projects / hubacji1 / iamcar2.git / blobdiff