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 \param x Horizontal coordinate of rear axle center.
35 \param y Vertical coordinate of rear axle center.
36 \param h Heading of the car in the interval [-pi,+pi] radians.
37 \param sp Speed of the car.
38 \param st Steering of the car.
43 RRTNode *p_ = nullptr;
55 double c() const { return this->c_; }
56 void c(double c) { this->c_ = c; }
58 RRTNode *p() const { return this->p_; }
59 void p(RRTNode *p) { this->p_ = p; }
61 bool t(unsigned int flag) { return this->t_ & flag; }
62 void set_t(unsigned int flag) { this->t_ |= flag; }
63 void clear_t(unsigned int flag) { this->t_ &= ~flag; }
67 double x() const { return this->x_; }
68 void x(double x) { this->x_ = x; }
70 double y() const { return this->y_; }
71 void y(double y) { this->y_ = y; }
73 double h() const { return this->h_; }
83 double sp() const { return this->sp_; }
84 void sp(double sp) { this->sp_ = sp; }
86 double st() const { return this->st_; }
87 void st(double st) { this->st_ = st; }
90 RRTNode(const BicycleCar &bc);
91 bool operator==(const RRTNode& n);
94 /*! \brief Polygon obstacle basic class.
96 \param poly Border polygon of the obstacle.
100 std::vector<std::tuple<double, double>> poly_;
103 std::vector<std::tuple<double, double>> &poly()
111 /*! \brief RRT* algorithm basic class.
113 \param icnt RRT algorithm iterations counter.
114 \param goals The vector of goal nodes.
115 \param nodes The vector of all nodes in RRT data structure.
116 \param samples The vector of all samples of RRT algorithm.
117 \param sample_dist_type Random distribution type for sampling function (0 -
122 unsigned int icnt_ = 0;
123 std::chrono::high_resolution_clock::time_point tstart_;
127 int sample_dist_type_ = 0;
129 std::vector<RRTNode> goals_;
130 std::vector<RRTNode> nodes_;
131 std::vector<Obstacle> obstacles_;
132 std::vector<RRTNode> samples_;
133 std::vector<RRTNode> steered_;
135 /*! \brief Update and return elapsed time.
138 /*! \brief Set normal distribution for sampling.
140 void set_sample_normal(
141 double x1, double x2,
142 double y1, double y2,
145 /*! \brief Set uniform distribution for sampling.
147 void set_sample_uniform(
148 double x1, double x2,
149 double y1, double y2,
153 /*! \brief Store RRT node to tree data structure.
155 virtual void store_node(RRTNode n);
158 std::tuple<bool, unsigned int, unsigned int>
159 collide(std::vector<std::tuple<double, double>> &poly);
160 virtual std::tuple<bool, unsigned int, unsigned int>
161 collide_steered_from(RRTNode &f);
162 virtual std::tuple<bool, unsigned int, unsigned int>
163 collide_two_nodes(RRTNode &f, RRTNode &t);
165 std::default_random_engine gen_;
166 std::normal_distribution<double> ndx_;
167 std::normal_distribution<double> ndy_;
168 std::normal_distribution<double> ndh_;
169 std::uniform_real_distribution<double> udx_;
170 std::uniform_real_distribution<double> udy_;
171 std::uniform_real_distribution<double> udh_;
172 virtual RRTNode *nn(RRTNode &t);
173 virtual std::vector<RRTNode *> nv(RRTNode &t);
174 void steer(RRTNode &f, RRTNode &t);
175 /*! \brief Join steered nodes to RRT data structure
177 \param f RRT node to join steered nodes to.
179 void join_steered(RRTNode *f);
180 virtual bool goal_found(RRTNode &f);
185 /*! \brief Initialize RRT algorithm if needed.
188 /*! \brief Deinitialize RRT algorithm if needed.
190 virtual void deinit();
191 /*! \brief Return path found by RRT*.
193 virtual std::vector<RRTNode *> path();
194 /*! \brief Return ``true`` if algorithm should stop.
196 Update counters (iteration, seconds, ...) and return if
197 the current iteration should be the last one.
200 /*! \brief Return ``true`` if the algorithm should finish.
202 Finish means that the algorithm will not be resumed.
204 bool should_finish();
205 /*! \brief Return ``true`` if the algorithm shoud break.
207 Break means that the algorithm can be resumed.
210 /*! \brief Return ``true`` if algorithm should continue.
212 `pcnt_` is set to `scnt_`, so the difference is 0 and it can
213 start from scratch. After the `should_continue` is called,
214 there must be `while (rrts.next()) {}` loop.
216 bool should_continue();
217 /*! \brief Run next RRT* iteration.
220 /*! \brief Set sampling info.
222 Based on `sample_dist_type`, set proper distribution
223 parameters. The distribution parameters are relative to `front`
224 node in `nodes` (init).
227 \param x1 Mean x value.
228 \param x2 Standard deviation of x.
229 \param y1 Mean y value.
230 \param y2 Standard deviation of y.
231 \param h1 Mean h value.
232 \param h2 Standard deviation of h.
234 For uniform sampling:
235 \param x1 Minimum x value.
236 \param x2 Maximum x value.
237 \param y1 Minimum y value.
238 \param y2 Maximum y value.
239 \param h1 Minimum h value.
240 \param h2 Maximum h value.
243 double x1, double x2,
244 double y1, double y2,
247 /*! \brief Generate JSON output.
250 /*! \brief Load JSON input.
252 void json(Json::Value jvi);
255 virtual double cost_build(RRTNode &f, RRTNode &t);
256 virtual double cost_search(RRTNode &f, RRTNode &t);
259 unsigned int icnt() const { return this->icnt_; }
260 double scnt() const { return this->scnt_; }
261 bool gf() const { return this->gf_; }
262 void gf(bool f) { this->gf_ = f; }
263 int sample_dist_type() const { return this->sample_dist_type_;}
264 void sample_dist_type(int t) { this->sample_dist_type_ = t; }
265 std::vector<RRTNode> &goals() { return this->goals_; }
266 std::vector<RRTNode> &nodes() { return this->nodes_; }
267 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
268 std::vector<RRTNode> &samples() { return this->samples_; }
269 std::vector<RRTNode> &steered() { return this->steered_; }
274 /*! \brief Compute cumulative cost of RRT node.
276 \param t RRT node to compute cumulative cost to.
278 double cc(RRTNode &t);