4 std::vector<RRTNode *> RRTExt13::first_path_optimization()
6 if (this->orig_path().size() == 0) {
7 this->orig_path_ = RRTS::path();
8 if (this->orig_path().size() == 0)
9 return this->orig_path();
11 this->orig_path_cost(cc(*this->orig_path().back()));
13 class DijkstraNode : public RRTNode {
15 DijkstraNode *s = nullptr;
27 DijkstraNode(const RRTNode& n) {
35 DijkstraNode *p_ = nullptr;
36 DijkstraNode *p() const { return this->p_; }
37 void p(DijkstraNode *p) { this->p_ = p; }
39 class DijkstraNodeComparator {
42 const DijkstraNode &n1,
43 const DijkstraNode &n2
49 std::vector<DijkstraNode> dn;
50 unsigned int ops = this->orig_path().size();
51 auto op = this->orig_path();
53 unsigned int dncnt = 0;
54 dn.push_back(DijkstraNode(*this->orig_path().front()));
55 dn.back().cc = cc(*this->orig_path().front());
56 dn.back().s = &dn.back();
57 dn.back().n = this->orig_path().front();
58 dn.back().i = dncnt++;
59 for (unsigned int i = 0; i < ops - 1; i++) {
60 auto ibc = BicycleCar();
64 for (unsigned int j = i + 1; j < ops; j++) {
65 auto jbc = BicycleCar();
69 if (!jbc.drivable(ibc)) {
73 //std::cerr<<std::endl;
74 dn.push_back(DijkstraNode(*op[j-1]));
75 dn.back().cc = cc(*op[j-1]);
76 dn.back().s = &dn.back();
77 dn.back().n = op[j-1];
78 dn.back().i = dncnt++;
84 dn.push_back(DijkstraNode(*this->orig_path().back()));
85 dn.back().cc = cc(*this->orig_path().back());
86 dn.back().s = &dn.back();
87 dn.back().n = this->orig_path().back();
88 dn.back().i = dncnt++;
91 std::vector<DijkstraNode>,
92 DijkstraNodeComparator
97 DijkstraNode f = pq.top();
99 for (unsigned int i = f.i + 1; i < dncnt; i++) {
100 double cost = f.cc + this->cost_search(f, dn[i]);
101 this->steer(f, dn[i]);
102 if (this->steered().size() == 0)
103 break; // TODO why not continue?
104 if (std::get<0>(this->collide_steered_from(f)))
106 if (cost < dn[i].cc) {
114 DijkstraNode *ln = nullptr;
119 if (ln == nullptr || ln->p() == nullptr)
120 return this->orig_path();
121 while (ln->p() != nullptr) {
123 RRTNode *f = ln->p()->n;
125 if (std::get<0>(this->collide_steered_from(*f)))
126 return this->orig_path();
127 this->join_steered(f);
128 t->p(&this->nodes().back());
129 t->c(this->cost_build(this->nodes().back(), *t));
135 std::vector<RRTNode *> RRTExt13::second_path_optimization()
137 if (this->first_optimized_path().size() == 0) {
138 return this->orig_path();
140 class DijkstraNode : public RRTNode {
142 DijkstraNode *s = nullptr;
143 RRTNode *n = nullptr;
154 DijkstraNode(const RRTNode& n) {
162 DijkstraNode *p_ = nullptr;
163 DijkstraNode *p() const { return this->p_; }
164 void p(DijkstraNode *p) { this->p_ = p; }
166 class DijkstraNodeComparator {
169 const DijkstraNode &n1,
170 const DijkstraNode &n2
173 return n1.cc > n2.cc;
176 std::vector<DijkstraNode> dn;
177 unsigned int ops = this->orig_path().size();
178 auto op = this->orig_path();
180 unsigned int dncnt = 0;
181 dn.push_back(DijkstraNode(*this->orig_path().front()));
182 dn.back().cc = cc(*this->orig_path().front());
183 dn.back().s = &dn.back();
184 dn.back().n = this->orig_path().front();
185 dn.back().i = dncnt++;
186 for (unsigned int i = ops - 1; i > 1; i--) {
187 auto ibc = BicycleCar();
191 for (unsigned int j = i - 1; j > 0; j--) {
192 auto jbc = BicycleCar();
196 if (!jbc.drivable(ibc)) {
200 //std::cerr<<std::endl;
201 dn.push_back(DijkstraNode(*op[j-1]));
202 dn.back().cc = cc(*op[j-1]);
203 dn.back().s = &dn.back();
204 dn.back().n = op[j-1];
205 dn.back().i = dncnt++;
211 dn.push_back(DijkstraNode(*this->orig_path().back()));
212 dn.back().cc = cc(*this->orig_path().back());
213 dn.back().s = &dn.back();
214 dn.back().n = this->orig_path().back();
215 dn.back().i = dncnt++;
218 std::vector<DijkstraNode>,
219 DijkstraNodeComparator
224 while (!pq.empty()) {
225 DijkstraNode t = pq.top();
227 for (unsigned int i = t.i - 1; i > 0; i--) {
228 double cost = dn[i].cc + this->cost_search(dn[i], t);
229 this->steer(dn[i], t);
230 if (this->steered().size() == 0)
231 break; // TODO why not continue?
232 if (std::get<0>(this->collide_steered_from(dn[i])))
242 DijkstraNode *fn = nullptr;
249 if (fn == nullptr || fn->p() == nullptr)
250 return this->first_optimized_path();
251 DijkstraNode *ln = &dn.back();
252 while (ln->p() != fn) {
254 RRTNode *f = ln->p()->n;
256 if (std::get<0>(this->collide_steered_from(*f)))
257 return this->first_optimized_path();
258 this->join_steered(f);
259 t->p(&this->nodes().back());
260 t->c(this->cost_build(this->nodes().back(), *t));
266 std::vector<RRTNode *> RRTExt13::path()
268 auto tstart = std::chrono::high_resolution_clock::now();
269 this->first_optimized_path_ = this->first_path_optimization();
270 if (this->first_optimized_path_.size() > 0)
271 this->first_optimized_path_cost(
272 cc(*this->first_optimized_path_.back())
275 return this->orig_path();
276 auto opt2 = this->second_path_optimization();
277 auto tend = std::chrono::high_resolution_clock::now();
278 auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
280 this->log_opt_time_.push_back(tdiff.count());
284 Json::Value RRTExt13::json()
286 Json::Value jvo = RRTS::json();
287 jvo["orig_path_cost"] = this->orig_path_cost();
291 unsigned int pcnt = 0;
292 for (auto n: this->orig_path()) {
293 jvo["orig_path"][pcnt][0] = n->x();
294 jvo["orig_path"][pcnt][1] = n->y();
295 jvo["orig_path"][pcnt][2] = n->h();
296 if (n->t(RRTNodeType::cusp))
298 if (n->t(RRTNodeType::connected))
302 jvo["orig_cusps-in-path"] = cu;
303 jvo["orig_connecteds-in-path"] = co;
308 void RRTExt13::json(Json::Value jvi)
310 return RRTS::json(jvi);