From 067d01730fbd1928c4354f6473cd9992e1dec85c Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Wed, 15 Mar 2023 12:08:22 +0100 Subject: [PATCH] Change naming and access convention --- rrts/incl/rrtext.hh | 2 +- rrts/incl/rrts.hh | 26 +++--- rrts/src/rrtext10.cc | 4 +- rrts/src/rrtext13.cc | 74 ++++++++--------- rrts/src/rrtext14.cc | 12 +-- rrts/src/rrtext15.cc | 4 +- rrts/src/rrtext16.cc | 6 +- rrts/src/rrtext17.cc | 2 +- rrts/src/rrtext18.cc | 2 +- rrts/src/rrtext19.cc | 10 +-- rrts/src/rrtext2.cc | 38 ++++----- rrts/src/rrtext21.cc | 6 +- rrts/src/rrtext8.cc | 58 +++++++------- rrts/src/rrts.cc | 187 ++++++++++++++++++++++--------------------- 14 files changed, 214 insertions(+), 217 deletions(-) diff --git a/rrts/incl/rrtext.hh b/rrts/incl/rrtext.hh index 8276477..10e8c26 100644 --- a/rrts/incl/rrtext.hh +++ b/rrts/incl/rrtext.hh @@ -200,7 +200,7 @@ private: KdNode(RRTNode* n); }; KdNode* kdroot_ = nullptr; - std::vector kdnodes_; + std::vector _kdnodes; void store(RRTNode* n, KdNode*& ref, unsigned int lvl); void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl); void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl); diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh index d1b881d..6e6574b 100644 --- a/rrts/incl/rrts.hh +++ b/rrts/incl/rrts.hh @@ -35,7 +35,7 @@ class RRTNode : public virtual Pose, public virtual CarMove { private: double _c = 0.0; double _cc = 0.0; - RRTNode* _p; + RRTNode* _p = nullptr; unsigned int _cusp = 0; int _segment_type = 0; // 0 ~ straight, 1 ~ left, -1 right public: @@ -82,7 +82,7 @@ public: /*! RRT* algorithm basic class. */ class RRTS { -private: +protected: BicycleCar _bc; RRTGoal _goal; unsigned int _icnt = 0; @@ -97,15 +97,13 @@ private: double _cost = 0.0; double _eta = 0.5; double _time = 0.0; - double _last_path_cost = 0.0; std::vector _last_path; protected: - void recompute_cc(RRTNode* g); + double min_gamma_eta(void) const; + bool should_continue(void) const; + void recompute_cc_for_predecessors_and(RRTNode* g); void recompute_path_cc(); - double min_gamma_eta() const; - bool should_continue() const; void join_steered(RRTNode* f); - RRTNode& nn(); bool connect(); void rewire(); bool goal_drivable_from(RRTNode const& f); @@ -116,6 +114,7 @@ protected: virtual void find_nn(RRTNode const& t); virtual void find_nv(RRTNode const& t); virtual void compute_path(); +protected: virtual void steer(RRTNode const& f, RRTNode const& t) = 0; virtual bool collide_steered() = 0; virtual RRTNode sample() = 0; @@ -123,14 +122,11 @@ protected: public: RRTS(); - /*! Get bicycle car used in the planner. */ - BicycleCar& bc(void) const; - /*! Set pose of the bicycle car used in the planner. */ - void bc(double x, double y, double h); + void set_bc_pose_to(Pose const& p); /*! Get goal. */ - RRTGoal& goal(void) const; + RRTGoal const& goal(void) const; /*! Set goal. */ void goal(double x, double y, double b, double e); @@ -147,6 +143,9 @@ public: /*! Set maximum number of iterations before reset. */ void icnt_max(unsigned int i); + /*! Start elapsed time counter. */ + void tstart(void); + /*! Return elapsed time. */ double scnt() const; @@ -162,9 +161,6 @@ public: /*! Get cost of the last path. */ double last_path_cost(void) const; - /*! Get cost of the last path. */ - void last_path_cost(double c); - /*! Get eta, the RRT* constant used in near vertices and steering. */ double eta() const; diff --git a/rrts/src/rrtext10.cc b/rrts/src/rrtext10.cc index f5d9b89..27ec70d 100644 --- a/rrts/src/rrtext10.cc +++ b/rrts/src/rrtext10.cc @@ -14,7 +14,7 @@ RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const { double q0[] = {f.x(), f.y(), f.h()}; double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc_.mtr()); + ReedsSheppStateSpace rsss(this->_bc.mtr()); return rsss.distance(q0, q1); } @@ -24,7 +24,7 @@ RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const double cost = f.edist(t); double heur = std::min(std::abs(t.h() - f.h()), 2 * M_PI - std::abs(t.h() - f.h())); - heur *= this->bc_.mtr(); + heur *= this->_bc.mtr(); cost = std::max(cost, heur); return cost + f.cusp() * 0.1; } diff --git a/rrts/src/rrtext13.cc b/rrts/src/rrtext13.cc index aa6e7c0..147e26b 100644 --- a/rrts/src/rrtext13.cc +++ b/rrts/src/rrtext13.cc @@ -42,7 +42,7 @@ 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)); @@ -55,11 +55,11 @@ RRTExt13::interesting_forward() 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; @@ -80,7 +80,7 @@ void 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)); @@ -90,17 +90,17 @@ RRTExt13::interesting_backward() 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(); @@ -131,29 +131,29 @@ RRTExt13::dijkstra_forward() DijkstraNode fd = pq.top(); RRTNode& f = *fd.node; 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)) { + 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_; + 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()); + t.c(this->cost_build(this->_nodes.back(), t)); if (!this->dn_[i].vi()) { pq.push(this->dn_[i]); } @@ -173,32 +173,32 @@ RRTExt13::dijkstra_backward() DijkstraNode td = pq.top(); RRTNode& t = *td.node; 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)) { + 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_; + 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()); + 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]); } @@ -212,25 +212,25 @@ void RRTExt13::compute_path() { 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; @@ -241,10 +241,10 @@ RRTExt13::compute_path() 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(); } } diff --git a/rrts/src/rrtext14.cc b/rrts/src/rrtext14.cc index 7b70b3e..b5708c0 100644 --- a/rrts/src/rrtext14.cc +++ b/rrts/src/rrtext14.cc @@ -12,21 +12,21 @@ RRTNode RRTExt14::sample() { if (this->circle_r_ == 0.0) { - RRTNode& f = this->nodes_.front(); - RRTNode& g = this->goal_; + RRTNode& f = this->_nodes.front(); + RRTNode& g = this->_goal; double dx = g.x() - f.x(); double dy = g.y() - f.y(); this->circle_r_ = sqrt(dx * dx + dy * dy); this->circle_c_.x((f.x() + g.x()) / 2.0); this->circle_c_.y((f.y() + g.y()) / 2.0); - return this->goal_; + return this->_goal; } - double r = this->circle_r_ * sqrt(this->udr_(this->gen_)); - double theta = this->udt_(this->gen_); + double r = this->circle_r_ * sqrt(this->udr_(this->_gen)); + double theta = this->udt_(this->_gen); RRTNode rs; rs.x(this->circle_c_.x() + r * cos(theta)); rs.y(this->circle_c_.y() + r * sin(theta)); - rs.h(this->udh_(this->gen_)); + rs.h(this->udh_(this->_gen)); return rs; } diff --git a/rrts/src/rrtext15.cc b/rrts/src/rrtext15.cc index 813483c..f2c5476 100644 --- a/rrts/src/rrtext15.cc +++ b/rrts/src/rrtext15.cc @@ -30,10 +30,10 @@ RRTExt15::next() { bool c = RRTS::next(); if (this->log_path_cost_.size() == 0) { - this->log_path_cost_.push_back(this->goal_.cc()); + this->log_path_cost_.push_back(this->_goal.cc()); } else { double lc = this->log_path_cost_.back(); - double gc = this->goal_.cc(); + double gc = this->_goal.cc(); if (gc > 0.0 && (lc == 0.0 || gc < lc)) { this->log_path_cost_.push_back(gc); } else { diff --git a/rrts/src/rrtext16.cc b/rrts/src/rrtext16.cc index d5953db..89c6ba4 100644 --- a/rrts/src/rrtext16.cc +++ b/rrts/src/rrtext16.cc @@ -25,11 +25,11 @@ cb_steer(double q[4], void *w) void RRTExt16::steer(RRTNode const& f, RRTNode const& t) { - this->steered_.clear(); + this->_steered.clear(); double q0[] = {f.x(), f.y(), f.h()}; double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc_.mtr()); - rsss.sample(q0, q1, this->eta_, cb_steer, &this->steered_); + ReedsSheppStateSpace rsss(this->_bc.mtr()); + rsss.sample(q0, q1, this->_eta, cb_steer, &this->_steered); } } // namespace rrts diff --git a/rrts/src/rrtext17.cc b/rrts/src/rrtext17.cc index 9512f20..36d33e2 100644 --- a/rrts/src/rrtext17.cc +++ b/rrts/src/rrtext17.cc @@ -11,7 +11,7 @@ namespace rrts { bool RRTExt17::should_finish() const { - return this->goal_.p() != nullptr || this->icnt_ > this->_imax; + return this->_goal.p() != nullptr || this->icnt() > this->icnt_max(); } } // namespace rrts diff --git a/rrts/src/rrtext18.cc b/rrts/src/rrtext18.cc index aef1ab9..72b451b 100644 --- a/rrts/src/rrtext18.cc +++ b/rrts/src/rrtext18.cc @@ -11,7 +11,7 @@ namespace rrts { bool RRTExt18::should_finish() const { - return this->icnt_ > this->_imax; + return this->icnt() > this->icnt_max(); } } // namespace rrts diff --git a/rrts/src/rrtext19.cc b/rrts/src/rrtext19.cc index 60caa57..9f1c5c8 100644 --- a/rrts/src/rrtext19.cc +++ b/rrts/src/rrtext19.cc @@ -24,17 +24,17 @@ cb_steer(double q[3], double t, void *w) void RRTExt19::steer(RRTNode const &f, RRTNode const &t) { - this->steered_.clear(); + this->_steered.clear(); double q0[] = {f.x(), f.y(), f.h()}; double q1[] = {t.x(), t.y(), t.h()}; DubinsPath path; - int r = dubins_shortest_path(&path, q0, q1, this->bc_.mtr()); + int r = dubins_shortest_path(&path, q0, q1, this->_bc.mtr()); if (r != 0) { return; } - dubins_path_sample_many(&path, this->eta_, cb_steer, &this->steered_); - if (this->steered_.size() > 0) { - this->steered_.front().sp(0.0); + dubins_path_sample_many(&path, this->_eta, cb_steer, &this->_steered); + if (this->_steered.size() > 0) { + this->_steered.front().sp(0.0); } } diff --git a/rrts/src/rrtext2.cc b/rrts/src/rrtext2.cc index 8af7422..3450735 100644 --- a/rrts/src/rrtext2.cc +++ b/rrts/src/rrtext2.cc @@ -30,27 +30,27 @@ bool RRTExt2::collide_steered() { unsigned int i = 0; - for (auto& n: this->steered_) { + for (auto& n: this->_steered) { if (this->collide(n)) { break; } i++; } - this->steered_.erase(this->steered_.begin() + i, this->steered_.end()); - return this->steered_.size() == 0; + this->_steered.erase(this->_steered.begin() + i, this->_steered.end()); + return this->_steered.size() == 0; } RRTExt2::RRTExt2() : RRTS() { this->c2_bc_.count = 4; - this->c2_bc_.verts[0].x = this->bc_.lfx(); - this->c2_bc_.verts[0].y = this->bc_.lfy(); - this->c2_bc_.verts[1].x = this->bc_.lrx(); - this->c2_bc_.verts[1].y = this->bc_.lry(); - this->c2_bc_.verts[2].x = this->bc_.rrx(); - this->c2_bc_.verts[2].y = this->bc_.rry(); - this->c2_bc_.verts[3].x = this->bc_.rfx(); - this->c2_bc_.verts[3].y = this->bc_.rfy(); + this->c2_bc_.verts[0].x = this->_bc.lfx(); + this->c2_bc_.verts[0].y = this->_bc.lfy(); + this->c2_bc_.verts[1].x = this->_bc.lrx(); + this->c2_bc_.verts[1].y = this->_bc.lry(); + this->c2_bc_.verts[2].x = this->_bc.rrx(); + this->c2_bc_.verts[2].y = this->_bc.rry(); + this->c2_bc_.verts[3].x = this->_bc.rfx(); + this->c2_bc_.verts[3].y = this->_bc.rfy(); } void @@ -58,14 +58,14 @@ RRTExt2::reset() { RRTS::reset(); this->c2_bc_.count = 4; - this->c2_bc_.verts[0].x = this->bc_.lfx(); - this->c2_bc_.verts[0].y = this->bc_.lfy(); - this->c2_bc_.verts[1].x = this->bc_.lrx(); - this->c2_bc_.verts[1].y = this->bc_.lry(); - this->c2_bc_.verts[2].x = this->bc_.rrx(); - this->c2_bc_.verts[2].y = this->bc_.rry(); - this->c2_bc_.verts[3].x = this->bc_.rfx(); - this->c2_bc_.verts[3].y = this->bc_.rfy(); + this->c2_bc_.verts[0].x = this->_bc.lfx(); + this->c2_bc_.verts[0].y = this->_bc.lfy(); + this->c2_bc_.verts[1].x = this->_bc.lrx(); + this->c2_bc_.verts[1].y = this->_bc.lry(); + this->c2_bc_.verts[2].x = this->_bc.rrx(); + this->c2_bc_.verts[2].y = this->_bc.rry(); + this->c2_bc_.verts[3].x = this->_bc.rfx(); + this->c2_bc_.verts[3].y = this->_bc.rfy(); } Json::Value diff --git a/rrts/src/rrtext21.cc b/rrts/src/rrtext21.cc index df7cd94..7c375af 100644 --- a/rrts/src/rrtext21.cc +++ b/rrts/src/rrtext21.cc @@ -30,14 +30,14 @@ bool RRTExt21::collide_steered() { unsigned int i = 0; - for (auto &n: this->steered_) { + for (auto &n: this->_steered) { if (this->collide(n)) { break; } i++; } - this->steered_.erase(this->steered_.begin() + i, this->steered_.end()); - return this->steered_.size() == 0; + this->_steered.erase(this->_steered.begin() + i, this->_steered.end()); + return this->_steered.size() == 0; } void diff --git a/rrts/src/rrtext8.cc b/rrts/src/rrtext8.cc index a9bf99e..5f7c6cd 100644 --- a/rrts/src/rrtext8.cc +++ b/rrts/src/rrtext8.cc @@ -16,8 +16,8 @@ void RRTExt8::store(RRTNode* n, KdNode*& ref, unsigned int lvl) { if (ref == nullptr) { - this->kdnodes_.push_back(KdNode(n)); - ref = &this->kdnodes_.back(); + this->_kdnodes.push_back(KdNode(n)); + ref = &this->_kdnodes.back(); } else if (lvl % 3 == 0 && n->x() < ref->node->x()) { this->store(n, ref->left, lvl + 1); } else if (lvl % 3 == 0) { @@ -39,39 +39,39 @@ RRTExt8::find_nn(RRTNode const& t, KdNode const* const ref, unsigned int lvl) if (ref == nullptr) { return; } - if (this->cost_search(*ref->node, t) < this->cost_ - || this->nn_ == nullptr) { - this->nn_ = ref->node; - this->cost_ = this->cost_search(*ref->node, t); + if (this->cost_search(*ref->node, t) < this->_cost + || this->_nn == nullptr) { + this->_nn = ref->node; + this->_cost = this->cost_search(*ref->node, t); } if (lvl % 3 == 0 && t.x() < ref->node->x()) { this->find_nn(t, ref->left, lvl + 1); - if (ref->node->x() - t.x() < this->cost_) { + if (ref->node->x() - t.x() < this->_cost) { this->find_nn(t, ref->right, lvl + 1); } } else if (lvl % 3 == 0) { this->find_nn(t, ref->right, lvl + 1); - if (t.x() - ref->node->x() < this->cost_) { + if (t.x() - ref->node->x() < this->_cost) { this->find_nn(t, ref->left, lvl + 1); } } else if (lvl % 3 == 1 && t.y() < ref->node->y()) { this->find_nn(t, ref->left, lvl + 1); - if (ref->node->y() - t.y() < this->cost_) { + if (ref->node->y() - t.y() < this->_cost) { this->find_nn(t, ref->right, lvl + 1); } } else if (lvl % 3 == 1) { this->find_nn(t, ref->right, lvl + 1); - if (t.y() - ref->node->y() < this->cost_) { + if (t.y() - ref->node->y() < this->_cost) { this->find_nn(t, ref->left, lvl + 1); } } else if (lvl % 3 == 2 && t.h() < ref->node->h()) { this->find_nn(t, ref->left, lvl + 1); - if (ref->node->h() - t.h() < this->cost_) { + if (ref->node->h() - t.h() < this->_cost) { this->find_nn(t, ref->right, lvl + 1); } } else { this->find_nn(t, ref->right, lvl + 1); - if (t.h() - ref->node->h() < this->cost_) { + if (t.h() - ref->node->h() < this->_cost) { this->find_nn(t, ref->left, lvl + 1); } } @@ -83,37 +83,37 @@ RRTExt8::find_nv(RRTNode const& t, KdNode const* const ref, unsigned int lvl) if (ref == nullptr) { return; } - if (this->cost_search(*ref->node, t) < this->cost_) { - this->nv_.push_back(ref->node); + if (this->cost_search(*ref->node, t) < this->_cost) { + this->_nv.push_back(ref->node); } if (lvl % 3 == 0 && t.x() < ref->node->x()) { this->find_nv(t, ref->left, lvl + 1); - if (ref->node->x() - t.x() < this->cost_) { + if (ref->node->x() - t.x() < this->_cost) { this->find_nv(t, ref->right, lvl + 1); } } else if (lvl % 3 == 0) { this->find_nv(t, ref->right, lvl + 1); - if (t.x() - ref->node->x() < this->cost_) { + if (t.x() - ref->node->x() < this->_cost) { this->find_nv(t, ref->left, lvl + 1); } } else if (lvl % 3 == 1 && t.y() < ref->node->y()) { this->find_nv(t, ref->left, lvl + 1); - if (ref->node->y() - t.y() < this->cost_) { + if (ref->node->y() - t.y() < this->_cost) { this->find_nv(t, ref->right, lvl + 1); } } else if (lvl % 3 == 1) { this->find_nv(t, ref->right, lvl + 1); - if (t.y() - ref->node->y() < this->cost_) { + if (t.y() - ref->node->y() < this->_cost) { this->find_nv(t, ref->left, lvl + 1); } } else if (lvl % 3 == 2 && t.h() < ref->node->h()) { this->find_nv(t, ref->left, lvl + 1); - if (ref->node->h() - t.h() < this->cost_) { + if (ref->node->h() - t.h() < this->_cost) { this->find_nv(t, ref->right, lvl + 1); } } else { this->find_nv(t, ref->right, lvl + 1); - if (t.h() - ref->node->h() < this->cost_) { + if (t.h() - ref->node->h() < this->_cost) { this->find_nv(t, ref->left, lvl + 1); } } @@ -121,39 +121,39 @@ RRTExt8::find_nv(RRTNode const& t, KdNode const* const ref, unsigned int lvl) RRTExt8::RRTExt8() : RRTS() { - this->kdnodes_.reserve(this->nodes_.capacity()); - this->store(&this->nodes_.front(), this->kdroot_, 0); + this->_kdnodes.reserve(this->_nodes.capacity()); + this->store(&this->_nodes.front(), this->kdroot_, 0); } void RRTExt8::reset() { RRTS::reset(); - this->kdnodes_.clear(); + this->_kdnodes.clear(); this->kdroot_ = nullptr; - this->store(&this->nodes_.front(), this->kdroot_, 0); + this->store(&this->_nodes.front(), this->kdroot_, 0); } void RRTExt8::store(RRTNode n) { RRTS::store(n); - this->store(&this->nodes_.back(), this->kdroot_, 0); + this->store(&this->_nodes.back(), this->kdroot_, 0); } void RRTExt8::find_nn(RRTNode const& t) { - this->nn_ = &this->nodes_.front(); - this->cost_ = this->cost_search(*this->nn_, t); + this->_nn = &this->_nodes.front(); + this->_cost = this->cost_search(*this->_nn, t); this->find_nn(t, this->kdroot_, 0); } void RRTExt8::find_nv(RRTNode const& t) { - this->nv_.clear(); - this->cost_ = this->min_gamma_eta(); + this->_nv.clear(); + this->_cost = this->min_gamma_eta(); this->find_nv(t, this->kdroot_, 0); } diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc index 7443bff..289a771 100644 --- a/rrts/src/rrts.cc +++ b/rrts/src/rrts.cc @@ -96,32 +96,34 @@ RRTNode::operator==(RRTNode const& n) } void -RRTS::recompute_cc(RRTNode* g) +RRTS::recompute_cc_for_predecessors_and(RRTNode* g) { - this->path_.clear(); + assert(this->_path.size() == 0); while (g != nullptr) { - this->path_.push_back(g); + this->_path.push_back(g); g = g->p(); } - std::reverse(this->path_.begin(), this->path_.end()); - for (unsigned int i = 1; i < this->path_.size(); i++) { - this->path_[i]->c(this->cost_build(*this->path_[i - 1], - *this->path_[i])); + std::reverse(this->_path.begin(), this->_path.end()); + for (unsigned int i = 1; i < this->_path.size(); i++) { + this->_path[i]->c(this->cost_build( + *this->_path[i - 1], + *this->_path[i])); } + this->_path.clear(); } void RRTS::recompute_path_cc() { - this->recompute_cc(&this->goal_); + this->recompute_cc_for_predecessors_and(&this->_goal); } double RRTS::min_gamma_eta() const { - double ns = this->nodes_.size(); + double ns = this->_nodes.size(); double gamma = pow(log(ns) / ns, 1.0 / 3.0); - return std::min(gamma, this->eta_); + return std::min(gamma, this->eta()); } bool @@ -133,28 +135,22 @@ RRTS::should_continue() const void RRTS::join_steered(RRTNode* f) { - while (this->steered_.size() > 0) { - this->store(this->steered_.front()); - RRTNode* t = &this->nodes_.back(); + while (this->_steered.size() > 0) { + this->store(this->_steered.front()); + RRTNode* t = &this->_nodes.back(); t->p(*f); t->c(this->cost_build(*f, *t)); t->cusp(*f); - this->steered_.erase(this->steered_.begin()); + this->_steered.erase(this->_steered.begin()); f = t; } } -RRTNode& -RRTS::nn() -{ - return *this->nn_; -} - bool RRTS::connect() { - RRTNode* f = this->nn_; - RRTNode* t = &this->steered_.front(); + RRTNode* f = this->_nn; + RRTNode* t = &this->_steered.front(); #if USE_RRTS double cost = f->cc() + this->cost_build(*f, *t); for (auto n: this->nv_) { @@ -165,30 +161,30 @@ RRTS::connect() } } // Check if it's possible to drive from *f to *t. If not, then fallback - // to *f = nn_. This could be also solved by additional steer from *f to + // to *f = _nn. This could be also solved by additional steer from *f to // *t instead of the following code. - this->bc_.set_pose(*f); - if (!this->bc_.drivable(*t)) { - f = this->nn_; + this->set_bc_pose_to(*f); + if (!this->_bc.drivable(*t)) { + f = this->_nn; } #endif - this->store(this->steered_.front()); - t = &this->nodes_.back(); + this->store(this->_steered.front()); + t = &this->_nodes.back(); t->p(*f); t->c(this->cost_build(*f, *t)); t->cusp(*f); - this->steered_.erase(this->steered_.begin()); + this->_steered.erase(this->_steered.begin()); return true; } void RRTS::rewire() { - RRTNode *f = &this->nodes_.back(); - for (auto n: this->nv_) { + RRTNode *f = &this->_nodes.back(); + for (auto n: this->_nv) { double fc = f->cc() + this->cost_build(*f, *n); - this->bc_.set_pose(*f); - bool drivable = this->bc_.drivable(*n); + this->set_bc_pose_to(*f); + bool drivable = this->_bc.drivable(*n); if (drivable && fc < n->cc()) { n->p(*f); n->c(this->cost_build(*f, *n)); @@ -199,14 +195,14 @@ RRTS::rewire() bool RRTS::goal_drivable_from(RRTNode const& f) { - this->bc_.set_pose(f); - return this->bc_.drivable(this->goal_); + this->set_bc_pose_to(f); + return this->_bc.drivable(this->_goal); } void RRTS::store(RRTNode n) { - this->nodes_.push_back(n); + this->_nodes.push_back(n); } double @@ -224,12 +220,12 @@ RRTS::cost_search(RRTNode const& f, RRTNode const& t) const void RRTS::find_nn(RRTNode const& t) { - this->nn_ = &this->nodes_.front(); - this->cost_ = this->cost_search(*this->nn_, t); - for (auto& f: this->nodes_) { - if (this->cost_search(f, t) < this->cost_) { - this->nn_ = &f; - this->cost_ = this->cost_search(f, t); + this->_nn = &this->_nodes.front(); + this->_cost = this->cost_search(*this->_nn, t); + for (auto& f: this->_nodes) { + if (this->cost_search(f, t) < this->_cost) { + this->_nn = &f; + this->_cost = this->cost_search(f, t); } } } @@ -237,11 +233,11 @@ RRTS::find_nn(RRTNode const& t) void RRTS::find_nv(RRTNode const& t) { - this->nv_.clear(); - this->cost_ = this->min_gamma_eta(); - for (auto& f: this->nodes_) { - if (this->cost_search(f, t) < this->cost_) { - this->nv_.push_back(&f); + this->_nv.clear(); + this->_cost = this->min_gamma_eta(); + for (auto& f: this->_nodes) { + if (this->cost_search(f, t) < this->_cost) { + this->_nv.push_back(&f); } } } @@ -250,16 +246,16 @@ void RRTS::compute_path() { this->_path.clear(); - RRTNode *g = &this->goal(); + RRTNode *g = &this->_goal; if (g->p() == nullptr) { return; } while (g != nullptr && this->_path.size() < 10000) { /* FIXME in ext13 * - * There shouldn't be this->path_.size() < 10000 condition. + * There shouldn't be this->_path.size() < 10000 condition. * However, the RRTS::compute_path() called from - * RRTExt13::compute_path tends to re-allocate this->path_ + * RRTExt13::compute_path tends to re-allocate this->_path * infinitely. There's probably node->p() = &node somewhere... */ this->_path.push_back(g); @@ -268,7 +264,7 @@ RRTS::compute_path() std::reverse(this->_path.begin(), this->_path.end()); } -RRTS::RRTS() : goal_(0.0, 0.0, 0.0, 0.0), gen_(std::random_device{}()) +RRTS::RRTS() : _goal(0.0, 0.0, 0.0, 0.0), _gen(std::random_device{}()) { this->_nodes.reserve(4000000); this->_steered.reserve(1000); @@ -277,13 +273,13 @@ RRTS::RRTS() : goal_(0.0, 0.0, 0.0, 0.0), gen_(std::random_device{}()) this->store(RRTNode()); // root } -BicycleCar& -RRTS::bc(void) const +void +RRTS::set_bc_pose_to(Pose const& p) { - return this->_bc; + this->_bc.set_pose_to(p); } -RRTGoal& +RRTGoal const& RRTS::goal(void) const { return this->_goal; @@ -319,6 +315,12 @@ RRTS::icnt_max(unsigned int i) this->_icnt_max = i; } +void +RRTS::tstart(void) +{ + this->_ter.start(); +} + double RRTS::scnt() const { @@ -346,19 +348,17 @@ RRTS::path() const double RRTS::path_cost() const { - return this->goal().cc(); + return this->_goal.cc(); } double RRTS::last_path_cost(void) const { - return this->_last_path_cost; -} - -void -RRTS::last_path_cost(double c) -{ - this->_last_path_cost = c; + if (this->_last_path.size() == 0) { + return 999.9; + } else { + return this->_last_path.back().cc(); + } } double @@ -386,7 +386,7 @@ RRTS::json() const jvo["path"][i][4] = n->st(); i++; } - jvo["goal_cc"] = this->goal().cc(); + jvo["goal_cc"] = this->_goal.cc(); jvo["time"] = this->scnt(); return jvo; } @@ -418,27 +418,26 @@ RRTS::json(Json::Value jvi) bool RRTS::next() { - if (this->icnt_ == 0) { - this->ter_.start(); + if (this->icnt() == 0) { + this->tstart(); } - this->icnt_ += 1; auto rs = this->sample(); #if 1 // anytime RRTs { - double d1 = this->cost_search(this->nodes_.front(), rs); - double d2 = this->cost_search(rs, this->goal_); - if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) { - rs = this->last_path_[rand() % this->last_path_.size()]; + double d1 = this->cost_search(this->_nodes.front(), rs); + double d2 = this->cost_search(rs, this->_goal); + if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) { + rs = this->_last_path[rand() % this->_last_path.size()]; } } #endif this->find_nn(rs); - this->steer(this->nn(), rs); + this->steer(*this->_nn, rs); if (this->collide_steered()) { return this->should_continue(); } #if USE_RRTS - this->find_nv(this->steered_.front()); + this->find_nv(this->_steered.front()); #endif if (!this->connect()) { return this->should_continue(); @@ -446,28 +445,29 @@ RRTS::next() #if USE_RRTS this->rewire(); #endif - unsigned int ss = this->steered_.size(); - this->join_steered(&this->nodes_.back()); - RRTNode* just_added = &this->nodes_.back(); + unsigned int ss = this->_steered.size(); + this->join_steered(&this->_nodes.back()); + RRTNode* just_added = &this->_nodes.back(); bool gf = false; while (ss > 0 && just_added->p() != nullptr) { - this->steer(*just_added, this->goal_); + this->steer(*just_added, this->_goal); if (this->collide_steered()) { ss--; just_added = just_added->p(); continue; } this->join_steered(just_added); - bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_; - bool gd = this->goal_drivable_from(this->nodes_.back()); + bool gn = this->_goal.edist(this->_nodes.back()) < this->eta(); + bool gd = this->goal_drivable_from(this->_nodes.back()); if (gn && gd) { - double nc = this->cost_build(this->nodes_.back(), - this->goal_); - double ncc = this->nodes_.back().cc() + nc; - if (this->goal_.p() == nullptr - || ncc < this->goal_.cc()) { - this->goal_.p(this->nodes_.back()); - this->goal_.c(nc); + double nc = this->cost_build( + this->_nodes.back(), + this->_goal); + double ncc = this->_nodes.back().cc() + nc; + if (this->_goal.p() == nullptr + || ncc < this->_goal.cc()) { + this->_goal.p(this->_nodes.back()); + this->_goal.c(nc); gf = true; } } @@ -477,7 +477,8 @@ RRTS::next() if (gf) { this->compute_path(); } - this->time_ = this->ter_.scnt(); + this->_time = this->scnt(); + this->icnt(this->icnt() + 1); return this->should_continue(); } @@ -486,17 +487,17 @@ RRTS::reset() { if (this->path_cost() != 0.0 && this->path_cost() < this->last_path_cost()) { - this->last_path_cost(this->path_cost); this->_last_path.clear(); for (auto n: this->_path) { this->_last_path.push_back(*n); + // FIXME _last_path nodes' pointers to parents } } - this->goal( - this->goal().x(), - this->goal().y(), - this->goal().b(), - this->goal().e()); + this->_goal = RRTGoal( + this->_goal.x(), + this->_goal.y(), + this->_goal.b(), + this->_goal.e()); this->_path.clear(); this->_steered.clear(); this->_nodes.erase(this->_nodes.begin() + 1, this->_nodes.end()); -- 2.39.2