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 t Type of the RRT node (RRTNodeType).
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.
78 \param sample_dist_type Random distribution type for sampling function (0 -
83 unsigned int icnt_ = 0;
84 std::chrono::high_resolution_clock::time_point tstart_;
88 int sample_dist_type_ = 0;
90 std::vector<RRTNode> goals_;
91 std::vector<RRTNode> nodes_;
92 std::vector<Obstacle> obstacles_;
93 std::vector<RRTNode> samples_;
94 std::vector<RRTNode> steered_;
96 /*! \brief Update and return elapsed time.
99 /*! \brief Set normal distribution for sampling.
101 void set_sample_normal(
102 double x1, double x2,
103 double y1, double y2,
106 /*! \brief Set uniform distribution for sampling.
108 void set_sample_uniform(
109 double x1, double x2,
110 double y1, double y2,
114 /*! \brief Store RRT node to tree data structure.
116 virtual void store_node(RRTNode n);
119 std::tuple<bool, unsigned int, unsigned int>
120 collide(std::vector<std::tuple<double, double>> &poly);
121 virtual std::tuple<bool, unsigned int, unsigned int>
122 collide_steered_from(RRTNode &f);
123 virtual std::tuple<bool, unsigned int, unsigned int>
124 collide_two_nodes(RRTNode &f, RRTNode &t);
126 std::default_random_engine gen_;
127 std::normal_distribution<double> ndx_;
128 std::normal_distribution<double> ndy_;
129 std::normal_distribution<double> ndh_;
130 std::uniform_real_distribution<double> udx_;
131 std::uniform_real_distribution<double> udy_;
132 std::uniform_real_distribution<double> udh_;
133 virtual RRTNode *nn(RRTNode &t);
134 virtual std::vector<RRTNode *> nv(RRTNode &t);
135 void steer(RRTNode &f, RRTNode &t);
136 /*! \brief Join steered nodes to RRT data structure
138 \param f RRT node to join steered nodes to.
140 void join_steered(RRTNode *f);
141 virtual bool goal_found(RRTNode &f);
146 /*! \brief Initialize RRT algorithm if needed.
149 /*! \brief Deinitialize RRT algorithm if needed.
151 virtual void deinit();
152 /*! \brief Return path found by RRT*.
154 virtual std::vector<RRTNode *> path();
155 /*! \brief Return ``true`` if algorithm should stop.
157 Update counters (iteration, seconds, ...) and return if
158 the current iteration should be the last one.
161 /*! \brief Return ``true`` if the algorithm should finish.
163 Finish means that the algorithm will not be resumed.
165 bool should_finish();
166 /*! \brief Return ``true`` if the algorithm shoud break.
168 Break means that the algorithm can be resumed.
171 /*! \brief Return ``true`` if algorithm should continue.
173 `pcnt_` is set to `scnt_`, so the difference is 0 and it can
174 start from scratch. After the `should_continue` is called,
175 there must be `while (rrts.next()) {}` loop.
177 bool should_continue();
178 /*! \brief Run next RRT* iteration.
181 /*! \brief Set sampling info.
183 Based on `sample_dist_type`, set proper distribution
184 parameters. The distribution parameters are relative to `front`
185 node in `nodes` (init).
188 \param x1 Mean x value.
189 \param x2 Standard deviation of x.
190 \param y1 Mean y value.
191 \param y2 Standard deviation of y.
192 \param h1 Mean h value.
193 \param h2 Standard deviation of h.
195 For uniform sampling:
196 \param x1 Minimum x value.
197 \param x2 Maximum x value.
198 \param y1 Minimum y value.
199 \param y2 Maximum y value.
200 \param h1 Minimum h value.
201 \param h2 Maximum h value.
204 double x1, double x2,
205 double y1, double y2,
208 /*! \brief Generate JSON output.
211 /*! \brief Load JSON input.
213 void json(Json::Value jvi);
216 virtual double cost_build(RRTNode &f, RRTNode &t);
217 virtual double cost_search(RRTNode &f, RRTNode &t);
220 unsigned int icnt() const { return this->icnt_; }
221 double scnt() const { return this->scnt_; }
222 bool gf() const { return this->gf_; }
223 void gf(bool f) { this->gf_ = f; }
224 int sample_dist_type() const { return this->sample_dist_type_;}
225 void sample_dist_type(int t) { this->sample_dist_type_ = t; }
226 std::vector<RRTNode> &goals() { return this->goals_; }
227 std::vector<RRTNode> &nodes() { return this->nodes_; }
228 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
229 std::vector<RRTNode> &samples() { return this->samples_; }
230 std::vector<RRTNode> &steered() { return this->steered_; }
235 /*! \brief Compute cumulative cost of RRT node.
237 \param t RRT node to compute cumulative cost to.
239 double cc(RRTNode &t);