void RRTS::log_path_cost()
{
- this->log_path_cost_.push_back(cc(this->goals().front()));
- this->log_path_time_ += 0.1;
+ if (this->log_path_cost_.size() == 0) {
+ this->log_path_cost_.push_back(this->goals().front().cc);
+ } else {
+ auto lc = this->log_path_cost_.back();
+ auto gc = this->goals().front().cc;
+ auto goal_is_better = this->goals().front().cc > 0 && lc < gc;
+ if (
+ this->log_path_cost_.back() > 0
+ && (
+ this->goals().front().cc == 0
+ || (
+ this->goals().front().cc > 0
+ && goal_is_better
+ )
+ )
+ ) {
+ this->log_path_cost_.push_back(
+ this->log_path_cost_.back()
+ );
+ } else {
+ this->log_path_cost_.push_back(
+ this->goals().front().cc
+ );
+ }
+ }
+ this->log_path_iter_ += 1;
}
bool RRTS::should_stop()
bool RRTS::should_finish()
{
// decide finish conditions (maybe comment some lines)
- //if (this->icnt_ > 999) return true;
- if (this->scnt_ > 2) return true;
+ if (this->icnt_ > 1000) return true;
+ //if (this->scnt_ > 2) return true;
if (this->finishit) return true;
- //if (this->gf()) return true;
+ if (this->gf()) return true;
// but continue by default
return false;
}
);
double adist = std::abs(f.h() - g.h());
if (edist < 0.05 && adist < M_PI / 32) {
- if (g.p() == nullptr || cc(f) + cost < cc(g)) {
+ if (g.p() == nullptr || f.cc + cost < g.cc) {
g.p(&f);
g.c(cost);
}
{
RRTNode *t = &this->steered().front();
RRTNode *f = this->nn(this->samples().back());
- double cost = this->cost_search(*f, *t);
+ double cost = f->cc + this->cost_build(*f, *t);
for (auto n: this->nv(*t)) {
if (
!std::get<0>(this->collide_two_nodes(*n, *t))
- && this->cost_search(*n, *t) < cost
+ && n->cc + this->cost_build(*n, *t) < cost
) {
f = n;
- cost = this->cost_search(*n, *t);
+ cost = n->cc + this->cost_build(*n, *t);
}
}
// steer from f->t and then continue with the steered.
this->join_tmp_steered(f);
f = &this->nodes().back();
}
- if (sqrt(
- pow(f->x() - t->x(), 2)
- + pow(f->y() - t->y(), 2)
- ) > 0.2)
- return false;
+ auto fbc = BicycleCar();
+ fbc.x(f->x());
+ fbc.y(f->y());
+ fbc.h(f->h());
+ auto tbc = BicycleCar();
+ tbc.x(t->x());
+ tbc.y(t->y());
+ tbc.h(t->h());
+ if (!tbc.drivable(fbc))
+ return false;
// cont.
this->store_node(this->steered().front());
t = &this->nodes().back();
for (auto n: this->nv(*f)) {
if (
!std::get<0>(this->collide_two_nodes(*f, *n))
- && cc(*f) + this->cost_search(*f, *n) < cc(*n)
+ && f->cc + this->cost_build(*f, *n) < n->cc
) {
+ this->tmp_steer(*f, *n);
+ if (this->tmp_steered_.size() > 0) {
+ auto col = this->collide_tmp_steered_from(*f);
+ if (std::get<0>(col))
+ continue;
+ this->join_tmp_steered(f);
+ f = &this->nodes().back();
+ }
n->p(f);
n->c(this->cost_build(*f, *n));
}
{
}
+void RRTS::reset()
+{
+ RRTNode init = RRTNode();
+ init.x(this->nodes().front().x());
+ init.y(this->nodes().front().y());
+ init.h(this->nodes().front().h());
+ this->nodes().clear();
+ this->store_node(RRTNode());
+ this->nodes().front().x(init.x());
+ this->nodes().front().y(init.y());
+ this->nodes().front().h(init.h());
+ this->samples().clear();
+ this->steered().clear();
+ this->path().clear();
+ this->gf(false);
+ for (auto& g: this->goals()) {
+ g.p(nullptr);
+ g.c_ = 0.0;
+ g.cc = 0.0;
+ }
+}
+
void RRTS::deinit()
{
this->nodes().clear();
this->gf_ = false;
}
-std::vector<RRTNode *> RRTS::path()
+void RRTS::compute_path()
{
- std::vector<RRTNode *> path;
if (this->goals().size() == 0)
- return path;
- RRTNode *goal = &this->goals().back();
+ return;
+ RRTNode *goal = &this->goals().front();
if (goal->p() == nullptr)
- return path;
+ return;
+ this->path_.clear();
while (goal != nullptr) {
- path.push_back(goal);
+ this->path_.push_back(goal);
goal = goal->p();
}
- std::reverse(path.begin(), path.end());
- return path;
+ std::reverse(this->path_.begin(), this->path_.end());
}
bool RRTS::next()
if (this->icnt_ == 0)
this->tstart_ = std::chrono::high_resolution_clock::now();
bool next = true;
- if (this->scnt_ > this->log_path_time_)
- this->log_path_cost();
- if (this->should_stop())
+ if (this->should_stop()) {
+ this->log_path_cost();
return false;
+ }
if (this->samples().size() == 0) {
this->samples().push_back(RRTNode());
this->samples().back().x(this->goals().front().x());
*this->nn(this->samples().back()),
this->samples().back()
);
- if (this->steered().size() == 0)
+ if (this->steered().size() == 0) {
+ this->log_path_cost();
return next;
+ }
auto col = this->collide_steered_from(
*this->nn(this->samples().back())
);
this->steered().pop_back();
}
}
- if (!this->connect())
+ if (!this->connect()) {
+ this->log_path_cost();
return next;
+ }
this->rewire();
unsigned scnt = this->steered().size();
this->join_steered(&this->nodes().back());
this->steered2_.push_back(jap);
jap = jap->p();
}
- this->gf(this->goal_found(this->nodes().back()));
+ auto gf = this->goal_found(this->nodes().back());
+ this->gf(gf);
just_added = just_added->p();
}
+ if (
+ this->gf()
+ && (
+ this->path().size() == 0
+ || this->goals().front().cc < this->path().back()->cc
+ )
+ ) {
+ this->compute_path();
+ }
+ this->log_path_cost();
return next;
}
}
{
if (this->path().size() > 0) {
- jvo["cost"] = cc(*this->path().back());
+ jvo["cost"] = this->path().back()->cc;
jvo["entry"][0] = this->goals().front().x();
jvo["entry"][1] = this->goals().front().y();
jvo["entry"][2] = this->goals().front().h();
for (auto i: this->log_path_cost_)
jvo["log_path_cost"][cnt++] = i;
}
+ {
+ unsigned int cnt = 0;
+ for (auto i: this->log_opt_time_)
+ jvo["log_opt_time"][cnt++] = i;
+ }
//{
// unsigned int ncnt = 0;
// for (auto n: this->nodes()) {