2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
9 void RRTExt12::steer1(RRTNode &f, RRTNode &t)
11 // assume that `t` is circle uniformly sampled
12 auto bc = BicycleCar();
17 auto max_steer = bc.st();
18 // map t.h() in -PI...+PI to -max_steer...+max_steer
19 bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
20 bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5);
21 this->steered().clear();
26 this->steered().size() == 0
27 && std::get<0>(this->collide_two_nodes(f, n))
32 this->steered().size() > 0
33 && std::get<0>(this->collide_two_nodes(
34 this->steered().back(),
38 || this->steered().size() > 25
42 this->steered().push_back(RRTNode(bc));
47 this->samples().back().x(bc.x());
48 this->samples().back().y(bc.y());
49 this->samples().back().h(bc.h());
51 bool RRTExt12::connect()
53 RRTNode *t = &this->steered().front();
54 RRTNode *f = this->use_nn;
55 double cost = this->cost_search(*f, *t);
56 // steer from f->t and then continue with the steered.
57 this->tmp_steer(*f, *t);
58 if (this->tmp_steered_.size() > 0) {
59 auto col = this->collide_tmp_steered_from(*f);
62 this->join_tmp_steered(f);
63 f = &this->nodes().back();
66 pow(f->x() - t->x(), 2)
67 + pow(f->y() - t->y(), 2)
71 this->store_node(this->steered().front());
72 t = &this->nodes().back();
74 t->c(this->cost_build(*f, *t));
75 t->set_t(RRTNodeType::connected);
81 this->tstart_ = std::chrono::high_resolution_clock::now();
83 if (this->icnt_ > this->log_path_iter_)
84 this->log_path_cost();
85 if (this->should_stop())
87 if (this->samples().size() == 0) {
88 this->samples().push_back(RRTNode());
89 this->samples().back().x(this->goals().front().x());
90 this->samples().back().y(this->goals().front().y());
91 this->samples().back().h(this->goals().front().h());
92 this->use_nn = &this->nodes().front();
98 this->samples().back()
100 if (this->steered().size() == 0)
102 auto col = this->collide_steered_from(*this->use_nn);
103 if (std::get<0>(col)) {
104 auto rcnt = this->steered().size() - std::get<1>(col);
106 this->steered().pop_back();
109 if (!this->connect())
112 unsigned scnt = this->steered().size();
113 this->join_steered(&this->nodes().back());
114 RRTNode *just_added = &this->nodes().back();
116 // store all the steered1 nodes
117 this->steered1_.push_back(just_added);
119 auto &g = this->goals().front();
120 this->steer2(*just_added, g);
121 auto col = this->collide_steered_from(*just_added);
122 if (std::get<0>(col)) {
123 auto rcnt = this->steered().size() - std::get<1>(col);
125 this->steered().pop_back();
128 this->join_steered(just_added);
129 // store all the steered2 nodes
130 RRTNode* jap = &this->nodes().back();
131 while (jap != just_added) {
132 this->steered2_.push_back(jap);
135 this->gf(this->goal_found(this->nodes().back()));
136 just_added = just_added->p();