#ifndef RRTS_H
#define RRTS_H
+#include <chrono>
+#include <functional>
+#include <json/json.h>
+#include <random>
+#include <vector>
+#include "bcar.h"
+
+#define ETA 1.0 // for steer, nv
+#define GAMMA(cV) ({ \
+ __typeof__ (cV) _cV = (cV); \
+ pow(log(_cV) / _cV, 1.0 / 3.0); \
+})
+
+/*! \brief Possible type of RRT node.
+
+\param cusp The node that is cusp (change in direction).
+\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;
+};
+
+/*! \brief RRT node basic class.
+
+\param c Cumulative cost from RRT data structure root.
+\param p Pointer to parent RRT node.
+\param t Type of the RRT node (RRTNodeType).
+// -- from BicycleCar
+\param x Horizontal coordinate of rear axle center.
+\param y Vertical coordinate of rear axle center.
+\param h Heading of the car in the interval [-pi,+pi] radians.
+\param sp Speed of the car.
+\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;
+ }
+};
+
+/*! \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();
+};
+
/*! \brief RRT* algorithm basic class.
+
+\param icnt RRT algorithm iterations counter.
+\param goals The vector of goal nodes.
+\param nodes The vector of all nodes in RRT data structure.
+\param samples The vector of all samples of RRT algorithm.
+\param sample_dist_type Random distribution type for sampling function (0 -
+normal, 1 - uniform, 2 - uniform circle)
*/
class RRTS {
- private:
+ 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.
+
+\param t RRT node to compute cumulative cost to.
+*/
+double cc(RRTNode &t);
+
#endif /* RRTS_H */