void
Ter::start()
{
- this->tstart_ = std::chrono::high_resolution_clock::now();
+ this->_tstart = std::chrono::high_resolution_clock::now();
}
double
Ter::scnt() const
{
- using namespace std::chrono;
- auto t = high_resolution_clock::now() - this->tstart_;
- auto d = duration_cast<duration<double>>(t);
+ auto t = std::chrono::high_resolution_clock::now() - this->_tstart;
+ auto d = std::chrono::duration_cast<std::chrono::seconds>(t);
return d.count();
}
double
RRTNode::c() const
{
- return this->c_;
+ return this->_c;
}
void
RRTNode::c(double c)
{
- assert(this->p_ != nullptr);
- this->c_ = c;
- this->cc_ = this->p_->cc() + c;
+ assert(this->_p != nullptr);
+ this->_c = c;
+ this->_cc = this->_p->cc() + c;
}
double
RRTNode::cc() const
{
- return this->cc_;
+ return this->_cc;
}
RRTNode*
RRTNode::p() const
{
- return this->p_;
+ return this->_p;
}
void
-RRTNode::p(RRTNode& p)
+RRTNode::p(RRTNode& p, bool can_be_too_close)
{
- if (this != &p) {
- this->p_ = &p;
+ assert(this != &p);
+ if (!can_be_too_close) {
+ assert(!(std::abs(p.x() - this->x()) < 1e-3
+ && std::abs(p.y() - this->y()) < 1e-3
+ && std::abs(p.h() - this->h()) < 1e-3));
}
+ this->_p = &p;
+ this->p_is_cusp(this->would_be_cusp_if_parent(p));
+ this->cusp_cnt(p);
+}
+
+void
+RRTNode::p(RRTNode& p)
+{
+ return this->p(p, false);
}
unsigned int
-RRTNode::cusp() const
+RRTNode::cusp_cnt() const
+{
+ return this->_cusp_cnt;
+}
+
+void
+RRTNode::cusp_cnt(RRTNode const& p)
+{
+ this->_cusp_cnt = p.cusp_cnt();
+ if (this->_p_is_cusp) {
+ this->_cusp_cnt++;
+ }
+}
+
+int
+RRTNode::st(void)
+{
+ return this->_segment_type;
+}
+
+void
+RRTNode::st(int st)
+{
+ this->_segment_type = st;
+}
+
+bool
+RRTNode::p_is_cusp(void) const
{
- return this->cusp_;
+ return this->_p_is_cusp;
}
void
-RRTNode::cusp(RRTNode const& p)
+RRTNode::p_is_cusp(bool isit)
{
- this->cusp_ = p.cusp();
- if (this->sp() != p.sp() || this->sp() == 0.0) {
- this->cusp_++;
+ this->_p_is_cusp = isit;
+}
+
+bool
+RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
+{
+ if (std::abs(p.sp()) < 1e-3) {
+ // It should not be possible to have two zero speed nodes next
+ // to each other. In practice, this sometimes happens.
+ //assert(std::abs(this->sp()) > 1e-3);
+ if (p.p()) {
+ if (sgn(p.p()->sp()) != sgn(this->sp())) {
+ return true;
+ } else {
+ return false;
+ }
+ } else {
+ return true; // only root has no parent and it is cusp
+ }
+ } else {
+ if (std::abs(this->sp()) < 1e-3) {
+ return false; // this is cusp, not the parent
+ } else if (sgn(p.sp()) != sgn(this->sp())) {
+ return true;
+ } else {
+ return false;
+ }
}
}
}
void
-RRTS::recompute_cc(RRTNode* g)
+RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
{
- this->path_.clear();
+ std::vector<RRTNode*> path;
+ path.reserve(this->_path.size());
while (g != nullptr) {
- this->path_.push_back(g);
+ 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(path.begin(), path.end());
+ for (unsigned int i = 1; i < path.size(); i++) {
+ path[i]->c(this->cost_build(*path[i - 1], *path[i]));
}
}
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();
+ // Require the steer method to return first node equal to nn:
+ assert(std::abs(t->x() - f->x()) < 1e-3
+ && std::abs(t->y() - f->y()) < 1e-3
+ && std::abs(t->h() - f->h()) < 1e-3);
+ this->_steered.erase(this->_steered.begin());
+ t = &this->_steered.front();
+ assert(!(std::abs(t->x() - f->x()) < 1e-3
+ && std::abs(t->y() - f->y()) < 1e-3
+ && std::abs(t->h() - f->h()) < 1e-3));
#if USE_RRTS
double cost = f->cc() + this->cost_build(*f, *t);
- for (auto n: this->nv_) {
+ for (auto n: this->_nv) {
double nc = n->cc() + this->cost_build(*n, *t);
if (nc < cost) {
f = n;
}
}
// 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)
{
- return this->bc_;
+ this->_bc.set_pose_to(p);
}
void
-RRTS::set_imax_reset(unsigned int i)
+RRTS::set_bc_to_become(std::string what)
{
- this->_imax = i;
+ this->_bc.become(what);
}
-void
-RRTS::set_goal(double x, double y, double b, double e)
+RRTGoal const&
+RRTS::goal(void) const
{
- this->goal_ = RRTGoal(x, y, b, e);
+ return this->_goal;
}
void
-RRTS::set_start(double x, double y, double h)
+RRTS::goal(double x, double y, double b, double e)
{
- this->nodes_.front().x(x);
- this->nodes_.front().y(y);
- this->nodes_.front().h(h);
+ this->_goal = RRTGoal(x, y, b, e);
}
-std::vector<Pose>
-RRTS::get_path() const
+unsigned int
+RRTS::icnt(void) const
{
- std::vector<Pose> path;
- for (auto n: this->path_) {
- path.push_back(Pose(n->x(), n->y(), n->h()));
- }
- return path;
+ return this->_icnt;
}
-double
-RRTS::get_path_cost() const
+void
+RRTS::icnt(unsigned int i)
{
- return this->goal_.cc();
+ this->_icnt = i;
}
unsigned int
-RRTS::icnt() const
+RRTS::icnt_max(void) const
{
- return this->icnt_;
+ return this->_icnt_max;
}
void
-RRTS::icnt(unsigned int i)
+RRTS::icnt_max(unsigned int i)
+{
+ this->_icnt_max = i;
+}
+
+void
+RRTS::tstart(void)
{
- this->icnt_ = i;
+ this->_ter.start();
}
double
RRTS::scnt() const
{
- return this->ter_.scnt();
+ return this->_ter.scnt();
+}
+
+void
+RRTS::set_init_pose_to(Pose const& p)
+{
+ this->_nodes.front().x(p.x());
+ this->_nodes.front().y(p.y());
+ this->_nodes.front().h(p.h());
+}
+
+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
+{
+ return this->_goal.cc();
+}
+
+double
+RRTS::last_path_cost(void) const
+{
+ if (this->_logged_paths.size() == 0) {
+ return 0.0;
+ }
+ assert(this->_logged_paths.back().size() > 0);
+ return this->_logged_paths.back().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
-RRTS::json() const
+RRTS::json(void) const
{
Json::Value jvo;
- unsigned int i = 0;
- for (auto n: this->path_) {
+ unsigned int i = 0, j = 0;
+ for (auto path: this->_logged_paths) {
+ i = 0;
+ for (auto n: path) {
+ jvo["paths"][j][i][0] = n.x();
+ jvo["paths"][j][i][1] = n.y();
+ jvo["paths"][j][i][2] = n.h();
+ jvo["paths"][j][i][3] = n.sp();
+ jvo["paths"][j][i][4] = n.st();
+ jvo["paths"][j][i][5] = false;
+ if (n.p_is_cusp()) {
+ assert(i > 0);
+ jvo["paths"][j][i - 1][5] = true;
+ }
+ i++;
+ }
+ // Initial point is part of the first segment.
+ jvo["paths"][j][0][3] = jvo["paths"][j][1][3];
+ jvo["paths"][j][0][4] = jvo["paths"][j][1][4];
+ // Goal point is part of the last segment.
+ jvo["paths"][j][i - 1][3] = jvo["paths"][j][i - 2][3];
+ jvo["paths"][j][i - 1][4] = jvo["paths"][j][i - 2][4];
+ // --
+ jvo["costs"][j] = path.back().cc();
+ j++;
+ }
+ i = 0;
+ for (auto n: this->_path) {
+ jvo["paths"][j][i][0] = n->x();
+ jvo["paths"][j][i][1] = n->y();
+ jvo["paths"][j][i][2] = n->h();
+ jvo["paths"][j][i][3] = n->sp();
+ jvo["paths"][j][i][4] = n->st();
+ jvo["paths"][j][i][5] = n->p_is_cusp();
jvo["path"][i][0] = n->x();
jvo["path"][i][1] = n->y();
jvo["path"][i][2] = n->h();
jvo["path"][i][3] = n->sp();
- jvo["path"][i][4] = n->segment_type;
+ jvo["path"][i][4] = n->st();
+ jvo["path"][i][5] = false;
+ if (n->p_is_cusp()) {
+ assert(i > 0);
+ jvo["path"][i - 1][5] = true;
+ }
i++;
}
- jvo["goal_cc"] = this->goal_.cc();
- jvo["time"] = this->time_;
+ // Initial point is part of the first segment.
+ jvo["path"][0][3] = jvo["path"][1][3];
+ jvo["path"][0][4] = jvo["path"][1][4];
+ // Goal point is part of the last segment.
+ jvo["path"][i - 1][3] = jvo["path"][i - 2][3];
+ jvo["path"][i - 1][4] = jvo["path"][i - 2][4];
+ // --
+ if (this->_path.size() > 0) {
+ jvo["costs"][j] = this->_path.back()->cc();
+ }
+ jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
+ jvo["cost"] = this->path_cost();
+ 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->set_init_pose_to(Pose(
+ 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()) {
+ auto& last_path = this->_logged_paths.back();
+ rs = last_path[rand() % 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_);
+ while (ss > 0 && just_added != nullptr) {
+ this->steer(*just_added, this->_goal);
if (this->collide_steered()) {
ss--;
just_added = just_added->p();
continue;
}
+ // The first of steered is the same as just_added.
+ this->_steered.erase(this->_steered.begin());
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(), true);
+ 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->_logged_paths.push_back(std::vector<RRTNode>());
+ auto& last_path = this->_logged_paths.back();
+ last_path.reserve(this->_path.size());
+ RRTNode* p = nullptr;
+ for (auto n: this->_path) {
+ last_path.push_back(*n);
+ if (p != nullptr) {
+ last_path.back().p(*p);
+ }
+ p = &last_path.back();
+ }
+ // Test that last path cost matches.
+ auto last_path_cost = last_path.back().cc();
+ for (unsigned int i = 1; i < last_path.size(); i++) {
+ last_path[i].c(this->cost_build(
+ last_path[i - 1],
+ last_path[i]));
}
+ assert(last_path_cost == last_path.back().cc());
}
- 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