double cost_ = 0.0;
double eta_ = 0.5;
double time_ = 0.0;
+ double last_goal_cc_ = 0.0;
double min_gamma_eta() const;
bool should_continue() const;
void join_steered(RRTNode* f);
}
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_) {
+ this->icnt_ -= 1;
+ return this->should_continue();
+ }
+}
+#endif
this->find_nn(rs);
this->steer(this->nn(), rs);
if (this->collide_steered()) {
void
RRTS::reset()
{
+ if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
+ this->last_goal_cc_ = this->goal_.cc();
+ }
this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
this->goal_.e());
this->path_.clear();