2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
13 RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
18 RRTExt13::DijkstraNode::vi()
28 RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
29 DijkstraNode const& n2)
31 return n1.node->cc() > n2.node->cc();
35 RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
36 DijkstraNode const& n2)
42 RRTExt13::interesting_forward()
45 this->dn_.reserve(this->path_.size());
46 #if 0 // all path poses are interesting
47 for (auto n: this->opath_) {
48 this->dn_.push_back(DijkstraNode(n));
49 this->dn_.back().i = this->dn_.size() - 1;
53 #if 1 // cusp and 1st non-drivable path poses are interesting
54 this->dn_.push_back(DijkstraNode(this->opath_.front()));
55 this->dn_.back().i = this->dn_.size() - 1;
56 for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
57 RRTNode& ni = *this->opath_[i];
58 this->bc_.set_pose(ni);
59 for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
60 RRTNode& nj = *this->opath_[j];
61 RRTNode& njl = *this->opath_[j - 1];
62 if (!this->bc_.drivable(nj)) {
63 this->dn_.push_back(DijkstraNode(&njl));
64 this->dn_.back().i = this->dn_.size() - 1;
68 if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
69 this->dn_.push_back(DijkstraNode(&njl));
70 this->dn_.back().i = this->dn_.size() - 1;
74 this->dn_.push_back(DijkstraNode(this->opath_.back()));
75 this->dn_.back().i = this->dn_.size() - 1;
80 RRTExt13::interesting_backward()
83 this->dn_.reserve(this->path_.size());
84 #if 0 // all path poses are interesting
85 for (auto n: this->opath_) {
86 this->dn_.push_back(DijkstraNode(n));
87 this->dn_.back().i = this->dn_.size() - 1;
89 std::reverse(this->dn_.begin(), this->dn_.end());
92 #if 1 // cusp and 1st non-drivable path poses are interesting
93 double goal_cc = this->goal_.cc();
94 this->dn_.push_back(DijkstraNode(this->opath_.back()));
95 this->dn_.back().i = this->dn_.size() - 1;
96 this->dn_.back().d = goal_cc - this->opath_.back()->cc();
97 for (unsigned int i = this->opath_.size() - 1; i > 1; i--) {
98 RRTNode& ni = *this->opath_[i];
99 this->bc_.set_pose(ni);
100 for (unsigned int j = i - 1; j > 0; j--) {
101 RRTNode& nj = *this->opath_[j];
102 RRTNode& njl = *this->opath_[j + 1];
103 if (!this->bc_.drivable(nj)) {
104 this->dn_.push_back(DijkstraNode(&njl));
105 this->dn_.back().i = this->dn_.size() - 1;
106 this->dn_.back().d = goal_cc - njl.cc();
110 if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
111 this->dn_.push_back(DijkstraNode(&njl));
112 this->dn_.back().i = this->dn_.size() - 1;
113 this->dn_.back().d = goal_cc - njl.cc();
117 this->dn_.push_back(DijkstraNode(this->opath_.front()));
118 this->dn_.back().i = this->dn_.size() - 1;
119 this->dn_.back().d = goal_cc - this->opath_.front()->cc();
124 RRTExt13::dijkstra_forward()
126 std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
127 DijkstraNodeComparator> pq;
128 this->dn_.front().vi();
129 pq.push(this->dn_.front());
130 while (!pq.empty()) {
131 DijkstraNode fd = pq.top();
132 RRTNode& f = *fd.node;
134 this->bc_.set_pose(f);
135 for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
136 RRTNode& t = *this->dn_[i].node;
137 if (!this->bc_.drivable(t)) {
140 double cost = f.cc() + this->cost_build(f, t);
142 if (this->steered_.size() == 0) {
145 unsigned int ss = this->steered_.size();
146 if (this->collide_steered()
147 || ss != this->steered_.size()) {
150 this->bc_.set_pose(this->steered_.back());
151 bool td = this->bc_.drivable(t);
152 bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
153 if (cost < t.cc() && td && tn) {
154 this->join_steered(&f);
155 t.p(this->nodes_.back());
156 t.c(this->cost_build(this->nodes_.back(), t));
157 if (!this->dn_[i].vi()) {
158 pq.push(this->dn_[i]);
166 RRTExt13::dijkstra_backward()
168 std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
169 DijkstraNodeComparator> pq;
170 this->dn_.front().vi();
171 pq.push(this->dn_.front());
172 while (!pq.empty()) {
173 DijkstraNode td = pq.top();
174 RRTNode& t = *td.node;
176 this->bc_.set_pose(t);
177 for (unsigned int i = td.i; i > 0; i--) {
178 DijkstraNode& fd = this->dn_[i];
179 RRTNode& f = *this->dn_[i].node;
180 if (!this->bc_.drivable(f)) {
183 double cost = td.d + this->cost_build(f, t);
185 if (this->steered_.size() == 0) {
188 unsigned int ss = this->steered_.size();
189 if (this->collide_steered()
190 || ss != this->steered_.size()) {
193 this->bc_.set_pose(this->steered_.back());
194 bool td = this->bc_.drivable(t);
195 bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
196 if (cost < fd.d && td && tn) {
198 this->join_steered(&f);
199 t.p(this->nodes_.back());
200 t.c(this->cost_build(this->nodes_.back(), t));
201 this->recompute_cc(&t);
202 if (!this->dn_[i].vi()) {
203 pq.push(this->dn_[i]);
208 this->recompute_path_cc();
212 RRTExt13::compute_path()
214 RRTS::compute_path();
215 if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
218 #if 1 // TODO 0.59 should work for sc4-1-0 only.
219 if (this->goal_.cc() * 0.8 > this->last_goal_cc_
220 && this->last_goal_cc_ != 0.0) {
224 bool measure_time = false;
225 if (this->opath_.size() == 0) {
226 this->opath_ = this->path_;
227 this->ogoal_cc_ = this->goal_.cc();
231 this->otime_ = -this->ter_.scnt();
233 double curr_cc = this->goal_.cc();
234 double last_cc = curr_cc + 1.0;
235 while (curr_cc < last_cc) {
237 RRTS::compute_path();
238 this->interesting_forward();
239 this->dijkstra_forward();
240 RRTS::compute_path();
241 this->interesting_backward();
242 this->dijkstra_backward();
243 RRTS::compute_path();
244 curr_cc = this->goal_.cc();
247 this->otime_ += this->ter_.scnt();
253 this->opath_.reserve(10000);
254 this->dn_.reserve(10000);
258 RRTExt13::json() const
260 auto jvo = RRTS::json();
262 for (auto n: this->opath_) {
263 jvo["opath"][i][0] = n->x();
264 jvo["opath"][i][1] = n->y();
265 jvo["opath"][i][2] = n->h();
266 jvo["opath"][i][3] = n->sp();
269 jvo["ogoal_cc"] = this->ogoal_cc_;
270 jvo["otime"] = this->otime_;
275 RRTExt13::json(Json::Value jvi)
284 this->opath_.clear();