return n1.node->cc() > n2.node->cc();
}
+int
+RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
+ DijkstraNode const& n2)
+{
+ return n1.d > n2.d;
+}
+
void
RRTExt13::interesting_forward()
{
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);
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
}
{
std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
DijkstraNodeComparator> pq;
- this->dn_.back().vi();
- pq.push(this->dn_.back());
+ this->dn_.front().vi();
+ pq.push(this->dn_.front());
while (!pq.empty()) {
DijkstraNode td = pq.top();
RRTNode& t = *td.node;
pq.pop();
- for (unsigned int i = td.i - 1; i > 0; i--) {
+ 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;
- double cost = f.cc() + this->cost_build(f, t);
+ 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;
}
- if (this->collide_steered()) {
+ 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) < this->eta_;
- if (cost < t.cc() && td && tn) {
+ 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]);
}