4 #include "reeds_shepp.h"
6 #define ETA 1.0 // for steer, nv
8 __typeof__ (cV) _cV = (cV); \
9 pow(log(_cV) / _cV, 1.0 / 3.0); \
12 template <typename T> int sgn(T val) {
13 return (T(0) < val) - (val < T(0));
20 RRTNode::RRTNode(const BicycleCar &bc) : BicycleCar(bc)
28 double RRTS::elapsed()
30 std::chrono::duration<double> dt;
31 dt = std::chrono::duration_cast<std::chrono::duration<double>>(
32 std::chrono::high_resolution_clock::now()
35 this->scnt_ = dt.count();
39 bool RRTS::should_stop()
41 // the following counters must be updated, do not comment
44 // decide the stop conditions (maybe comment some lines)
45 if (this->icnt_ > 999) return true;
46 if (this->scnt_ > 10) return true;
47 if (this->gf()) return true;
48 // but continue by default
52 void RRTS::store_node(RRTNode n)
54 this->nodes().push_back(n);
58 std::tuple<bool, unsigned int, unsigned int>
59 RRTS::collide(std::vector<std::tuple<double, double>> &poly)
61 for (auto &o: this->obstacles())
62 if (std::get<0>(::collide(poly, o.poly())))
63 return ::collide(poly, o.poly());
64 return std::make_tuple(false, 0, 0);
67 std::tuple<bool, unsigned int, unsigned int>
68 RRTS::collide_steered_from(RRTNode &f)
70 std::vector<std::tuple<double, double>> s;
71 s.push_back(std::make_tuple(f.x(), f.y()));
72 for (auto &n: this->steered()) {
73 s.push_back(std::make_tuple(n.lfx(), n.lfy()));
74 s.push_back(std::make_tuple(n.lrx(), n.lry()));
75 s.push_back(std::make_tuple(n.rrx(), n.rry()));
76 s.push_back(std::make_tuple(n.rfx(), n.rfy()));
78 auto col = this->collide(s);
79 auto strip_from = this->steered().size() - std::get<1>(col) / 4;
80 if (std::get<0>(col) && strip_from > 0) {
81 while (strip_from-- > 0) {
82 this->steered().pop_back();
84 return this->collide_steered_from(f);
89 std::tuple<bool, unsigned int, unsigned int>
90 RRTS::collide_two_nodes(RRTNode &f, RRTNode &t)
92 std::vector<std::tuple<double, double>> p;
93 p.push_back(std::make_tuple(f.lfx(), f.lfy()));
94 p.push_back(std::make_tuple(f.lrx(), f.lry()));
95 p.push_back(std::make_tuple(f.rrx(), f.rry()));
96 p.push_back(std::make_tuple(f.rfx(), f.rfy()));
97 p.push_back(std::make_tuple(t.lfx(), t.lfy()));
98 p.push_back(std::make_tuple(t.lrx(), t.lry()));
99 p.push_back(std::make_tuple(t.rrx(), t.rry()));
100 p.push_back(std::make_tuple(t.rfx(), t.rfy()));
101 return this->collide(p);
104 double RRTS::cost_build(RRTNode &f, RRTNode &t)
107 cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
111 double RRTS::cost_search(RRTNode &f, RRTNode &t)
114 cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
120 double x = this->ndx_(this->gen_);
121 double y = this->ndy_(this->gen_);
122 double h = this->ndh_(this->gen_);
123 this->samples().push_back(RRTNode());
124 this->samples().back().x(x);
125 this->samples().back().y(y);
126 this->samples().back().h(h);
129 RRTNode *RRTS::nn(RRTNode &t)
131 RRTNode *nn = &this->nodes().front();
132 double cost = this->cost_search(*nn, t);
133 for (auto &f: this->nodes()) {
134 if (this->cost_search(f, t) < cost) {
136 cost = this->cost_search(f, t);
142 std::vector<RRTNode *> RRTS::nv(RRTNode &t)
144 std::vector<RRTNode *> nv;
145 double cost = std::min(GAMMA(this->nodes().size()), ETA);
146 for (auto &f: this->nodes())
147 if (this->cost_search(f, t) < cost)
152 int cb_rs_steer(double q[4], void *user_data)
154 std::vector<RRTNode> *nodes = (std::vector<RRTNode> *) user_data;
155 RRTNode *ln = nullptr;
156 if (nodes->size() > 0)
158 nodes->push_back(RRTNode());
159 nodes->back().x(q[0]);
160 nodes->back().y(q[1]);
161 nodes->back().h(q[2]);
162 nodes->back().sp(q[3]);
163 if (nodes->back().sp() == 0)
164 nodes->back().set_t(RRTNodeType::cusp);
165 else if (ln != nullptr && sgn(ln->sp()) != sgn(nodes->back().sp()))
166 ln->set_t(RRTNodeType::cusp);
170 void RRTS::steer(RRTNode &f, RRTNode &t)
172 this->steered().clear();
173 double q0[] = {f.x(), f.y(), f.h()};
174 double q1[] = {t.x(), t.y(), t.h()};
175 ReedsSheppStateSpace rsss(f.mtr());
176 rsss.sample(q0, q1, 0.5, cb_rs_steer, &this->steered());
179 void RRTS::join_steered(RRTNode *f)
181 while (this->steered().size() > 0) {
182 this->store_node(this->steered().front());
183 RRTNode *t = &this->nodes().back();
185 t->c(this->cost_build(*f, *t));
186 this->steered().erase(this->steered().begin());
191 bool RRTS::goal_found(RRTNode &f)
194 for (auto &g: this->goals()) {
195 double cost = this->cost_build(f, g);
197 pow(f.x() - g.x(), 2)
198 + pow(f.y() - g.y(), 2)
200 double adist = std::abs(f.h() - g.h());
201 if (edist < 0.05 && adist < M_PI / 32) {
203 if (g.p() == nullptr || cc(f) + cost < cc(g)) {
215 RRTNode *t = &this->steered().front();
216 RRTNode *f = this->nn(this->samples().back());
217 double cost = this->cost_search(*f, *t);
218 for (auto n: this->nv(*t)) {
220 !std::get<0>(this->collide_two_nodes(*n, *t))
221 && this->cost_search(*n, *t) < cost
224 cost = this->cost_search(*n, *t);
227 this->store_node(this->steered().front());
228 t = &this->nodes().back();
230 t->c(this->cost_build(*f, *t));
231 t->set_t(RRTNodeType::connected);
237 RRTNode *f = &this->nodes().back();
238 for (auto n: this->nv(*f)) {
240 !std::get<0>(this->collide_two_nodes(*f, *n))
241 && cc(*f) + this->cost_search(*f, *n) < cc(*n)
244 n->c(this->cost_build(*f, *n));
258 std::vector<RRTNode *> RRTS::path()
260 std::vector<RRTNode *> path;
261 if (this->goals().size() == 0)
263 RRTNode *goal = &this->goals().front();
264 for (auto &n: this->goals()) {
267 && (n.c() < goal->c() || goal->p() == nullptr)
272 if (goal->p() == nullptr)
274 while (goal != nullptr) {
275 path.push_back(goal);
278 std::reverse(path.begin(), path.end());
284 if (this->icnt_ == 0)
285 this->tstart_ = std::chrono::high_resolution_clock::now();
287 if (this->should_stop())
291 *this->nn(this->samples().back()),
292 this->samples().back()
294 if (std::get<0>(this->collide_steered_from(
295 *this->nn(this->samples().back())
298 if (!this->connect())
301 unsigned scnt = this->steered().size();
302 this->steered().erase(this->steered().begin());
303 this->join_steered(&this->nodes().back());
304 RRTNode *just_added = &this->nodes().back();
307 for (auto &g: this->goals()) {
308 this->steer(*just_added, g);
309 if (std::get<0>(this->collide_steered_from(
313 this->join_steered(just_added);
315 this->gf(this->goal_found(this->nodes().back()));
316 just_added = just_added->p();
321 void RRTS::set_sample(
322 double mx, double dx,
323 double my, double dy,
327 this->ndx_ = std::normal_distribution<double>(mx, dx);
328 this->ndy_ = std::normal_distribution<double>(my, dy);
329 this->ndh_ = std::normal_distribution<double>(mh, dh);
333 : gen_(std::random_device{}())
335 this->goals().reserve(100);
336 this->nodes().reserve(4000000);
337 this->samples().reserve(1000);
338 this->steered().reserve(20000);
339 this->store_node(RRTNode()); // root
342 double cc(RRTNode &t)
346 while (n != nullptr) {