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)
-{
- return n1.d > n2.d;
-}
-
void
RRTExt13::interesting_forward()
{
this->dn_.clear();
- this->dn_.reserve(this->path_.size());
+ 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;
for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
RRTNode& ni = *this->opath_[i];
- this->bc_.set_pose(ni);
+ this->set_bc_pose_to(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)) {
+ 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())) {
+ if (nj.p_is_cusp()) {
this->dn_.push_back(DijkstraNode(&njl));
this->dn_.back().i = this->dn_.size() - 1;
}
RRTExt13::interesting_backward()
{
this->dn_.clear();
- this->dn_.reserve(this->path_.size());
+ this->dn_.reserve(this->_path.size());
#if 0 // all path poses are interesting
for (auto n: this->opath_) {
this->dn_.push_back(DijkstraNode(n));
return;
#endif
#if 1 // cusp and 1st non-drivable path poses are interesting
- double goal_cc = this->goal_.cc();
+ 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);
+ this->set_bc_pose_to(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)) {
+ 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())) {
+ if (nj.p_is_cusp()) {
this->dn_.push_back(DijkstraNode(&njl));
this->dn_.back().i = this->dn_.size() - 1;
this->dn_.back().d = goal_cc - njl.cc();
pq.push(this->dn_.front());
while (!pq.empty()) {
DijkstraNode fd = pq.top();
- RRTNode& f = *fd.node;
+ RRTNode& f = *fd.p_rrtnode;
pq.pop();
- this->bc_.set_pose(f);
+ this->set_bc_pose_to(f);
for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
- RRTNode& t = *this->dn_[i].node;
- if (!this->bc_.drivable(t)) {
+ RRTNode& t = *this->dn_[i].p_rrtnode;
+ if (!this->_bc.drivable(t)) {
continue;
}
double cost = f.cc() + this->cost_build(f, t);
this->steer(f, t);
- if (this->steered_.size() == 0) {
+ if (this->_steered.size() == 0) {
break;
}
- unsigned int ss = this->steered_.size();
+ unsigned int ss = this->_steered.size();
if (this->collide_steered()
- || ss != this->steered_.size()) {
+ || 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_;
+ // The first steered is the same as f.
+ this->_steered.erase(this->_steered.begin());
+ this->set_bc_pose_to(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));
+ t.p(this->_nodes.back(), true);
+ t.c(this->cost_build(this->_nodes.back(), t));
if (!this->dn_[i].vi()) {
pq.push(this->dn_[i]);
}
pq.push(this->dn_.front());
while (!pq.empty()) {
DijkstraNode td = pq.top();
- RRTNode& t = *td.node;
+ RRTNode& t = *td.p_rrtnode;
pq.pop();
- this->bc_.set_pose(t);
+ this->set_bc_pose_to(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)) {
+ RRTNode& f = *this->dn_[i].p_rrtnode;
+ if (!this->_bc.drivable(f)) {
continue;
}
double cost = td.d + this->cost_build(f, t);
this->steer(f, t);
- if (this->steered_.size() == 0) {
+ if (this->_steered.size() == 0) {
break;
}
- unsigned int ss = this->steered_.size();
+ unsigned int ss = this->_steered.size();
if (this->collide_steered()
- || ss != this->steered_.size()) {
+ || 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_;
+ // The first steered is the same as f.
+ this->_steered.erase(this->_steered.begin());
+ this->set_bc_pose_to(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_cc(&t);
+ t.p(this->_nodes.back(), true);
+ t.c(this->cost_build(this->_nodes.back(), t));
+ this->recompute_cc_for_predecessors_and(&t);
if (!this->dn_[i].vi()) {
pq.push(this->dn_[i]);
}
}
void
-RRTExt13::compute_path()
+RRTExt13::compute_path(void)
{
RRTS::compute_path();
- if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
+ if (this->_goal.cc() == 0.0 || this->_path.size() == 0) {
return;
}
#if 1 // TODO 0.59 should work for sc4-1-0 only.
- if (this->goal_.cc() * 0.8 > this->last_goal_cc_
- && this->last_goal_cc_ != 0.0) {
+ if (this->_goal.cc() * 0.8 > this->last_path_cost()
+ && this->last_path_cost() != 0.0) {
return;
}
#endif
bool measure_time = false;
if (this->opath_.size() == 0) {
- this->opath_ = this->path_;
- this->ogoal_cc_ = this->goal_.cc();
+ this->opath_ = this->_path;
+ this->ogoal_cc_ = this->_goal.cc();
measure_time = true;
}
if (measure_time) {
- this->otime_ = -this->ter_.scnt();
+ this->otime_ = -this->scnt();
}
- double curr_cc = this->goal_.cc();
+ double curr_cc = this->_goal.cc();
double last_cc = curr_cc + 1.0;
while (curr_cc < last_cc) {
last_cc = curr_cc;
this->interesting_backward();
this->dijkstra_backward();
RRTS::compute_path();
- curr_cc = this->goal_.cc();
+ curr_cc = this->_goal.cc();
}
if (measure_time) {
- this->otime_ += this->ter_.scnt();
+ this->otime_ += this->scnt();
}
}
}
Json::Value
-RRTExt13::json() const
+RRTExt13::json(void) const
{
auto jvo = RRTS::json();
unsigned int i = 0;
}
void
-RRTExt13::json(Json::Value jvi)
-{
- RRTS::json(jvi);
-}
-
-void
-RRTExt13::reset()
+RRTExt13::reset(void)
{
RRTS::reset();
this->opath_.clear();
projects / hubacji1 / iamcar2.git / blobdiff