]> rtime.felk.cvut.cz Git - hubacji1/rrts.git/blob - src/rrtext13.cc
dada6a375ea1416c1c26f8a6d251af31ce22b518
[hubacji1/rrts.git] / src / rrtext13.cc
1 #include <algorithm>
2 #include <queue>
3 #include "rrtext.hh"
4
5 namespace rrts {
6
7 RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
8 {
9 }
10
11 bool
12 RRTExt13::DijkstraNode::vi()
13 {
14         if (this->v) {
15                 return true;
16         }
17         this->v = true;
18         return false;
19 }
20
21 int
22 RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
23                 DijkstraNode const& n2)
24 {
25         return n1.node->cc() > n2.node->cc();
26 }
27
28 void
29 RRTExt13::interesting_forward()
30 {
31         this->dn_.clear();
32         this->dn_.reserve(this->path_.size());
33 #if 0 // all path poses are interesting
34         for (auto n: this->opath_) {
35                 this->dn_.push_back(DijkstraNode(n));
36                 this->dn_.back().i = this->dn_.size() - 1;
37         }
38         return;
39 #endif
40 #if 1 // cusp and 1st non-drivable path poses are interesting
41         this->dn_.push_back(DijkstraNode(this->opath_.front()));
42         this->dn_.back().i = this->dn_.size() - 1;
43         for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
44                 RRTNode& ni = *this->opath_[i];
45                 this->bc_.set_pose(ni);
46                 for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
47                         RRTNode& nj = *this->opath_[j];
48                         RRTNode& njl = *this->opath_[j - 1];
49                         if (!this->bc_.drivable(nj)) {
50                                 this->dn_.push_back(DijkstraNode(&njl));
51                                 this->dn_.back().i = this->dn_.size() - 1;
52                                 i = j;
53                                 break;
54                         }
55                         if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
56                                 this->dn_.push_back(DijkstraNode(&njl));
57                                 this->dn_.back().i = this->dn_.size() - 1;
58                         }
59                 }
60         }
61         this->dn_.push_back(DijkstraNode(this->opath_.back()));
62         this->dn_.back().i = this->dn_.size() - 1;
63 #endif
64 }
65
66 void
67 RRTExt13::interesting_backward()
68 {
69         this->dn_.clear();
70         this->dn_.reserve(this->path_.size());
71 #if 0 // all path poses are interesting
72         for (auto n: this->opath_) {
73                 this->dn_.push_back(DijkstraNode(n));
74                 this->dn_.back().i = this->dn_.size() - 1;
75         }
76         std::reverse(this->dn_.begin(), this->dn_.end());
77         return;
78 #endif
79 #if 1 // cusp and 1st non-drivable path poses are interesting
80         this->dn_.push_back(DijkstraNode(this->opath_.back()));
81         this->dn_.back().i = this->dn_.size() - 1;
82         for (unsigned int i = this->opath_.size() - 1; i > 1; i--) {
83                 RRTNode& ni = *this->opath_[i];
84                 this->bc_.set_pose(ni);
85                 for (unsigned int j = i - 1; j > 0; j--) {
86                         RRTNode& nj = *this->opath_[j];
87                         RRTNode& njl = *this->opath_[j + 1];
88                         if (!this->bc_.drivable(nj)) {
89                                 this->dn_.push_back(DijkstraNode(&njl));
90                                 this->dn_.back().i = this->dn_.size() - 1;
91                                 i = j;
92                                 break;
93                         }
94                         if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
95                                 this->dn_.push_back(DijkstraNode(&njl));
96                                 this->dn_.back().i = this->dn_.size() - 1;
97                         }
98                 }
99         }
100         this->dn_.push_back(DijkstraNode(this->opath_.front()));
101         this->dn_.back().i = this->dn_.size() - 1;
102 #endif
103 }
104
105 void
106 RRTExt13::dijkstra_forward()
107 {
108         std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
109                 DijkstraNodeComparator> pq;
110         this->dn_.front().vi();
111         pq.push(this->dn_.front());
112         while (!pq.empty()) {
113                 DijkstraNode fd = pq.top();
114                 RRTNode& f = *fd.node;
115                 pq.pop();
116                 this->bc_.set_pose(f);
117                 for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
118                         RRTNode& t = *this->dn_[i].node;
119                         if (!this->bc_.drivable(t)) {
120                                 continue;
121                         }
122                         double cost = f.cc() + this->cost_build(f, t);
123                         this->steer(f, t);
124                         if (this->steered_.size() == 0) {
125                                 break;
126                         }
127                         unsigned int ss = this->steered_.size();
128                         if (this->collide_steered()
129                                         || ss != this->steered_.size()) {
130                                 continue;
131                         }
132                         this->bc_.set_pose(this->steered_.back());
133                         bool td = this->bc_.drivable(t);
134                         bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
135                         if (cost < t.cc() && td && tn) {
136                                 this->join_steered(&f);
137                                 t.p(this->nodes_.back());
138                                 t.c(this->cost_build(this->nodes_.back(), t));
139                                 if (!this->dn_[i].vi()) {
140                                         pq.push(this->dn_[i]);
141                                 }
142                         }
143                 }
144         }
145 }
146
147 void
148 RRTExt13::dijkstra_backward()
149 {
150         std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
151                 DijkstraNodeComparator> pq;
152         this->dn_.back().vi();
153         pq.push(this->dn_.back());
154         while (!pq.empty()) {
155                 DijkstraNode td = pq.top();
156                 RRTNode& t = *td.node;
157                 pq.pop();
158                 for (unsigned int i = td.i - 1; i > 0; i--) {
159                         RRTNode& f = *this->dn_[i].node;
160                         double cost = f.cc() + this->cost_build(f, t);
161                         this->steer(f, t);
162                         if (this->steered_.size() == 0) {
163                                 break;
164                         }
165                         if (this->collide_steered()) {
166                                 continue;
167                         }
168                         this->bc_.set_pose(this->steered_.back());
169                         bool td = this->bc_.drivable(t);
170                         bool tn = this->bc_.edist(t) < this->eta_;
171                         if (cost < t.cc() && td && tn) {
172                                 this->join_steered(&f);
173                                 t.p(this->nodes_.back());
174                                 t.c(this->cost_build(this->nodes_.back(), t));
175                                 if (!this->dn_[i].vi()) {
176                                         pq.push(this->dn_[i]);
177                                 }
178                         }
179                 }
180         }
181 }
182
183 void
184 RRTExt13::compute_path()
185 {
186         RRTS::compute_path();
187         if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
188                 return;
189         }
190 #if 0 // TODO 0.59 should work for sc4-1-0 only.
191         if (this->goal_.cc() * 0.59 > this->last_goal_cc_
192                         && this->last_goal_cc_ != 0.0) {
193                 return;
194         }
195 #endif
196         bool measure_time = false;
197         if (this->opath_.size() == 0) {
198                 this->opath_ = this->path_;
199                 this->ogoal_cc_ = this->goal_.cc();
200                 measure_time = true;
201         }
202         if (measure_time) {
203                 this->otime_ = -this->ter_.scnt();
204         }
205         double curr_cc = this->goal_.cc();
206         double last_cc = curr_cc + 1.0;
207         while (curr_cc < last_cc) {
208                 last_cc = curr_cc;
209                 RRTS::compute_path();
210                 this->interesting_forward();
211                 this->dijkstra_forward();
212                 RRTS::compute_path();
213                 this->interesting_backward();
214                 this->dijkstra_backward();
215                 RRTS::compute_path();
216                 curr_cc = this->goal_.cc();
217         }
218         if (measure_time) {
219                 this->otime_ += this->ter_.scnt();
220         }
221 }
222
223 RRTExt13::RRTExt13()
224 {
225         this->opath_.reserve(10000);
226         this->dn_.reserve(10000);
227 }
228
229 Json::Value
230 RRTExt13::json() const
231 {
232         auto jvo = RRTS::json();
233         unsigned int i = 0;
234         for (auto n: this->opath_) {
235                 jvo["opath"][i][0] = n->x();
236                 jvo["opath"][i][1] = n->y();
237                 jvo["opath"][i][2] = n->h();
238                 i++;
239         }
240         jvo["ogoal_cc"] = this->ogoal_cc_;
241         jvo["otime"] = this->otime_;
242         return jvo;
243 }
244
245 void
246 RRTExt13::json(Json::Value jvi)
247 {
248         RRTS::json(jvi);
249 }
250
251 void
252 RRTExt13::reset()
253 {
254         RRTS::reset();
255         this->opath_.clear();
256 }
257
258 } // namespace rrts