+#include <algorithm>
#include <queue>
-#include "rrtext.h"
+#include "rrtext.hh"
-void RRTExt13::reset()
+namespace rrts {
+
+RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
+{
+}
+
+bool
+RRTExt13::DijkstraNode::vi()
+{
+ if (this->v) {
+ return true;
+ }
+ this->v = true;
+ return false;
+}
+
+int
+RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
+ DijkstraNode const& n2)
+{
+ return n1.node->cc() > n2.node->cc();
+}
+
+int
+RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
+ DijkstraNode const& n2)
{
- RRTS::reset();
- this->orig_path().clear();
- this->first_optimized_path().clear();
- this->orig_path_cost_ = 9999.9;
- this->first_optimized_path_cost_ = 9999.9;
+ return n1.d > n2.d;
}
-void RRTExt13::first_path_optimization()
+void
+RRTExt13::interesting_forward()
{
- if (this->orig_path().size() == 0) {
- this->orig_path_ = RRTS::path();
- if (this->orig_path().size() == 0)
- return;
- else
- this->orig_path_cost(this->orig_path().back()->cc);
- }
- class DijkstraNode : public RRTNode {
- public:
- DijkstraNode *s = nullptr;
- RRTNode *n = nullptr;
- unsigned int i = 0;
- double cc = 9999;
- bool v = false;
- bool vi()
- {
- if (this->v)
- return true;
- this->v = true;
- return false;
- }
- DijkstraNode(const RRTNode& n) {
- this->x(n.x());
- this->y(n.y());
- this->h(n.h());
- this->sp(n.sp());
- this->st(n.st());
- }
- // override
- DijkstraNode *p_ = nullptr;
- DijkstraNode *p() const { return this->p_; }
- void p(DijkstraNode *p) { this->p_ = p; }
- };
- class DijkstraNodeComparator {
- public:
- int operator() (
- const DijkstraNode &n1,
- const DijkstraNode &n2
- )
- {
- return n1.cc > n2.cc;
- }
- };
- std::vector<DijkstraNode> dn;
- unsigned int ops = this->orig_path().size();
- auto op = this->orig_path();
- dn.reserve(ops);
- unsigned int dncnt = 0;
- dn.push_back(DijkstraNode(*this->orig_path().front()));
- dn.back().cc = this->orig_path().front()->cc;
- dn.back().s = &dn.back();
- dn.back().n = this->orig_path().front();
- dn.back().i = dncnt++;
- for (unsigned int i = 0; i < ops - 1; i++) {
- auto ibc = BicycleCar();
- ibc.x(op[i]->x());
- ibc.y(op[i]->y());
- ibc.h(op[i]->h());
- for (unsigned int j = i + 1; j < ops; j++) {
- auto jbc = BicycleCar();
- jbc.x(op[j]->x());
- jbc.y(op[j]->y());
- jbc.h(op[j]->h());
- if (!jbc.drivable(ibc)) {
- dn.push_back(DijkstraNode(*op[j-1]));
- dn.back().cc = op[j-1]->cc;
- dn.back().s = &dn.back();
- dn.back().n = op[j-1];
- dn.back().i = dncnt++;
- i = j;
- break;
- }
- }
- }
- dn.push_back(DijkstraNode(*this->orig_path().back()));
- dn.back().cc = this->orig_path().back()->cc;
- dn.back().s = &dn.back();
- dn.back().n = this->orig_path().back();
- dn.back().i = dncnt++;
- std::priority_queue<
- DijkstraNode,
- std::vector<DijkstraNode>,
- DijkstraNodeComparator
- > pq;
- dn.front().vi();
- pq.push(dn.front());
- while (!pq.empty()) {
- DijkstraNode f = pq.top();
- pq.pop();
- for (unsigned int i = f.i + 1; i < dncnt; i++) {
- double cost = f.cc + this->cost_search(f, dn[i]);
- this->steer(f, dn[i]);
- if (this->steered().size() == 0)
- break; // TODO why not continue?
- if (std::get<0>(this->collide_steered_from(f)))
- continue;
- if (cost < dn[i].cc) {
- dn[i].cc = cost;
- dn[i].p(f.s);
- if (!dn[i].vi())
- pq.push(dn[i]);
- }
- }
- }
- DijkstraNode *ln = nullptr;
- for (auto &n: dn) {
- if (n.v)
- ln = &n;
- }
- if (ln == nullptr || ln->p() == nullptr)
- return;
- while (ln->p() != nullptr) {
- RRTNode *t = ln->n;
- RRTNode *f = ln->p()->n;
- this->steer(*f, *t);
- if (std::get<0>(this->collide_steered_from(*f)))
- return;
- this->join_steered(f);
- t->p(&this->nodes().back());
- t->c(this->cost_build(this->nodes().back(), *t));
- ln = ln->p();
- }
- RRTS::compute_path();
+ this->dn_.clear();
+ 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;
+ }
+ return;
+#endif
+#if 1 // cusp and 1st non-drivable path poses are interesting
+ this->dn_.push_back(DijkstraNode(this->opath_.front()));
+ 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);
+ 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)) {
+ this->dn_.push_back(DijkstraNode(&njl));
+ this->dn_.back().i = this->dn_.size() - 1;
+ i = j;
+ break;
+ }
+ if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+ this->dn_.push_back(DijkstraNode(&njl));
+ this->dn_.back().i = this->dn_.size() - 1;
+ }
+ }
+ }
+ this->dn_.push_back(DijkstraNode(this->opath_.back()));
+ this->dn_.back().i = this->dn_.size() - 1;
+#endif
}
-void RRTExt13::second_path_optimization()
+void
+RRTExt13::interesting_backward()
{
- if (this->first_optimized_path().size() == 0) {
- return;
- }
- class DijkstraNode : public RRTNode {
- public:
- DijkstraNode *s = nullptr;
- RRTNode *n = nullptr;
- unsigned int i = 0;
- double cc = 9999;
- bool v = false;
- bool vi()
- {
- if (this->v)
- return true;
- this->v = true;
- return false;
- }
- DijkstraNode(const RRTNode& n) {
- this->x(n.x());
- this->y(n.y());
- this->h(n.h());
- this->sp(n.sp());
- this->st(n.st());
- }
- // override
- DijkstraNode *p_ = nullptr;
- DijkstraNode *p() const { return this->p_; }
- void p(DijkstraNode *p) { this->p_ = p; }
- };
- class DijkstraNodeComparator {
- public:
- int operator() (
- const DijkstraNode &n1,
- const DijkstraNode &n2
- )
- {
- return n1.cc > n2.cc;
- }
- };
- std::vector<DijkstraNode> dn;
- unsigned int ops = this->orig_path().size();
- auto op = this->orig_path();
- dn.reserve(ops);
- unsigned int dncnt = 0;
- dn.push_back(DijkstraNode(*this->orig_path().front()));
- dn.back().cc = this->orig_path().front()->cc;
- dn.back().s = &dn.back();
- dn.back().n = this->orig_path().front();
- dn.back().i = dncnt++;
- for (unsigned int i = ops - 1; i > 1; i--) {
- auto ibc = BicycleCar();
- ibc.x(op[i]->x());
- ibc.y(op[i]->y());
- ibc.h(op[i]->h());
- for (unsigned int j = i - 1; j > 0; j--) {
- auto jbc = BicycleCar();
- jbc.x(op[j]->x());
- jbc.y(op[j]->y());
- jbc.h(op[j]->h());
- if (!jbc.drivable(ibc)) {
- dn.push_back(DijkstraNode(*op[j-1]));
- dn.back().cc = op[j-1]->cc;
- dn.back().s = &dn.back();
- dn.back().n = op[j-1];
- dn.back().i = dncnt++;
- i = j;
- break;
- }
- }
- }
- dn.push_back(DijkstraNode(*this->orig_path().back()));
- dn.back().cc = this->orig_path().back()->cc;
- dn.back().s = &dn.back();
- dn.back().n = this->orig_path().back();
- dn.back().i = dncnt++;
- std::priority_queue<
- DijkstraNode,
- std::vector<DijkstraNode>,
- DijkstraNodeComparator
- > pq;
- // TODO rewrite
- dn.back().vi();
- pq.push(dn.back());
- while (!pq.empty()) {
- DijkstraNode t = pq.top();
- pq.pop();
- for (unsigned int i = t.i - 1; i > 0; i--) {
- double cost = dn[i].cc + this->cost_search(dn[i], t);
- this->steer(dn[i], t);
- if (this->steered().size() == 0)
- break; // TODO why not continue?
- if (std::get<0>(this->collide_steered_from(dn[i])))
- continue;
- if (cost < t.cc) {
- t.cc = cost;
- t.p(dn[i].s);
- if (!dn[i].vi())
- pq.push(dn[i]);
- }
- }
- }
- DijkstraNode *fn = nullptr;
- for (auto &n: dn) {
- if (n.v) {
- fn = &n;
- break;
- }
- }
- if (fn == nullptr || fn->p() == nullptr)
- return;
- DijkstraNode *ln = &dn.back();
- while (ln->p() != fn) {
- RRTNode *t = ln->n;
- RRTNode *f = ln->p()->n;
- this->steer(*f, *t);
- if (std::get<0>(this->collide_steered_from(*f)))
- return;
- this->join_steered(f);
- t->p(&this->nodes().back());
- t->c(this->cost_build(this->nodes().back(), *t));
- ln = ln->p();
- }
- RRTS::compute_path();
+ this->dn_.clear();
+ 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;
+ }
+ std::reverse(this->dn_.begin(), this->dn_.end());
+ return;
+#endif
+#if 1 // cusp and 1st non-drivable path poses are interesting
+ 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);
+ 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)) {
+ this->dn_.push_back(DijkstraNode(&njl));
+ this->dn_.back().i = this->dn_.size() - 1;
+ this->dn_.back().d = goal_cc - njl.cc();
+ i = j;
+ break;
+ }
+ if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+ this->dn_.push_back(DijkstraNode(&njl));
+ this->dn_.back().i = this->dn_.size() - 1;
+ this->dn_.back().d = goal_cc - njl.cc();
+ }
+ }
+ }
+ this->dn_.push_back(DijkstraNode(this->opath_.front()));
+ this->dn_.back().i = this->dn_.size() - 1;
+ this->dn_.back().d = goal_cc - this->opath_.front()->cc();
+#endif
}
-void RRTExt13::compute_path()
+void
+RRTExt13::dijkstra_forward()
{
- RRTS::compute_path();
- auto tstart = std::chrono::high_resolution_clock::now();
- this->first_path_optimization();
- this->first_optimized_path_ = RRTS::path();
- if (this->first_optimized_path_.size() > 0) {
- this->first_optimized_path_cost(
- this->first_optimized_path_.back()->cc
- );
- } else {
- return;
- }
- this->second_path_optimization();
- auto tend = std::chrono::high_resolution_clock::now();
- auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
- tend - tstart);
- this->log_opt_time_.push_back(tdiff.count());
+ std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
+ DijkstraNodeComparator> pq;
+ this->dn_.front().vi();
+ pq.push(this->dn_.front());
+ while (!pq.empty()) {
+ DijkstraNode fd = pq.top();
+ RRTNode& f = *fd.node;
+ pq.pop();
+ this->bc_.set_pose(f);
+ for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
+ RRTNode& t = *this->dn_[i].node;
+ if (!this->bc_.drivable(t)) {
+ continue;
+ }
+ double cost = f.cc() + this->cost_build(f, t);
+ this->steer(f, t);
+ if (this->steered_.size() == 0) {
+ break;
+ }
+ unsigned int ss = this->steered_.size();
+ if (this->collide_steered()
+ || 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_;
+ if (cost < t.cc() && td && tn) {
+ this->join_steered(&f);
+ t.p(this->nodes_.back());
+ t.c(this->cost_build(this->nodes_.back(), t));
+ if (!this->dn_[i].vi()) {
+ pq.push(this->dn_[i]);
+ }
+ }
+ }
+ }
}
-Json::Value RRTExt13::json()
+void
+RRTExt13::dijkstra_backward()
{
- Json::Value jvo = RRTS::json();
- jvo["orig_path_cost"] = this->orig_path_cost();
- {
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: this->orig_path()) {
- jvo["orig_path"][pcnt][0] = n->x();
- jvo["orig_path"][pcnt][1] = n->y();
- jvo["orig_path"][pcnt][2] = n->h();
- if (n->t(RRTNodeType::cusp))
- cu++;
- if (n->t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- jvo["orig_cusps-in-path"] = cu;
- jvo["orig_connecteds-in-path"] = co;
- }
- return jvo;
+ std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
+ DijkstraNodeComparator> pq;
+ this->dn_.front().vi();
+ pq.push(this->dn_.front());
+ while (!pq.empty()) {
+ DijkstraNode td = pq.top();
+ RRTNode& t = *td.node;
+ pq.pop();
+ this->bc_.set_pose(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)) {
+ continue;
+ }
+ double cost = td.d + this->cost_build(f, t);
+ this->steer(f, t);
+ if (this->steered_.size() == 0) {
+ break;
+ }
+ unsigned int ss = this->steered_.size();
+ if (this->collide_steered()
+ || 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_;
+ 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_path_cc();
+ if (!this->dn_[i].vi()) {
+ pq.push(this->dn_[i]);
+ }
+ }
+ }
+ }
}
-void RRTExt13::json(Json::Value jvi)
+void
+RRTExt13::compute_path()
{
- return RRTS::json(jvi);
+ RRTS::compute_path();
+ if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
+ return;
+ }
+#if 0 // TODO 0.59 should work for sc4-1-0 only.
+ if (this->goal_.cc() * 0.59 > this->last_goal_cc_
+ && this->last_goal_cc_ != 0.0) {
+ return;
+ }
+#endif
+ bool measure_time = false;
+ if (this->opath_.size() == 0) {
+ this->opath_ = this->path_;
+ this->ogoal_cc_ = this->goal_.cc();
+ measure_time = true;
+ }
+ if (measure_time) {
+ this->otime_ = -this->ter_.scnt();
+ }
+ double curr_cc = this->goal_.cc();
+ double last_cc = curr_cc + 1.0;
+ while (curr_cc < last_cc) {
+ last_cc = curr_cc;
+ RRTS::compute_path();
+ this->interesting_forward();
+ this->dijkstra_forward();
+ RRTS::compute_path();
+ this->interesting_backward();
+ this->dijkstra_backward();
+ RRTS::compute_path();
+ curr_cc = this->goal_.cc();
+ }
+ if (measure_time) {
+ this->otime_ += this->ter_.scnt();
+ }
}
+
+RRTExt13::RRTExt13()
+{
+ this->opath_.reserve(10000);
+ this->dn_.reserve(10000);
+}
+
+Json::Value
+RRTExt13::json() const
+{
+ auto jvo = RRTS::json();
+ unsigned int i = 0;
+ for (auto n: this->opath_) {
+ jvo["opath"][i][0] = n->x();
+ jvo["opath"][i][1] = n->y();
+ jvo["opath"][i][2] = n->h();
+ i++;
+ }
+ jvo["ogoal_cc"] = this->ogoal_cc_;
+ jvo["otime"] = this->otime_;
+ return jvo;
+}
+
+void
+RRTExt13::json(Json::Value jvi)
+{
+ RRTS::json(jvi);
+}
+
+void
+RRTExt13::reset()
+{
+ RRTS::reset();
+ this->opath_.clear();
+}
+
+} // namespace rrts
projects / hubacji1 / rrts.git / blobdiff