- 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();