KdNode(RRTNode* n);
};
KdNode* kdroot_ = nullptr;
- std::vector<KdNode> kdnodes_;
+ std::vector<KdNode> _kdnodes;
void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
private:
double _c = 0.0;
double _cc = 0.0;
- RRTNode* _p;
+ RRTNode* _p = nullptr;
unsigned int _cusp = 0;
int _segment_type = 0; // 0 ~ straight, 1 ~ left, -1 right
public:
/*! RRT* algorithm basic class. */
class RRTS {
-private:
+protected:
BicycleCar _bc;
RRTGoal _goal;
unsigned int _icnt = 0;
double _cost = 0.0;
double _eta = 0.5;
double _time = 0.0;
- double _last_path_cost = 0.0;
std::vector<RRTNode> _last_path;
protected:
- void recompute_cc(RRTNode* g);
+ double min_gamma_eta(void) const;
+ bool should_continue(void) const;
+ void recompute_cc_for_predecessors_and(RRTNode* g);
void recompute_path_cc();
- double min_gamma_eta() const;
- bool should_continue() const;
void join_steered(RRTNode* f);
- RRTNode& nn();
bool connect();
void rewire();
bool goal_drivable_from(RRTNode const& f);
virtual void find_nn(RRTNode const& t);
virtual void find_nv(RRTNode const& t);
virtual void compute_path();
+protected:
virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
virtual bool collide_steered() = 0;
virtual RRTNode sample() = 0;
public:
RRTS();
- /*! Get bicycle car used in the planner. */
- BicycleCar& bc(void) const;
-
/*! Set pose of the bicycle car used in the planner. */
- void bc(double x, double y, double h);
+ void set_bc_pose_to(Pose const& p);
/*! Get goal. */
- RRTGoal& goal(void) const;
+ RRTGoal const& goal(void) const;
/*! Set goal. */
void goal(double x, double y, double b, double e);
/*! Set maximum number of iterations before reset. */
void icnt_max(unsigned int i);
+ /*! Start elapsed time counter. */
+ void tstart(void);
+
/*! Return elapsed time. */
double scnt() const;
/*! Get cost of the last path. */
double last_path_cost(void) const;
- /*! Get cost of the last path. */
- void last_path_cost(double c);
-
/*! Get eta, the RRT* constant used in near vertices and steering. */
double eta() const;
{
double q0[] = {f.x(), f.y(), f.h()};
double q1[] = {t.x(), t.y(), t.h()};
- ReedsSheppStateSpace rsss(this->bc_.mtr());
+ ReedsSheppStateSpace rsss(this->_bc.mtr());
return rsss.distance(q0, q1);
}
double cost = f.edist(t);
double heur = std::min(std::abs(t.h() - f.h()),
2 * M_PI - std::abs(t.h() - f.h()));
- heur *= this->bc_.mtr();
+ heur *= this->_bc.mtr();
cost = std::max(cost, heur);
return cost + f.cusp() * 0.1;
}
RRTExt13::interesting_forward()
{
this->dn_.clear();
- this->dn_.reserve(this->path_.size());
+ this->dn_.reserve(this->_path.size());
#if 0 // all path poses are interesting
for (auto n: this->opath_) {
this->dn_.push_back(DijkstraNode(n));
this->dn_.back().i = this->dn_.size() - 1;
for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
RRTNode& ni = *this->opath_[i];
- this->bc_.set_pose(ni);
+ this->set_bc_pose_to(ni);
for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
RRTNode& nj = *this->opath_[j];
RRTNode& njl = *this->opath_[j - 1];
- if (!this->bc_.drivable(nj)) {
+ if (!this->_bc.drivable(nj)) {
this->dn_.push_back(DijkstraNode(&njl));
this->dn_.back().i = this->dn_.size() - 1;
i = j;
RRTExt13::interesting_backward()
{
this->dn_.clear();
- this->dn_.reserve(this->path_.size());
+ this->dn_.reserve(this->_path.size());
#if 0 // all path poses are interesting
for (auto n: this->opath_) {
this->dn_.push_back(DijkstraNode(n));
return;
#endif
#if 1 // cusp and 1st non-drivable path poses are interesting
- double goal_cc = this->goal_.cc();
+ double goal_cc = this->_goal.cc();
this->dn_.push_back(DijkstraNode(this->opath_.back()));
this->dn_.back().i = this->dn_.size() - 1;
this->dn_.back().d = goal_cc - this->opath_.back()->cc();
for (unsigned int i = this->opath_.size() - 1; i > 1; i--) {
RRTNode& ni = *this->opath_[i];
- this->bc_.set_pose(ni);
+ this->set_bc_pose_to(ni);
for (unsigned int j = i - 1; j > 0; j--) {
RRTNode& nj = *this->opath_[j];
RRTNode& njl = *this->opath_[j + 1];
- if (!this->bc_.drivable(nj)) {
+ if (!this->_bc.drivable(nj)) {
this->dn_.push_back(DijkstraNode(&njl));
this->dn_.back().i = this->dn_.size() - 1;
this->dn_.back().d = goal_cc - njl.cc();
DijkstraNode fd = pq.top();
RRTNode& f = *fd.node;
pq.pop();
- this->bc_.set_pose(f);
+ this->set_bc_pose_to(f);
for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
RRTNode& t = *this->dn_[i].node;
- if (!this->bc_.drivable(t)) {
+ if (!this->_bc.drivable(t)) {
continue;
}
double cost = f.cc() + this->cost_build(f, t);
this->steer(f, t);
- if (this->steered_.size() == 0) {
+ if (this->_steered.size() == 0) {
break;
}
- unsigned int ss = this->steered_.size();
+ unsigned int ss = this->_steered.size();
if (this->collide_steered()
- || ss != this->steered_.size()) {
+ || ss != this->_steered.size()) {
continue;
}
- this->bc_.set_pose(this->steered_.back());
- bool td = this->bc_.drivable(t);
- bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
+ this->set_bc_pose_to(this->_steered.back());
+ bool td = this->_bc.drivable(t);
+ bool tn = this->_bc.edist(t) < 2.0 * this->_eta;
if (cost < t.cc() && td && tn) {
this->join_steered(&f);
- t.p(this->nodes_.back());
- t.c(this->cost_build(this->nodes_.back(), t));
+ t.p(this->_nodes.back());
+ t.c(this->cost_build(this->_nodes.back(), t));
if (!this->dn_[i].vi()) {
pq.push(this->dn_[i]);
}
DijkstraNode td = pq.top();
RRTNode& t = *td.node;
pq.pop();
- this->bc_.set_pose(t);
+ this->set_bc_pose_to(t);
for (unsigned int i = td.i; i > 0; i--) {
DijkstraNode& fd = this->dn_[i];
RRTNode& f = *this->dn_[i].node;
- if (!this->bc_.drivable(f)) {
+ if (!this->_bc.drivable(f)) {
continue;
}
double cost = td.d + this->cost_build(f, t);
this->steer(f, t);
- if (this->steered_.size() == 0) {
+ if (this->_steered.size() == 0) {
break;
}
- unsigned int ss = this->steered_.size();
+ unsigned int ss = this->_steered.size();
if (this->collide_steered()
- || ss != this->steered_.size()) {
+ || ss != this->_steered.size()) {
continue;
}
- this->bc_.set_pose(this->steered_.back());
- bool td = this->bc_.drivable(t);
- bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
+ this->set_bc_pose_to(this->_steered.back());
+ bool td = this->_bc.drivable(t);
+ bool tn = this->_bc.edist(t) < 2.0 * this->_eta;
if (cost < fd.d && td && tn) {
fd.d = cost;
this->join_steered(&f);
- t.p(this->nodes_.back());
- t.c(this->cost_build(this->nodes_.back(), t));
- this->recompute_cc(&t);
+ t.p(this->_nodes.back());
+ t.c(this->cost_build(this->_nodes.back(), t));
+ this->recompute_cc_for_predecessors_and(&t);
if (!this->dn_[i].vi()) {
pq.push(this->dn_[i]);
}
RRTExt13::compute_path()
{
RRTS::compute_path();
- if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
+ if (this->_goal.cc() == 0.0 || this->_path.size() == 0) {
return;
}
#if 1 // TODO 0.59 should work for sc4-1-0 only.
- if (this->goal_.cc() * 0.8 > this->last_goal_cc_
- && this->last_goal_cc_ != 0.0) {
+ if (this->_goal.cc() * 0.8 > this->last_path_cost()
+ && this->last_path_cost() != 0.0) {
return;
}
#endif
bool measure_time = false;
if (this->opath_.size() == 0) {
- this->opath_ = this->path_;
- this->ogoal_cc_ = this->goal_.cc();
+ this->opath_ = this->_path;
+ this->ogoal_cc_ = this->_goal.cc();
measure_time = true;
}
if (measure_time) {
- this->otime_ = -this->ter_.scnt();
+ this->otime_ = -this->scnt();
}
- double curr_cc = this->goal_.cc();
+ double curr_cc = this->_goal.cc();
double last_cc = curr_cc + 1.0;
while (curr_cc < last_cc) {
last_cc = curr_cc;
this->interesting_backward();
this->dijkstra_backward();
RRTS::compute_path();
- curr_cc = this->goal_.cc();
+ curr_cc = this->_goal.cc();
}
if (measure_time) {
- this->otime_ += this->ter_.scnt();
+ this->otime_ += this->scnt();
}
}
RRTExt14::sample()
{
if (this->circle_r_ == 0.0) {
- RRTNode& f = this->nodes_.front();
- RRTNode& g = this->goal_;
+ RRTNode& f = this->_nodes.front();
+ RRTNode& g = this->_goal;
double dx = g.x() - f.x();
double dy = g.y() - f.y();
this->circle_r_ = sqrt(dx * dx + dy * dy);
this->circle_c_.x((f.x() + g.x()) / 2.0);
this->circle_c_.y((f.y() + g.y()) / 2.0);
- return this->goal_;
+ return this->_goal;
}
- double r = this->circle_r_ * sqrt(this->udr_(this->gen_));
- double theta = this->udt_(this->gen_);
+ double r = this->circle_r_ * sqrt(this->udr_(this->_gen));
+ double theta = this->udt_(this->_gen);
RRTNode rs;
rs.x(this->circle_c_.x() + r * cos(theta));
rs.y(this->circle_c_.y() + r * sin(theta));
- rs.h(this->udh_(this->gen_));
+ rs.h(this->udh_(this->_gen));
return rs;
}
{
bool c = RRTS::next();
if (this->log_path_cost_.size() == 0) {
- this->log_path_cost_.push_back(this->goal_.cc());
+ this->log_path_cost_.push_back(this->_goal.cc());
} else {
double lc = this->log_path_cost_.back();
- double gc = this->goal_.cc();
+ double gc = this->_goal.cc();
if (gc > 0.0 && (lc == 0.0 || gc < lc)) {
this->log_path_cost_.push_back(gc);
} else {
void
RRTExt16::steer(RRTNode const& f, RRTNode const& t)
{
- this->steered_.clear();
+ this->_steered.clear();
double q0[] = {f.x(), f.y(), f.h()};
double q1[] = {t.x(), t.y(), t.h()};
- ReedsSheppStateSpace rsss(this->bc_.mtr());
- rsss.sample(q0, q1, this->eta_, cb_steer, &this->steered_);
+ ReedsSheppStateSpace rsss(this->_bc.mtr());
+ rsss.sample(q0, q1, this->_eta, cb_steer, &this->_steered);
}
} // namespace rrts
bool
RRTExt17::should_finish() const
{
- return this->goal_.p() != nullptr || this->icnt_ > this->_imax;
+ return this->_goal.p() != nullptr || this->icnt() > this->icnt_max();
}
} // namespace rrts
bool
RRTExt18::should_finish() const
{
- return this->icnt_ > this->_imax;
+ return this->icnt() > this->icnt_max();
}
} // namespace rrts
void
RRTExt19::steer(RRTNode const &f, RRTNode const &t)
{
- this->steered_.clear();
+ this->_steered.clear();
double q0[] = {f.x(), f.y(), f.h()};
double q1[] = {t.x(), t.y(), t.h()};
DubinsPath path;
- int r = dubins_shortest_path(&path, q0, q1, this->bc_.mtr());
+ int r = dubins_shortest_path(&path, q0, q1, this->_bc.mtr());
if (r != 0) {
return;
}
- dubins_path_sample_many(&path, this->eta_, cb_steer, &this->steered_);
- if (this->steered_.size() > 0) {
- this->steered_.front().sp(0.0);
+ dubins_path_sample_many(&path, this->_eta, cb_steer, &this->_steered);
+ if (this->_steered.size() > 0) {
+ this->_steered.front().sp(0.0);
}
}
RRTExt2::collide_steered()
{
unsigned int i = 0;
- for (auto& n: this->steered_) {
+ for (auto& n: this->_steered) {
if (this->collide(n)) {
break;
}
i++;
}
- this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
- return this->steered_.size() == 0;
+ this->_steered.erase(this->_steered.begin() + i, this->_steered.end());
+ return this->_steered.size() == 0;
}
RRTExt2::RRTExt2() : RRTS()
{
this->c2_bc_.count = 4;
- this->c2_bc_.verts[0].x = this->bc_.lfx();
- this->c2_bc_.verts[0].y = this->bc_.lfy();
- this->c2_bc_.verts[1].x = this->bc_.lrx();
- this->c2_bc_.verts[1].y = this->bc_.lry();
- this->c2_bc_.verts[2].x = this->bc_.rrx();
- this->c2_bc_.verts[2].y = this->bc_.rry();
- this->c2_bc_.verts[3].x = this->bc_.rfx();
- this->c2_bc_.verts[3].y = this->bc_.rfy();
+ this->c2_bc_.verts[0].x = this->_bc.lfx();
+ this->c2_bc_.verts[0].y = this->_bc.lfy();
+ this->c2_bc_.verts[1].x = this->_bc.lrx();
+ this->c2_bc_.verts[1].y = this->_bc.lry();
+ this->c2_bc_.verts[2].x = this->_bc.rrx();
+ this->c2_bc_.verts[2].y = this->_bc.rry();
+ this->c2_bc_.verts[3].x = this->_bc.rfx();
+ this->c2_bc_.verts[3].y = this->_bc.rfy();
}
void
{
RRTS::reset();
this->c2_bc_.count = 4;
- this->c2_bc_.verts[0].x = this->bc_.lfx();
- this->c2_bc_.verts[0].y = this->bc_.lfy();
- this->c2_bc_.verts[1].x = this->bc_.lrx();
- this->c2_bc_.verts[1].y = this->bc_.lry();
- this->c2_bc_.verts[2].x = this->bc_.rrx();
- this->c2_bc_.verts[2].y = this->bc_.rry();
- this->c2_bc_.verts[3].x = this->bc_.rfx();
- this->c2_bc_.verts[3].y = this->bc_.rfy();
+ this->c2_bc_.verts[0].x = this->_bc.lfx();
+ this->c2_bc_.verts[0].y = this->_bc.lfy();
+ this->c2_bc_.verts[1].x = this->_bc.lrx();
+ this->c2_bc_.verts[1].y = this->_bc.lry();
+ this->c2_bc_.verts[2].x = this->_bc.rrx();
+ this->c2_bc_.verts[2].y = this->_bc.rry();
+ this->c2_bc_.verts[3].x = this->_bc.rfx();
+ this->c2_bc_.verts[3].y = this->_bc.rfy();
}
Json::Value
RRTExt21::collide_steered()
{
unsigned int i = 0;
- for (auto &n: this->steered_) {
+ for (auto &n: this->_steered) {
if (this->collide(n)) {
break;
}
i++;
}
- this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
- return this->steered_.size() == 0;
+ this->_steered.erase(this->_steered.begin() + i, this->_steered.end());
+ return this->_steered.size() == 0;
}
void
RRTExt8::store(RRTNode* n, KdNode*& ref, unsigned int lvl)
{
if (ref == nullptr) {
- this->kdnodes_.push_back(KdNode(n));
- ref = &this->kdnodes_.back();
+ this->_kdnodes.push_back(KdNode(n));
+ ref = &this->_kdnodes.back();
} else if (lvl % 3 == 0 && n->x() < ref->node->x()) {
this->store(n, ref->left, lvl + 1);
} else if (lvl % 3 == 0) {
if (ref == nullptr) {
return;
}
- if (this->cost_search(*ref->node, t) < this->cost_
- || this->nn_ == nullptr) {
- this->nn_ = ref->node;
- this->cost_ = this->cost_search(*ref->node, t);
+ if (this->cost_search(*ref->node, t) < this->_cost
+ || this->_nn == nullptr) {
+ this->_nn = ref->node;
+ this->_cost = this->cost_search(*ref->node, t);
}
if (lvl % 3 == 0 && t.x() < ref->node->x()) {
this->find_nn(t, ref->left, lvl + 1);
- if (ref->node->x() - t.x() < this->cost_) {
+ if (ref->node->x() - t.x() < this->_cost) {
this->find_nn(t, ref->right, lvl + 1);
}
} else if (lvl % 3 == 0) {
this->find_nn(t, ref->right, lvl + 1);
- if (t.x() - ref->node->x() < this->cost_) {
+ if (t.x() - ref->node->x() < this->_cost) {
this->find_nn(t, ref->left, lvl + 1);
}
} else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
this->find_nn(t, ref->left, lvl + 1);
- if (ref->node->y() - t.y() < this->cost_) {
+ if (ref->node->y() - t.y() < this->_cost) {
this->find_nn(t, ref->right, lvl + 1);
}
} else if (lvl % 3 == 1) {
this->find_nn(t, ref->right, lvl + 1);
- if (t.y() - ref->node->y() < this->cost_) {
+ if (t.y() - ref->node->y() < this->_cost) {
this->find_nn(t, ref->left, lvl + 1);
}
} else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
this->find_nn(t, ref->left, lvl + 1);
- if (ref->node->h() - t.h() < this->cost_) {
+ if (ref->node->h() - t.h() < this->_cost) {
this->find_nn(t, ref->right, lvl + 1);
}
} else {
this->find_nn(t, ref->right, lvl + 1);
- if (t.h() - ref->node->h() < this->cost_) {
+ if (t.h() - ref->node->h() < this->_cost) {
this->find_nn(t, ref->left, lvl + 1);
}
}
if (ref == nullptr) {
return;
}
- if (this->cost_search(*ref->node, t) < this->cost_) {
- this->nv_.push_back(ref->node);
+ if (this->cost_search(*ref->node, t) < this->_cost) {
+ this->_nv.push_back(ref->node);
}
if (lvl % 3 == 0 && t.x() < ref->node->x()) {
this->find_nv(t, ref->left, lvl + 1);
- if (ref->node->x() - t.x() < this->cost_) {
+ if (ref->node->x() - t.x() < this->_cost) {
this->find_nv(t, ref->right, lvl + 1);
}
} else if (lvl % 3 == 0) {
this->find_nv(t, ref->right, lvl + 1);
- if (t.x() - ref->node->x() < this->cost_) {
+ if (t.x() - ref->node->x() < this->_cost) {
this->find_nv(t, ref->left, lvl + 1);
}
} else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
this->find_nv(t, ref->left, lvl + 1);
- if (ref->node->y() - t.y() < this->cost_) {
+ if (ref->node->y() - t.y() < this->_cost) {
this->find_nv(t, ref->right, lvl + 1);
}
} else if (lvl % 3 == 1) {
this->find_nv(t, ref->right, lvl + 1);
- if (t.y() - ref->node->y() < this->cost_) {
+ if (t.y() - ref->node->y() < this->_cost) {
this->find_nv(t, ref->left, lvl + 1);
}
} else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
this->find_nv(t, ref->left, lvl + 1);
- if (ref->node->h() - t.h() < this->cost_) {
+ if (ref->node->h() - t.h() < this->_cost) {
this->find_nv(t, ref->right, lvl + 1);
}
} else {
this->find_nv(t, ref->right, lvl + 1);
- if (t.h() - ref->node->h() < this->cost_) {
+ if (t.h() - ref->node->h() < this->_cost) {
this->find_nv(t, ref->left, lvl + 1);
}
}
RRTExt8::RRTExt8() : RRTS()
{
- this->kdnodes_.reserve(this->nodes_.capacity());
- this->store(&this->nodes_.front(), this->kdroot_, 0);
+ this->_kdnodes.reserve(this->_nodes.capacity());
+ this->store(&this->_nodes.front(), this->kdroot_, 0);
}
void
RRTExt8::reset()
{
RRTS::reset();
- this->kdnodes_.clear();
+ this->_kdnodes.clear();
this->kdroot_ = nullptr;
- this->store(&this->nodes_.front(), this->kdroot_, 0);
+ this->store(&this->_nodes.front(), this->kdroot_, 0);
}
void
RRTExt8::store(RRTNode n)
{
RRTS::store(n);
- this->store(&this->nodes_.back(), this->kdroot_, 0);
+ this->store(&this->_nodes.back(), this->kdroot_, 0);
}
void
RRTExt8::find_nn(RRTNode const& t)
{
- this->nn_ = &this->nodes_.front();
- this->cost_ = this->cost_search(*this->nn_, t);
+ this->_nn = &this->_nodes.front();
+ this->_cost = this->cost_search(*this->_nn, t);
this->find_nn(t, this->kdroot_, 0);
}
void
RRTExt8::find_nv(RRTNode const& t)
{
- this->nv_.clear();
- this->cost_ = this->min_gamma_eta();
+ this->_nv.clear();
+ this->_cost = this->min_gamma_eta();
this->find_nv(t, this->kdroot_, 0);
}
}
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);
}
}
}
RRTS::compute_path()
{
this->_path.clear();
- RRTNode *g = &this->goal();
+ RRTNode *g = &this->_goal;
if (g->p() == nullptr) {
return;
}
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);
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->store(RRTNode()); // root
}
-BicycleCar&
-RRTS::bc(void) const
+void
+RRTS::set_bc_pose_to(Pose const& p)
{
- return this->_bc;
+ this->_bc.set_pose_to(p);
}
-RRTGoal&
+RRTGoal const&
RRTS::goal(void) const
{
return this->_goal;
this->_icnt_max = i;
}
+void
+RRTS::tstart(void)
+{
+ this->_ter.start();
+}
+
double
RRTS::scnt() const
{
double
RRTS::path_cost() const
{
- return this->goal().cc();
+ return this->_goal.cc();
}
double
RRTS::last_path_cost(void) const
{
- return this->_last_path_cost;
-}
-
-void
-RRTS::last_path_cost(double c)
-{
- this->_last_path_cost = c;
+ if (this->_last_path.size() == 0) {
+ return 999.9;
+ } else {
+ return this->_last_path.back().cc();
+ }
}
double
jvo["path"][i][4] = n->st();
i++;
}
- jvo["goal_cc"] = this->goal().cc();
+ jvo["goal_cc"] = this->_goal.cc();
jvo["time"] = this->scnt();
return jvo;
}
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();
}
{
if (this->path_cost() != 0.0
&& this->path_cost() < this->last_path_cost()) {
- this->last_path_cost(this->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(
- this->goal().x(),
- this->goal().y(),
- this->goal().b(),
- this->goal().e());
+ 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());
projects / hubacji1 / iamcar2.git / commitdiff