+#include <algorithm>
#include <queue>
#include "rrtext.hh"
}
void
-RRTExt13::pick_interesting()
+RRTExt13::interesting_forward()
{
this->dn_.clear();
this->dn_.reserve(this->path_.size());
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++) {
- //unsigned int i = 0; { // just for cusp, comment out drivable section
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 (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())
- || njl.edist(nj) < this->eta_) {
+ 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::interesting_backward()
+{
+ 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
+ this->dn_.push_back(DijkstraNode(this->opath_.back()));
+ this->dn_.back().i = this->dn_.size() - 1;
+ 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;
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_.push_back(DijkstraNode(this->opath_.front()));
this->dn_.back().i = this->dn_.size() - 1;
#endif
}
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_;
+ 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());
RRTExt13::compute_path()
{
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) {
if (measure_time) {
this->otime_ = -this->ter_.scnt();
}
- this->pick_interesting();
- this->dijkstra_forward();
-#if 0 // TODO Fix as the code does not always finish.
- RRTS::compute_path();
- this->pick_interesting();
- this->dijkstra_backward();
-#endif
- RRTS::compute_path();
+ 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();
}
{
RRTS::reset();
this->opath_.clear();
- this->ogoal_cc_ = 0.0;
- this->otime_ = 0.0;
}
} // namespace rrts