}
};
-bool RRTBase::optp_dijkstra()
+bool RRTBase::optp_dijkstra(
+ std::vector<RRTNode *> &cusps,
+ std::vector<int> &npi)
{
- if (this->tlog().size() == 0)
- return false;
- float oc = this->tlog().back().front()->ccost();
- std::vector<RRTNode *> tmp_cusps;
- for (auto n: this->tlog().back()) {
- if (sgn(n->s()) == 0) {
- tmp_cusps.push_back(n);
- } else if (n->parent() &&
- sgn(n->s()) != sgn(n->parent()->s())) {
- tmp_cusps.push_back(n);
- tmp_cusps.push_back(n->parent());
- }
- }
- if (tmp_cusps.size() < 2)
- return false;
- std::vector<RRTNode *> cusps;
- for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
- if (tmp_cusps[i] != tmp_cusps[i + 1])
- cusps.push_back(tmp_cusps[i]);
- }
- std::reverse(cusps.begin(), cusps.end());
- // Begin of Dijkstra
std::vector<RRTNodeDijkstra> dnodes;
for (unsigned int i = 0; i < cusps.size(); i++)
if (i > 0)
this->cost(cusps[tmp.ni], cusps[i]);
steered = this->steer(cusps[tmp.ni], cusps[i]);
for (unsigned int j = 0; j < steered.size() - 1; j++)
- steered[j]->add_child(
- steered[j + 1],
- this->cost(
- steered[j],
- steered[j + 1]));
+ steered[j]->add_child(steered[j + 1], 1);
if (this->collide(
steered[0],
steered[steered.size() - 1]))
if (!dnodes[i].vi())
pq.push(dnodes[i]);
}
+ for (auto n: steered)
+ delete n;
}
}
- std::vector<int> npi; // new path indexes
unsigned int tmpi = 0;
for (auto n: dnodes) {
if (n.v && n.ni > tmpi)
}
npi.push_back(tmpi);
std::reverse(npi.begin(), npi.end());
+ return true;
+}
+
+bool RRTBase::opt_path()
+{
+ if (this->tlog().size() == 0)
+ return false;
+ float oc = this->tlog().back().front()->ccost();
+ std::vector<RRTNode *> tmp_cusps;
+ for (auto n: this->tlog().back()) {
+ if (sgn(n->s()) == 0) {
+ tmp_cusps.push_back(n);
+ } else if (n->parent() &&
+ sgn(n->s()) != sgn(n->parent()->s())) {
+ tmp_cusps.push_back(n);
+ tmp_cusps.push_back(n->parent());
+ }
+ }
+ if (tmp_cusps.size() < 2)
+ return false;
+ std::vector<RRTNode *> cusps;
+ for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
+ if (tmp_cusps[i] != tmp_cusps[i + 1])
+ cusps.push_back(tmp_cusps[i]);
+ }
+ std::reverse(cusps.begin(), cusps.end());
+ std::vector<int> npi; // new path indexes
+ if (!this->optp_dijkstra(cusps, npi))
+ return false;
RRTNode *pn = cusps[npi[0]];
for (unsigned int i = 0; i < npi.size() - 1; i++) {
for (auto ns: this->steer(cusps[npi[i]], cusps[npi[i + 1]])) {
pn = ns;
}
}
- // End of Dijkstra
this->root()->update_ccost();
if (this->tlog().back().front()->ccost() < oc)
return true;
return false;
}
-bool RRTBase::opt_path()
-{
- return this->optp_dijkstra();
-}
-
bool RRTBase::rebase(RRTNode *nr)
{
if (!nr || this->goal_ == nr || this->root_ == nr)