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.
42 RRTNode *p_ = nullptr;
56 double c() const { return this->c_; }
60 this->cc = this->p_->cc + this->c_;
63 RRTNode *p() const { return this->p_; }
64 void p(RRTNode *p) { this->p_ = p; }
66 bool t(unsigned int flag) { return this->t_ & flag; }
67 void set_t(unsigned int flag) { this->t_ |= flag; }
68 void clear_t(unsigned int flag) { this->t_ &= ~flag; }
72 double x() const { return this->x_; }
73 void x(double x) { this->x_ = x; }
75 double y() const { return this->y_; }
76 void y(double y) { this->y_ = y; }
78 double h() const { return this->h_; }
88 double sp() const { return this->sp_; }
89 void sp(double sp) { this->sp_ = sp; }
91 double st() const { return this->st_; }
92 void st(double st) { this->st_ = st; }
95 RRTNode(const BicycleCar &bc);
96 bool operator==(const RRTNode& n);
97 friend std::ostream &operator<<(
102 out << "[" << bc.x();
103 out << "," << bc.y();
104 out << "," << bc.h();
110 /*! \brief Polygon obstacle basic class.
112 \param poly Border polygon of the obstacle.
116 std::vector<std::tuple<double, double>> poly_;
119 std::vector<std::tuple<double, double>> &poly()
127 /*! \brief RRT* algorithm basic class.
129 \param icnt RRT algorithm iterations counter.
130 \param goals The vector of goal nodes.
131 \param nodes The vector of all nodes in RRT data structure.
132 \param samples The vector of all samples of RRT algorithm.
133 \param sample_dist_type Random distribution type for sampling function (0 -
134 normal, 1 - uniform, 2 - uniform circle)
138 unsigned int icnt_ = 0;
139 std::chrono::high_resolution_clock::time_point tstart_;
143 int sample_dist_type_ = 0;
145 std::vector<RRTNode> goals_;
146 std::vector<RRTNode> nodes_;
147 std::vector<Obstacle> obstacles_;
148 std::vector<RRTNode> samples_;
149 std::vector<RRTNode> steered_;
150 std::vector<RRTNode *> path_;
151 double log_path_time_ = 0.1;
152 unsigned int log_path_iter_ = 20;
154 /*! \brief Update and return elapsed time.
157 /*! \brief Log current path cost.
159 void log_path_cost();
160 /*! \brief Set normal distribution for sampling.
162 void set_sample_normal(
163 double x1, double x2,
164 double y1, double y2,
167 /*! \brief Set uniform distribution for sampling.
169 void set_sample_uniform(
170 double x1, double x2,
171 double y1, double y2,
174 /*! \brief Set uniform circle distribution for sampling.
176 void set_sample_uniform_circle();
177 RRTNode* use_nn; // Used for RRTExt12.
178 std::vector<RRTNode> tmp_steered_;
179 bool finishit = false;
180 double path_cost_before_opt_ = 9999;
183 /*! \brief Store RRT node to tree data structure.
185 virtual void store_node(RRTNode n);
188 std::tuple<bool, unsigned int, unsigned int>
189 collide(std::vector<std::tuple<double, double>> &poly);
190 virtual std::tuple<bool, unsigned int, unsigned int>
191 collide_steered_from(RRTNode &f);
192 virtual std::tuple<bool, unsigned int, unsigned int>
193 collide_tmp_steered_from(RRTNode &f);
194 virtual std::tuple<bool, unsigned int, unsigned int>
195 collide_two_nodes(RRTNode &f, RRTNode &t);
197 std::default_random_engine gen_;
198 std::normal_distribution<double> ndx_;
199 std::normal_distribution<double> ndy_;
200 std::normal_distribution<double> ndh_;
201 std::uniform_real_distribution<double> udx_;
202 std::uniform_real_distribution<double> udy_;
203 std::uniform_real_distribution<double> udh_;
204 std::uniform_int_distribution<unsigned int> udi1_;
205 std::uniform_int_distribution<unsigned int> udi2_;
206 virtual RRTNode *nn(RRTNode &t);
207 virtual std::vector<RRTNode *> nv(RRTNode &t);
208 void steer(RRTNode &f, RRTNode &t);
209 void tmp_steer(RRTNode &f, RRTNode &t);
210 virtual void steer1(RRTNode &f, RRTNode &t);
211 virtual void steer2(RRTNode &f, RRTNode &t);
212 /*! \brief Join steered nodes to RRT data structure
214 \param f RRT node to join steered nodes to.
216 void join_steered(RRTNode *f);
217 void join_tmp_steered(RRTNode *f);
218 virtual bool goal_found(RRTNode &f);
220 virtual bool connect();
224 std::vector<double> log_opt_time_;
225 std::vector<double> log_path_cost_;
226 struct { double x=0; double y=0; double b=0; double e=0; } entry;
227 bool entry_set = false;
228 struct { double x=0; double y=0; double h=0; } entry1;
229 struct { double x=0; double y=0; double h=0; } entry2;
230 bool entries_set = false;
231 std::vector<RRTNode *> steered1_;
232 std::vector<RRTNode *> steered2_;
234 /*! \brief Initialize RRT algorithm if needed.
237 /*! \brief Deinitialize RRT algorithm if needed.
239 virtual void deinit();
240 /*! \brief Return path found by RRT*.
242 virtual std::vector<RRTNode *>& path()
246 virtual void compute_path();
247 /*! \brief Return ``true`` if algorithm should stop.
249 Update counters (iteration, seconds, ...) and return if
250 the current iteration should be the last one.
253 /*! \brief Return ``true`` if the algorithm should finish.
255 Finish means that the algorithm will not be resumed.
257 bool should_finish();
258 /*! \brief Return ``true`` if the algorithm shoud break.
260 Break means that the algorithm can be resumed.
263 /*! \brief Return ``true`` if algorithm should continue.
265 `pcnt_` is set to `scnt_`, so the difference is 0 and it can
266 start from scratch. After the `should_continue` is called,
267 there must be `while (rrts.next()) {}` loop.
269 bool should_continue();
270 /*! \brief Run next RRT* iteration.
273 /*! \brief Set sampling info.
275 Based on `sample_dist_type`, set proper distribution
276 parameters. The distribution parameters are relative to `front`
277 node in `nodes` (init).
280 \param x1 Mean x value.
281 \param x2 Standard deviation of x.
282 \param y1 Mean y value.
283 \param y2 Standard deviation of y.
284 \param h1 Mean h value.
285 \param h2 Standard deviation of h.
287 For uniform sampling:
288 \param x1 Minimum x value.
289 \param x2 Maximum x value.
290 \param y1 Minimum y value.
291 \param y2 Maximum y value.
292 \param h1 Minimum h value.
293 \param h2 Maximum h value.
295 For uniform circle sampling:
304 double x1, double x2,
305 double y1, double y2,
308 /*! \brief Generate JSON output.
311 /*! \brief Load JSON input.
313 void json(Json::Value jvi);
316 virtual double cost_build(RRTNode &f, RRTNode &t);
317 virtual double cost_search(RRTNode &f, RRTNode &t);
320 unsigned int icnt() const { return this->icnt_; }
321 void icnt(unsigned int i) { this->icnt_ = i; }
322 double scnt() const { return this->scnt_; }
323 bool gf() const { return this->gf_; }
324 void gf(bool f) { this->gf_ = f; }
325 int sample_dist_type() const { return this->sample_dist_type_;}
326 void sample_dist_type(int t) { this->sample_dist_type_ = t; }
327 std::vector<RRTNode> &goals() { return this->goals_; }
328 std::vector<RRTNode> &nodes() { return this->nodes_; }
329 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
330 std::vector<RRTNode> &samples() { return this->samples_; }
331 std::vector<RRTNode> &steered() { return this->steered_; }
336 /*! \brief Compute cumulative cost of RRT node.
338 \param t RRT node to compute cumulative cost to.
340 double cc(RRTNode &t);