3 void RRTExt12::steer1(RRTNode &f, RRTNode &t)
5 // assume that `t` is circle uniformly sampled
6 auto bc = BicycleCar();
11 auto max_steer = bc.st();
12 // map t.h() in -PI...+PI to -max_steer...+max_steer
13 bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
14 bc.sp((this->udy_(this->gen_) > 0.5) ? 0.2 : -0.2);
15 this->steered().clear();
20 this->steered().size() == 0
21 && std::get<0>(this->collide_two_nodes(f, n))
26 this->steered().size() > 0
27 && std::get<0>(this->collide_two_nodes(
28 this->steered().back(),
32 || this->steered().size() > 25
36 this->steered().push_back(RRTNode(bc));
41 this->samples().back().x(bc.x());
42 this->samples().back().y(bc.y());
43 this->samples().back().h(bc.h());
45 bool RRTExt12::connect()
47 RRTNode *t = &this->steered().front();
48 RRTNode *f = this->use_nn;
49 double cost = this->cost_search(*f, *t);
50 // steer from f->t and then continue with the steered.
51 this->tmp_steer(*f, *t);
52 if (this->tmp_steered_.size() > 0) {
53 auto col = this->collide_tmp_steered_from(*f);
56 this->join_tmp_steered(f);
57 f = &this->nodes().back();
60 pow(f->x() - t->x(), 2)
61 + pow(f->y() - t->y(), 2)
65 this->store_node(this->steered().front());
66 t = &this->nodes().back();
68 t->c(this->cost_build(*f, *t));
69 t->set_t(RRTNodeType::connected);
75 this->tstart_ = std::chrono::high_resolution_clock::now();
77 if (this->scnt_ > this->log_path_time_)
78 this->log_path_cost();
79 if (this->should_stop())
81 if (this->samples().size() == 0) {
82 this->samples().push_back(RRTNode());
83 this->samples().back().x(this->goals().front().x());
84 this->samples().back().y(this->goals().front().y());
85 this->samples().back().h(this->goals().front().h());
86 this->use_nn = &this->nodes().front();
92 this->samples().back()
94 if (this->steered().size() == 0)
96 auto col = this->collide_steered_from(*this->use_nn);
97 if (std::get<0>(col)) {
98 auto rcnt = this->steered().size() - std::get<1>(col);
100 this->steered().pop_back();
103 if (!this->connect())
106 unsigned scnt = this->steered().size();
107 this->join_steered(&this->nodes().back());
108 RRTNode *just_added = &this->nodes().back();
110 // store all the steered1 nodes
111 this->steered1_.push_back(just_added);
113 auto &g = this->goals().front();
114 this->steer2(*just_added, g);
115 auto col = this->collide_steered_from(*just_added);
116 if (std::get<0>(col)) {
117 auto rcnt = this->steered().size() - std::get<1>(col);
119 this->steered().pop_back();
122 this->join_steered(just_added);
123 // store all the steered2 nodes
124 RRTNode* jap = &this->nodes().back();
125 while (jap != just_added) {
126 this->steered2_.push_back(jap);
129 this->gf(this->goal_found(this->nodes().back()));
130 just_added = just_added->p();