+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
#include <algorithm>
#include <cassert>
#include "rrts.hh"
+#ifndef USE_RRTS
+#define USE_RRTS 0 // TODO improve, this solution isn't clear.
+#endif
+
namespace rrts {
void
}
}
+unsigned int
+RRTNode::cusp() const
+{
+ return this->cusp_;
+}
+
+void
+RRTNode::cusp(RRTNode const& p)
+{
+ this->cusp_ = p.cusp();
+ if (this->sp() != p.sp() || this->sp() == 0.0) {
+ this->cusp_++;
+ }
+}
+
bool
RRTNode::operator==(RRTNode const& n)
{
return this == &n;
}
+void
+RRTS::recompute_cc(RRTNode* g)
+{
+ this->path_.clear();
+ while (g != nullptr) {
+ 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]));
+ }
+}
+
+void
+RRTS::recompute_path_cc()
+{
+ this->recompute_cc(&this->goal_);
+}
+
double
RRTS::min_gamma_eta() const
{
RRTNode* t = &this->nodes_.back();
t->p(*f);
t->c(this->cost_build(*f, *t));
+ t->cusp(*f);
this->steered_.erase(this->steered_.begin());
f = t;
}
{
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_) {
double nc = n->cc() + this->cost_build(*n, *t);
if (!this->bc_.drivable(*t)) {
f = this->nn_;
}
+#endif
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());
return true;
}
std::reverse(this->path_.begin(), this->path_.end());
}
-RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
+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()
+{
+ return this->bc_;
+}
+
+void
+RRTS::set_imax_reset(unsigned int i)
+{
+ this->_imax = i;
+}
+
+void
+RRTS::set_goal(double x, double y, double b, double e)
+{
+ this->goal_ = RRTGoal(x, y, b, e);
+}
+
+void
+RRTS::set_start(double x, double y, double h)
+{
+ this->nodes_.front().x(x);
+ this->nodes_.front().y(y);
+ this->nodes_.front().h(h);
+}
+
+std::vector<Pose>
+RRTS::get_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::get_path_cost() const
+{
+ return this->goal_.cc();
+}
+
unsigned int
RRTS::icnt() const
{
return this->ter_.scnt();
}
+double
+RRTS::eta() const
+{
+ return this->eta_;
+}
+
+void
+RRTS::eta(double e)
+{
+ this->eta_ = e;
+}
+
Json::Value
RRTS::json() const
{
{
assert(jvi["init"] != Json::nullValue);
assert(jvi["goal"] != Json::nullValue);
- assert(jvi["obst"] != 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->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()];
+ }
+}
+#endif
this->find_nn(rs);
this->steer(this->nn(), rs);
if (this->collide_steered()) {
return this->should_continue();
}
+#if USE_RRTS
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();
+ bool gf = false;
while (ss > 0 && just_added->p() != nullptr) {
- //if (!this->goal_drivable_from(*just_added)) {
- // ss--;
- // just_added = just_added->p();
- // continue;
- //}
this->steer(*just_added, this->goal_);
if (this->collide_steered()) {
ss--;
|| ncc < this->goal_.cc()) {
this->goal_.p(this->nodes_.back());
this->goal_.c(nc);
- this->compute_path();
+ gf = true;
}
}
ss--;
just_added = just_added->p();
}
-
- ////if (!this->goal_drivable_from(this->nodes_.back())) {
- //// return this->should_continue();
- ////}
- //this->steer(this->nodes_.back(), this->goal_);
- //if (this->collide_steered()) {
- // return this->should_continue();
- //}
- //this->join_steered(&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);
- // this->compute_path();
- // }
- //}
+ if (gf) {
+ this->compute_path();
+ }
this->time_ = this->ter_.scnt();
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);
+ }
+ }
this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
this->goal_.e());
this->path_.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);
}
} // namespace rrts
projects / hubacji1 / rrts.git / blobdiff