#include <queue>
#include "rrtext.h"
-std::vector<RRTNode *> RRTExt3::first_path_optimization()
+void RRTExt3::reset()
+{
+ RRTS::reset();
+ this->orig_path().clear();
+ this->first_optimized_path().clear();
+ this->orig_path_cost_ = 9999.9;
+ this->first_optimized_path_cost_ = 9999.9;
+}
+
+void RRTExt3::first_path_optimization()
{
if (this->orig_path().size() == 0) {
this->orig_path_ = RRTS::path();
if (this->orig_path().size() == 0)
- return this->orig_path();
+ return;
else
- this->orig_path_cost(cc(*this->orig_path().back()));
+ this->orig_path_cost(this->orig_path().back()->cc);
}
class DijkstraNode : public RRTNode {
public:
|| n->t(RRTNodeType::connected)
) {
dn.push_back(DijkstraNode(*n));
- dn.back().cc = cc(*n);
+ dn.back().cc = n->cc;
dn.back().s = &dn.back();
dn.back().n = n;
dn.back().i = dncnt++;
}
}
dn.push_back(DijkstraNode(*this->orig_path().back()));
- dn.back().cc = cc(*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++;
ln = &n;
}
if (ln == nullptr || ln->p() == nullptr)
- return this->orig_path();
+ 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->orig_path();
+ return;
this->join_steered(f);
t->p(&this->nodes().back());
t->c(this->cost_build(this->nodes().back(), *t));
ln = ln->p();
}
- return RRTS::path();
+ RRTS::compute_path();
}
-std::vector<RRTNode *> RRTExt3::second_path_optimization()
+void RRTExt3::second_path_optimization()
{
if (this->first_optimized_path().size() == 0) {
- return this->orig_path();
+ return;
}
class DijkstraNode : public RRTNode {
public:
|| n->t(RRTNodeType::connected)
) {
dn.push_back(DijkstraNode(*n));
- dn.back().cc = cc(*n);
+ dn.back().cc = n->cc;
dn.back().s = &dn.back();
dn.back().n = n;
dn.back().i = dncnt++;
}
}
dn.push_back(DijkstraNode(*this->orig_path().back()));
- dn.back().cc = cc(*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++;
}
}
if (fn == nullptr || fn->p() == nullptr)
- return this->first_optimized_path();
+ 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->first_optimized_path();
+ return;
this->join_steered(f);
t->p(&this->nodes().back());
t->c(this->cost_build(this->nodes().back(), *t));
ln = ln->p();
}
- return RRTS::path();
+ RRTS::compute_path();
}
-std::vector<RRTNode *> RRTExt3::path()
+void RRTExt3::compute_path()
{
- this->first_optimized_path_ = this->first_path_optimization();
- if (this->first_optimized_path_.size() > 0)
+ 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(
- cc(*this->first_optimized_path_.back())
+ this->first_optimized_path_.back()->cc
);
- else
- return this->orig_path();
- return this->second_path_optimization();
+ } 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());
}
Json::Value RRTExt3::json()
unsigned int cu = 0;
unsigned int co = 0;
unsigned int pcnt = 0;
- for (auto n: this->path()) {
+ 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();