6 #include <jsoncpp/json/json.h>
11 #define ETA 1.0 // for steer, nv
12 #define GAMMA(cV) ({ \
13 __typeof__ (cV) _cV = (cV); \
14 pow(log(_cV) / _cV, 1.0 / 3.0); \
17 /*! \brief Possible type of RRT node.
19 \param cusp The node that is cusp (change in direction).
20 \param connected The node that branches generated steered path.
24 static const unsigned int cusp = 1 << 0;
25 static const unsigned int connected = 1 << 1;
28 /*! \brief RRT node basic class.
30 \param c Cumulative cost from RRT data structure root.
31 \param p Pointer to parent RRT node.
32 \param ch The vector of pointers to children RRT nodes.
34 class RRTNode : public BicycleCar {
37 RRTNode *p_ = nullptr;
41 double c() const { return this->c_; }
42 void c(double c) { this->c_ = c; }
44 RRTNode *p() const { return this->p_; }
45 void p(RRTNode *p) { this->p_ = p; }
47 bool t(unsigned int flag) { return this->t_ & flag; }
48 void set_t(unsigned int flag) { this->t_ |= flag; }
49 void clear_t(unsigned int flag) { this->t_ &= ~flag; }
52 RRTNode(const BicycleCar &bc);
55 /*! \brief Polygon obstacle basic class.
57 \param poly Border polygon of the obstacle.
61 std::vector<std::tuple<double, double>> poly_;
64 std::vector<std::tuple<double, double>> &poly()
72 /*! \brief RRT* algorithm basic class.
74 \param icnt RRT algorithm iterations counter.
75 \param goals The vector of goal nodes.
76 \param nodes The vector of all nodes in RRT data structure.
77 \param samples The vector of all samples of RRT algorithm.
81 unsigned int icnt_ = 0;
82 std::chrono::high_resolution_clock::time_point tstart_;
87 std::vector<RRTNode> goals_;
88 std::vector<RRTNode> nodes_;
89 std::vector<Obstacle> obstacles_;
90 std::vector<RRTNode> samples_;
91 std::vector<RRTNode> steered_;
93 /*! \brief Update and return elapsed time.
97 /*! \brief Store RRT node to tree data structure.
99 virtual void store_node(RRTNode n);
102 std::tuple<bool, unsigned int, unsigned int>
103 collide(std::vector<std::tuple<double, double>> &poly);
104 virtual std::tuple<bool, unsigned int, unsigned int>
105 collide_steered_from(RRTNode &f);
106 virtual std::tuple<bool, unsigned int, unsigned int>
107 collide_two_nodes(RRTNode &f, RRTNode &t);
109 std::default_random_engine gen_;
110 std::normal_distribution<double> ndx_;
111 std::normal_distribution<double> ndy_;
112 std::normal_distribution<double> ndh_;
113 virtual RRTNode *nn(RRTNode &t);
114 virtual std::vector<RRTNode *> nv(RRTNode &t);
115 void steer(RRTNode &f, RRTNode &t);
116 /*! \brief Join steered nodes to RRT data structure
118 \param f RRT node to join steered nodes to.
120 void join_steered(RRTNode *f);
121 bool goal_found(RRTNode &f);
126 /*! \brief Initialize RRT algorithm if needed.
129 /*! \brief Deinitialize RRT algorithm if needed.
131 virtual void deinit();
132 /*! \brief Return path found by RRT*.
134 virtual std::vector<RRTNode *> path();
135 /*! \brief Return ``true`` if algorithm should stop.
137 Update counters (iteration, seconds, ...) and return if
138 the current iteration should be the last one.
141 /*! \brief Return ``true`` if algorithm should continue.
143 `pcnt_` is set to `scnt_`, so the difference is 0 and it can
144 start from scratch. After the `should_continue` is called,
145 there must be `while (rrts.next()) {}` loop.
147 bool should_continue();
148 /*! \brief Run next RRT* iteration.
151 /*! \brief Set sampling info.
153 There is normal distribution sampling for `x`, `y`, and
154 `h` parameters of RRT node.
156 \param mx Mean x value.
157 \param dx Standard deviation of x.
158 \param my Mean y value.
159 \param dy Standard deviation of y.
160 \param mh Mean h value.
161 \param dh Standard deviation of h.
164 double mx, double dx,
165 double my, double dy,
168 /*! \brief Generate JSON output.
173 virtual double cost_build(RRTNode &f, RRTNode &t);
174 virtual double cost_search(RRTNode &f, RRTNode &t);
177 unsigned int icnt() const { return this->icnt_; }
178 double scnt() const { return this->scnt_; }
179 bool gf() const { return this->gf_; }
180 void gf(bool f) { this->gf_ = f; }
181 std::vector<RRTNode> &goals() { return this->goals_; }
182 std::vector<RRTNode> &nodes() { return this->nodes_; }
183 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
184 std::vector<RRTNode> &samples() { return this->samples_; }
185 std::vector<RRTNode> &steered() { return this->steered_; }
190 /*! \brief Compute cumulative cost of RRT node.
192 \param t RRT node to compute cumulative cost to.
194 double cc(RRTNode &t);