From: Jiri Vlasak Date: Tue, 14 Mar 2023 21:07:38 +0000 (+0100) Subject: Rename rrts variables and methods X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/commitdiff_plain/d091467a1fd22b8b85bca0fad37738aa9d06ccdd Rename rrts variables and methods --- diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh index 4d50145..d1b881d 100644 --- a/rrts/incl/rrts.hh +++ b/rrts/incl/rrts.hh @@ -82,23 +82,24 @@ public: /*! RRT* algorithm basic class. */ class RRTS { +private: + BicycleCar _bc; + RRTGoal _goal; + unsigned int _icnt = 0; + unsigned int _icnt_max = 1000; + Ter _ter; + std::default_random_engine _gen; + std::vector _nodes; + std::vector _steered; + std::vector _path; + RRTNode* _nn = nullptr; + std::vector _nv; + double _cost = 0.0; + double _eta = 0.5; + double _time = 0.0; + double _last_path_cost = 0.0; + std::vector _last_path; protected: - BicycleCar bc_; - RRTGoal goal_; - unsigned int icnt_ = 0; - unsigned int _imax = 1000; - Ter ter_; - std::default_random_engine gen_; - std::vector nodes_; - std::vector steered_; - std::vector path_; - RRTNode* nn_ = nullptr; - std::vector nv_; - double cost_ = 0.0; - double eta_ = 0.5; - double time_ = 0.0; - double last_goal_cc_ = 0.0; - std::vector last_path_; void recompute_cc(RRTNode* g); void recompute_path_cc(); double min_gamma_eta() const; @@ -108,6 +109,7 @@ protected: bool connect(); void rewire(); bool goal_drivable_from(RRTNode const& f); +protected: virtual void store(RRTNode n); virtual double cost_build(RRTNode const& f, RRTNode const& t) const; virtual double cost_search(RRTNode const& f, RRTNode const& t) const; @@ -121,34 +123,52 @@ protected: public: RRTS(); - /*! Set internal bicycle car. */ - BicycleCar &bc(); + /*! Get bicycle car used in the planner. */ + BicycleCar& bc(void) const; - /*! Set maximum number of iterations before reset. */ - void set_imax_reset(unsigned int i); + /*! Set pose of the bicycle car used in the planner. */ + void bc(double x, double y, double h); + + /*! Get goal. */ + RRTGoal& goal(void) const; /*! Set goal. */ - void set_goal(double x, double y, double b, double e); + void goal(double x, double y, double b, double e); + + /*! Get number of iterations. */ + unsigned int icnt(void) const; + + /*! Set number of iterations. */ + void icnt(unsigned int i); + + /*! Get maximum number of iterations before reset. */ + unsigned int icnt_max(void) const; + + /*! Set maximum number of iterations before reset. */ + void icnt_max(unsigned int i); + + /*! Return elapsed time. */ + double scnt() const; /*! Set start. */ - void set_start(double x, double y, double h); + void start(double x, double y, double h); /*! Get path. */ - std::vector get_path() const; + std::vector path() const; /*! Get path cost. */ - double get_path_cost() const; + double path_cost() const; - /*! Get iterations counter. */ - unsigned int icnt() const; - - /*! Set iterations counter. */ - void icnt(unsigned int i); + /*! Get cost of the last path. */ + double last_path_cost(void) const; - /*! Return elapsed time. */ - double scnt() 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; + + /*! Set eta. */ void eta(double e); /*! Generate JSON output. */ @@ -156,8 +176,8 @@ public: /*! Load JSON input. */ void json(Json::Value jvi); - - /*! Run next RRT* iteration. */ +public: + /*! Run next RRT* iteration. Return True if should continue. */ virtual bool next(); /*! Reset the algorithm. */ diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc index b550224..7443bff 100644 --- a/rrts/src/rrts.cc +++ b/rrts/src/rrts.cc @@ -249,12 +249,12 @@ RRTS::find_nv(RRTNode const& t) void RRTS::compute_path() { - this->path_.clear(); - RRTNode *g = &this->goal_; + this->_path.clear(); + RRTNode *g = &this->goal(); if (g->p() == nullptr) { return; } - while (g != nullptr && this->path_.size() < 10000) { + while (g != nullptr && this->_path.size() < 10000) { /* FIXME in ext13 * * There shouldn't be this->path_.size() < 10000 condition. @@ -262,91 +262,115 @@ RRTS::compute_path() * RRTExt13::compute_path tends to re-allocate this->path_ * infinitely. There's probably node->p() = &node somewhere... */ - this->path_.push_back(g); + this->_path.push_back(g); g = g->p(); } - std::reverse(this->path_.begin(), this->path_.end()); + std::reverse(this->_path.begin(), this->_path.end()); } RRTS::RRTS() : goal_(0.0, 0.0, 0.0, 0.0), gen_(std::random_device{}()) { - this->nodes_.reserve(4000000); - this->steered_.reserve(1000); - this->path_.reserve(10000); - this->nv_.reserve(1000); + this->_nodes.reserve(4000000); + this->_steered.reserve(1000); + this->_path.reserve(10000); + this->_nv.reserve(1000); this->store(RRTNode()); // root } -BicycleCar & -RRTS::bc() +BicycleCar& +RRTS::bc(void) const { - return this->bc_; + return this->_bc; +} + +RRTGoal& +RRTS::goal(void) const +{ + return this->_goal; } void -RRTS::set_imax_reset(unsigned int i) +RRTS::goal(double x, double y, double b, double e) +{ + this->_goal = RRTGoal(x, y, b, e); +} + +unsigned int +RRTS::icnt(void) const +{ + return this->_icnt; +} + +void +RRTS::icnt(unsigned int i) +{ + this->_icnt = i; +} + +unsigned int +RRTS::icnt_max(void) const { - this->_imax = i; + return this->_icnt_max; } void -RRTS::set_goal(double x, double y, double b, double e) +RRTS::icnt_max(unsigned int i) { - this->goal_ = RRTGoal(x, y, b, e); + this->_icnt_max = i; +} + +double +RRTS::scnt() const +{ + return this->_ter.scnt(); } void -RRTS::set_start(double x, double y, double h) +RRTS::start(double x, double y, double h) { - this->nodes_.front().x(x); - this->nodes_.front().y(y); - this->nodes_.front().h(h); + this->_nodes.front().x(x); + this->_nodes.front().y(y); + this->_nodes.front().h(h); } std::vector -RRTS::get_path() const +RRTS::path() const { std::vector path; - for (auto n: this->path_) { + for (auto n: this->_path) { path.push_back(Pose(n->x(), n->y(), n->h())); } return path; } double -RRTS::get_path_cost() const +RRTS::path_cost() const { - return this->goal_.cc(); + return this->goal().cc(); } -unsigned int -RRTS::icnt() const +double +RRTS::last_path_cost(void) const { - return this->icnt_; + return this->_last_path_cost; } void -RRTS::icnt(unsigned int i) -{ - this->icnt_ = i; -} - -double -RRTS::scnt() const +RRTS::last_path_cost(double c) { - return this->ter_.scnt(); + this->_last_path_cost = c; } double RRTS::eta() const { - return this->eta_; + return this->_eta; } void RRTS::eta(double e) { - this->eta_ = e; + this->_eta = e; } Json::Value @@ -354,7 +378,7 @@ RRTS::json() const { Json::Value jvo; unsigned int i = 0; - for (auto n: this->path_) { + for (auto n: this->_path) { // TODO because path() has just x, y, h jvo["path"][i][0] = n->x(); jvo["path"][i][1] = n->y(); jvo["path"][i][2] = n->h(); @@ -362,8 +386,8 @@ RRTS::json() const jvo["path"][i][4] = n->st(); i++; } - jvo["goal_cc"] = this->goal_.cc(); - jvo["time"] = this->time_; + jvo["goal_cc"] = this->goal().cc(); + jvo["time"] = this->scnt(); return jvo; } @@ -372,16 +396,19 @@ RRTS::json(Json::Value jvi) { assert(jvi["init"] != Json::nullValue); assert(jvi["goal"] != Json::nullValue); - this->nodes_.front().x(jvi["init"][0].asDouble()); - this->nodes_.front().y(jvi["init"][1].asDouble()); - this->nodes_.front().h(jvi["init"][2].asDouble()); + this->start( + jvi["init"][0].asDouble(), + jvi["init"][1].asDouble(), + jvi["init"][2].asDouble()); if (jvi["goal"].size() == 4) { - this->goal_ = RRTGoal(jvi["goal"][0].asDouble(), + this->goal( + jvi["goal"][0].asDouble(), jvi["goal"][1].asDouble(), jvi["goal"][2].asDouble(), jvi["goal"][3].asDouble()); } else { - this->goal_ = RRTGoal(jvi["goal"][0].asDouble(), + this->goal( + jvi["goal"][0].asDouble(), jvi["goal"][1].asDouble(), jvi["goal"][2].asDouble(), jvi["goal"][2].asDouble()); @@ -457,23 +484,25 @@ RRTS::next() void RRTS::reset() { - if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) { - this->last_goal_cc_ = this->goal_.cc(); - this->last_path_.clear(); - for (auto n: this->path_) { - this->last_path_.push_back(*n); + 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); } } - 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()); - this->nv_.clear(); - this->nn_ = nullptr; - this->bc_.x(0); - this->bc_.y(0); - this->bc_.h(0); + this->goal( + 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()); + this->_nv.clear(); + this->_nn = nullptr; + this->_bc = BicycleCar(); } } // namespace rrts