/*! 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<RRTNode> _nodes;
+ std::vector<RRTNode> _steered;
+ std::vector<RRTNode*> _path;
+ RRTNode* _nn = nullptr;
+ std::vector<RRTNode*> _nv;
+ double _cost = 0.0;
+ double _eta = 0.5;
+ double _time = 0.0;
+ double _last_path_cost = 0.0;
+ std::vector<RRTNode> _last_path;
protected:
- BicycleCar bc_;
- RRTGoal goal_;
- unsigned int icnt_ = 0;
- unsigned int _imax = 1000;
- Ter ter_;
- std::default_random_engine gen_;
- std::vector<RRTNode> nodes_;
- std::vector<RRTNode> steered_;
- std::vector<RRTNode*> path_;
- RRTNode* nn_ = nullptr;
- std::vector<RRTNode*> nv_;
- double cost_ = 0.0;
- double eta_ = 0.5;
- double time_ = 0.0;
- double last_goal_cc_ = 0.0;
- std::vector<RRTNode> last_path_;
void recompute_cc(RRTNode* g);
void recompute_path_cc();
double min_gamma_eta() const;
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;
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<Pose> get_path() const;
+ std::vector<Pose> 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. */
/*! 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. */
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.
* 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<Pose>
-RRTS::get_path() const
+RRTS::path() const
{
std::vector<Pose> 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
{
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();
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;
}
{
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());
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
projects / hubacji1 / iamcar2.git / commitdiff