#define ETA 1.0 // for steer, nv
#define GAMMA(cV) ({ \
- __typeof__ (cV) _cV = (cV); \
- pow(log(_cV) / _cV, 1.0 / 3.0); \
+ __typeof__ (cV) _cV = (cV); \
+ pow(log(_cV) / _cV, 1.0 / 3.0); \
})
/*! \brief Possible type of RRT node.
\param connected The node that branches generated steered path.
*/
class RRTNodeType {
- public:
- static const unsigned int cusp = 1 << 0;
- static const unsigned int connected = 1 << 1;
+ public:
+ static const unsigned int cusp = 1 << 0;
+ static const unsigned int connected = 1 << 1;
};
/*! \brief RRT node basic class.
\param st Steering of the car.
*/
class RRTNode {
- private:
- RRTNode *p_ = nullptr;
- unsigned int t_ = 0;
- // -- from BicycleCar
- // coordinates
- double x_ = 0;
- double y_ = 0;
- double h_ = 0;
- // moving
- double sp_ = 0;
- double st_ = 0;
- public:
- double c_ = 0;
- double cc = 0.0;
- // getters, setters
- double c() const { return this->c_; }
- void c(double c)
- {
- this->c_ = c;
- this->cc = this->p_->cc + this->c_;
- }
-
- RRTNode *p() const { return this->p_; }
- void p(RRTNode *p) { this->p_ = p; }
-
- bool t(unsigned int flag) { return this->t_ & flag; }
- void set_t(unsigned int flag) { this->t_ |= flag; }
- void clear_t(unsigned int flag) { this->t_ &= ~flag; }
-
- // -- from BicycleCar
- // getters, setters
- double x() const { return this->x_; }
- void x(double x) { this->x_ = x; }
-
- double y() const { return this->y_; }
- void y(double y) { this->y_ = y; }
-
- double h() const { return this->h_; }
- void h(double h)
- {
- while (h < -M_PI)
- h += 2 * M_PI;
- while (h > +M_PI)
- h -= 2 * M_PI;
- this->h_ = h;
- }
-
- double sp() const { return this->sp_; }
- void sp(double sp) { this->sp_ = sp; }
-
- double st() const { return this->st_; }
- void st(double st) { this->st_ = st; }
-
- RRTNode();
- RRTNode(const BicycleCar &bc);
- bool operator==(const RRTNode& n);
- friend std::ostream &operator<<(
- std::ostream &out,
- const RRTNode &bc
- )
- {
- out << "[" << bc.x();
- out << "," << bc.y();
- out << "," << bc.h();
- out << "]";
- return out;
- }
+ private:
+ RRTNode *p_ = nullptr;
+ unsigned int t_ = 0;
+ // -- from BicycleCar
+ // coordinates
+ double x_ = 0;
+ double y_ = 0;
+ double h_ = 0;
+ // moving
+ double sp_ = 0;
+ double st_ = 0;
+ public:
+ double c_ = 0;
+ double cc = 0.0;
+ // getters, setters
+ double c() const { return this->c_; }
+ void c(double c)
+ {
+ this->c_ = c;
+ this->cc = this->p_->cc + this->c_;
+ }
+
+ RRTNode *p() const { return this->p_; }
+ void p(RRTNode *p) { this->p_ = p; }
+
+ bool t(unsigned int flag) { return this->t_ & flag; }
+ void set_t(unsigned int flag) { this->t_ |= flag; }
+ void clear_t(unsigned int flag) { this->t_ &= ~flag; }
+
+ // -- from BicycleCar
+ // getters, setters
+ double x() const { return this->x_; }
+ void x(double x) { this->x_ = x; }
+
+ double y() const { return this->y_; }
+ void y(double y) { this->y_ = y; }
+
+ double h() const { return this->h_; }
+ void h(double h)
+ {
+ while (h < -M_PI)
+ h += 2 * M_PI;
+ while (h > +M_PI)
+ h -= 2 * M_PI;
+ this->h_ = h;
+ }
+
+ double sp() const { return this->sp_; }
+ void sp(double sp) { this->sp_ = sp; }
+
+ double st() const { return this->st_; }
+ void st(double st) { this->st_ = st; }
+
+ RRTNode();
+ RRTNode(const BicycleCar &bc);
+ bool operator==(const RRTNode& n);
+ friend std::ostream &operator<<(
+ std::ostream &out,
+ const RRTNode &bc
+ )
+ {
+ out << "[" << bc.x();
+ out << "," << bc.y();
+ out << "," << bc.h();
+ out << "]";
+ return out;
+ }
};
/*! \brief Polygon obstacle basic class.
\param poly Border polygon of the obstacle.
*/
class Obstacle {
- private:
- std::vector<std::tuple<double, double>> poly_;
- public:
- // getters, setters
- std::vector<std::tuple<double, double>> &poly()
- {
- return this->poly_;
- }
-
- Obstacle();
+ private:
+ std::vector<std::tuple<double, double>> poly_;
+ public:
+ // getters, setters
+ std::vector<std::tuple<double, double>> &poly()
+ {
+ return this->poly_;
+ }
+
+ Obstacle();
};
/*! \brief RRT* algorithm basic class.
normal, 1 - uniform, 2 - uniform circle)
*/
class RRTS {
- protected:
- unsigned int icnt_ = 0;
- std::chrono::high_resolution_clock::time_point tstart_;
- double scnt_ = 0;
- double pcnt_ = 0;
- bool gf_ = false;
- int sample_dist_type_ = 0;
-
- std::vector<RRTNode> goals_;
- std::vector<RRTNode> nodes_;
- std::vector<Obstacle> obstacles_;
- std::vector<RRTNode> samples_;
- std::vector<RRTNode> steered_;
- std::vector<RRTNode *> path_;
- double log_path_time_ = 0.1;
- unsigned int log_path_iter_ = 20;
-
- /*! \brief Update and return elapsed time.
- */
- double elapsed();
- /*! \brief Log current path cost.
- */
- void log_path_cost();
- /*! \brief Set normal distribution for sampling.
- */
- void set_sample_normal(
- double x1, double x2,
- double y1, double y2,
- double h1, double h2
- );
- /*! \brief Set uniform distribution for sampling.
- */
- void set_sample_uniform(
- double x1, double x2,
- double y1, double y2,
- double h1, double h2
- );
- /*! \brief Set uniform circle distribution for sampling.
- */
- void set_sample_uniform_circle();
- RRTNode* use_nn; // Used for RRTExt12.
- std::vector<RRTNode> tmp_steered_;
- bool finishit = false;
- double path_cost_before_opt_ = 9999;
-
- BicycleCar bc;
- /*! \brief Store RRT node to tree data structure.
- */
- virtual void store_node(RRTNode n);
-
- // RRT procedures
- std::tuple<bool, unsigned int, unsigned int>
- collide(std::vector<std::tuple<double, double>> &poly);
- virtual std::tuple<bool, unsigned int, unsigned int>
- collide_steered_from(RRTNode &f);
- virtual std::tuple<bool, unsigned int, unsigned int>
- collide_tmp_steered_from(RRTNode &f);
- virtual std::tuple<bool, unsigned int, unsigned int>
- collide_two_nodes(RRTNode &f, RRTNode &t);
- void sample();
- std::default_random_engine gen_;
- std::normal_distribution<double> ndx_;
- std::normal_distribution<double> ndy_;
- std::normal_distribution<double> ndh_;
- std::uniform_real_distribution<double> udx_;
- std::uniform_real_distribution<double> udy_;
- std::uniform_real_distribution<double> udh_;
- std::uniform_int_distribution<unsigned int> udi1_;
- std::uniform_int_distribution<unsigned int> udi2_;
- virtual RRTNode *nn(RRTNode &t);
- virtual std::vector<RRTNode *> nv(RRTNode &t);
- void steer(RRTNode &f, RRTNode &t);
- void tmp_steer(RRTNode &f, RRTNode &t);
- virtual void steer1(RRTNode &f, RRTNode &t);
- virtual void steer2(RRTNode &f, RRTNode &t);
- /*! \brief Join steered nodes to RRT data structure
-
- \param f RRT node to join steered nodes to.
- */
- void join_steered(RRTNode *f);
- void join_tmp_steered(RRTNode *f);
- virtual bool goal_found(RRTNode &f);
- // RRT* procedures
- virtual bool connect();
- void rewire();
- public:
- /// ---
- std::vector<double> log_opt_time_;
- std::vector<double> log_path_cost_;
- struct { double x=0; double y=0; double b=0; double e=0; } entry;
- bool entry_set = false;
- struct { double x=0; double y=0; double h=0; } entry1;
- struct { double x=0; double y=0; double h=0; } entry2;
- bool entries_set = false;
- std::vector<RRTNode *> steered1_;
- std::vector<RRTNode *> steered2_;
- /// ---
- /*! \brief Initialize RRT algorithm if needed.
- */
- virtual void init();
- virtual void reset();
- /*! \brief Deinitialize RRT algorithm if needed.
- */
- virtual void deinit();
- /*! \brief Return path found by RRT*.
- */
- virtual std::vector<RRTNode *>& path()
- {
- return this->path_;
- }
- virtual void compute_path();
- /*! \brief Return ``true`` if algorithm should stop.
-
- Update counters (iteration, seconds, ...) and return if
- the current iteration should be the last one.
- */
- bool should_stop();
- /*! \brief Return ``true`` if the algorithm should finish.
-
- Finish means that the algorithm will not be resumed.
- */
- bool should_finish();
- /*! \brief Return ``true`` if the algorithm shoud break.
-
- Break means that the algorithm can be resumed.
- */
- bool should_break();
- /*! \brief Return ``true`` if algorithm should continue.
-
- `pcnt_` is set to `scnt_`, so the difference is 0 and it can
- start from scratch. After the `should_continue` is called,
- there must be `while (rrts.next()) {}` loop.
- */
- bool should_continue();
- /*! \brief Run next RRT* iteration.
- */
- virtual bool next();
- /*! \brief Set sampling info.
-
- Based on `sample_dist_type`, set proper distribution
- parameters. The distribution parameters are relative to `front`
- node in `nodes` (init).
-
- For normal sampling:
- \param x1 Mean x value.
- \param x2 Standard deviation of x.
- \param y1 Mean y value.
- \param y2 Standard deviation of y.
- \param h1 Mean h value.
- \param h2 Standard deviation of h.
-
- For uniform sampling:
- \param x1 Minimum x value.
- \param x2 Maximum x value.
- \param y1 Minimum y value.
- \param y2 Maximum y value.
- \param h1 Minimum h value.
- \param h2 Maximum h value.
-
- For uniform circle sampling:
- \param x1 Ignored.
- \param x2 Ignored.
- \param y1 Ignored.
- \param y2 Ignored.
- \param h1 Ignored.
- \param h2 Ignored.
- */
- void set_sample(
- double x1, double x2,
- double y1, double y2,
- double h1, double h2
- );
- /*! \brief Generate JSON output.
- */
- Json::Value json();
- /*! \brief Load JSON input.
- */
- void json(Json::Value jvi);
-
- // RRT procedures
- virtual double cost_build(RRTNode &f, RRTNode &t);
- virtual double cost_search(RRTNode &f, RRTNode &t);
-
- // getters, setters
- unsigned int icnt() const { return this->icnt_; }
- void icnt(unsigned int i) { this->icnt_ = i; }
- double scnt() const { return this->scnt_; }
- bool gf() const { return this->gf_; }
- void gf(bool f) { this->gf_ = f; }
- int sample_dist_type() const { return this->sample_dist_type_;}
- void sample_dist_type(int t) { this->sample_dist_type_ = t; }
- std::vector<RRTNode> &goals() { return this->goals_; }
- std::vector<RRTNode> &nodes() { return this->nodes_; }
- std::vector<Obstacle> &obstacles() { return this->obstacles_; }
- std::vector<RRTNode> &samples() { return this->samples_; }
- std::vector<RRTNode> &steered() { return this->steered_; }
-
- RRTS();
+ protected:
+ unsigned int icnt_ = 0;
+ std::chrono::high_resolution_clock::time_point tstart_;
+ double scnt_ = 0;
+ double pcnt_ = 0;
+ bool gf_ = false;
+ int sample_dist_type_ = 0;
+
+ std::vector<RRTNode> goals_;
+ std::vector<RRTNode> nodes_;
+ std::vector<Obstacle> obstacles_;
+ std::vector<RRTNode> samples_;
+ std::vector<RRTNode> steered_;
+ std::vector<RRTNode *> path_;
+ double log_path_time_ = 0.1;
+ unsigned int log_path_iter_ = 20;
+
+ /*! \brief Update and return elapsed time.
+ */
+ double elapsed();
+ /*! \brief Log current path cost.
+ */
+ void log_path_cost();
+ /*! \brief Set normal distribution for sampling.
+ */
+ void set_sample_normal(
+ double x1, double x2,
+ double y1, double y2,
+ double h1, double h2
+ );
+ /*! \brief Set uniform distribution for sampling.
+ */
+ void set_sample_uniform(
+ double x1, double x2,
+ double y1, double y2,
+ double h1, double h2
+ );
+ /*! \brief Set uniform circle distribution for sampling.
+ */
+ void set_sample_uniform_circle();
+ RRTNode* use_nn; // Used for RRTExt12.
+ std::vector<RRTNode> tmp_steered_;
+ bool finishit = false;
+ double path_cost_before_opt_ = 9999;
+
+ BicycleCar bc;
+ /*! \brief Store RRT node to tree data structure.
+ */
+ virtual void store_node(RRTNode n);
+
+ // RRT procedures
+ std::tuple<bool, unsigned int, unsigned int>
+ collide(std::vector<std::tuple<double, double>> &poly);
+ virtual std::tuple<bool, unsigned int, unsigned int>
+ collide_steered_from(RRTNode &f);
+ virtual std::tuple<bool, unsigned int, unsigned int>
+ collide_tmp_steered_from(RRTNode &f);
+ virtual std::tuple<bool, unsigned int, unsigned int>
+ collide_two_nodes(RRTNode &f, RRTNode &t);
+ void sample();
+ std::default_random_engine gen_;
+ std::normal_distribution<double> ndx_;
+ std::normal_distribution<double> ndy_;
+ std::normal_distribution<double> ndh_;
+ std::uniform_real_distribution<double> udx_;
+ std::uniform_real_distribution<double> udy_;
+ std::uniform_real_distribution<double> udh_;
+ std::uniform_int_distribution<unsigned int> udi1_;
+ std::uniform_int_distribution<unsigned int> udi2_;
+ virtual RRTNode *nn(RRTNode &t);
+ virtual std::vector<RRTNode *> nv(RRTNode &t);
+ void steer(RRTNode &f, RRTNode &t);
+ void tmp_steer(RRTNode &f, RRTNode &t);
+ virtual void steer1(RRTNode &f, RRTNode &t);
+ virtual void steer2(RRTNode &f, RRTNode &t);
+ /*! \brief Join steered nodes to RRT data structure
+
+ \param f RRT node to join steered nodes to.
+ */
+ void join_steered(RRTNode *f);
+ void join_tmp_steered(RRTNode *f);
+ virtual bool goal_found(RRTNode &f);
+ // RRT* procedures
+ virtual bool connect();
+ void rewire();
+ public:
+ /// ---
+ std::vector<double> log_opt_time_;
+ std::vector<double> log_path_cost_;
+ struct { double x=0; double y=0; double b=0; double e=0; } entry;
+ bool entry_set = false;
+ struct { double x=0; double y=0; double h=0; } entry1;
+ struct { double x=0; double y=0; double h=0; } entry2;
+ bool entries_set = false;
+ std::vector<RRTNode *> steered1_;
+ std::vector<RRTNode *> steered2_;
+ /// ---
+ /*! \brief Initialize RRT algorithm if needed.
+ */
+ virtual void init();
+ virtual void reset();
+ /*! \brief Deinitialize RRT algorithm if needed.
+ */
+ virtual void deinit();
+ /*! \brief Return path found by RRT*.
+ */
+ virtual std::vector<RRTNode *>& path()
+ {
+ return this->path_;
+ }
+ virtual void compute_path();
+ /*! \brief Return ``true`` if algorithm should stop.
+
+ Update counters (iteration, seconds, ...) and return if
+ the current iteration should be the last one.
+ */
+ bool should_stop();
+ /*! \brief Return ``true`` if the algorithm should finish.
+
+ Finish means that the algorithm will not be resumed.
+ */
+ bool should_finish();
+ /*! \brief Return ``true`` if the algorithm shoud break.
+
+ Break means that the algorithm can be resumed.
+ */
+ bool should_break();
+ /*! \brief Return ``true`` if algorithm should continue.
+
+ `pcnt_` is set to `scnt_`, so the difference is 0 and it can
+ start from scratch. After the `should_continue` is called,
+ there must be `while (rrts.next()) {}` loop.
+ */
+ bool should_continue();
+ /*! \brief Run next RRT* iteration.
+ */
+ virtual bool next();
+ /*! \brief Set sampling info.
+
+ Based on `sample_dist_type`, set proper distribution
+ parameters. The distribution parameters are relative to `front`
+ node in `nodes` (init).
+
+ For normal sampling:
+ \param x1 Mean x value.
+ \param x2 Standard deviation of x.
+ \param y1 Mean y value.
+ \param y2 Standard deviation of y.
+ \param h1 Mean h value.
+ \param h2 Standard deviation of h.
+
+ For uniform sampling:
+ \param x1 Minimum x value.
+ \param x2 Maximum x value.
+ \param y1 Minimum y value.
+ \param y2 Maximum y value.
+ \param h1 Minimum h value.
+ \param h2 Maximum h value.
+
+ For uniform circle sampling:
+ \param x1 Ignored.
+ \param x2 Ignored.
+ \param y1 Ignored.
+ \param y2 Ignored.
+ \param h1 Ignored.
+ \param h2 Ignored.
+ */
+ void set_sample(
+ double x1, double x2,
+ double y1, double y2,
+ double h1, double h2
+ );
+ /*! \brief Generate JSON output.
+ */
+ Json::Value json();
+ /*! \brief Load JSON input.
+ */
+ void json(Json::Value jvi);
+
+ // RRT procedures
+ virtual double cost_build(RRTNode &f, RRTNode &t);
+ virtual double cost_search(RRTNode &f, RRTNode &t);
+
+ // getters, setters
+ unsigned int icnt() const { return this->icnt_; }
+ void icnt(unsigned int i) { this->icnt_ = i; }
+ double scnt() const { return this->scnt_; }
+ bool gf() const { return this->gf_; }
+ void gf(bool f) { this->gf_ = f; }
+ int sample_dist_type() const { return this->sample_dist_type_;}
+ void sample_dist_type(int t) { this->sample_dist_type_ = t; }
+ std::vector<RRTNode> &goals() { return this->goals_; }
+ std::vector<RRTNode> &nodes() { return this->nodes_; }
+ std::vector<Obstacle> &obstacles() { return this->obstacles_; }
+ std::vector<RRTNode> &samples() { return this->samples_; }
+ std::vector<RRTNode> &steered() { return this->steered_; }
+
+ RRTS();
};
/*! \brief Compute cumulative cost of RRT node.
projects / hubacji1 / rrts.git / blobdiff