7 RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
12 RRTExt13::DijkstraNode::vi()
22 RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
23 DijkstraNode const& n2)
25 return n1.node->cc() > n2.node->cc();
29 RRTExt13::interesting_forward()
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;
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;
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;
61 this->dn_.push_back(DijkstraNode(this->opath_.back()));
62 this->dn_.back().i = this->dn_.size() - 1;
67 RRTExt13::interesting_backward()
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;
76 std::reverse(this->dn_.begin(), this->dn_.end());
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;
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;
100 this->dn_.push_back(DijkstraNode(this->opath_.front()));
101 this->dn_.back().i = this->dn_.size() - 1;
106 RRTExt13::dijkstra_forward()
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;
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)) {
122 double cost = f.cc() + this->cost_build(f, t);
124 if (this->steered_.size() == 0) {
127 unsigned int ss = this->steered_.size();
128 if (this->collide_steered()
129 || ss != this->steered_.size()) {
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]);
148 RRTExt13::dijkstra_backward()
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;
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);
162 if (this->steered_.size() == 0) {
165 if (this->collide_steered()) {
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]);
184 RRTExt13::compute_path()
186 RRTS::compute_path();
187 if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
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) {
196 bool measure_time = false;
197 if (this->opath_.size() == 0) {
198 this->opath_ = this->path_;
199 this->ogoal_cc_ = this->goal_.cc();
203 this->otime_ = -this->ter_.scnt();
205 double curr_cc = this->goal_.cc();
206 double last_cc = curr_cc + 1.0;
207 while (curr_cc < last_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();
219 this->otime_ += this->ter_.scnt();
225 this->opath_.reserve(10000);
226 this->dn_.reserve(10000);
230 RRTExt13::json() const
232 auto jvo = RRTS::json();
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();
240 jvo["ogoal_cc"] = this->ogoal_cc_;
241 jvo["otime"] = this->otime_;
246 RRTExt13::json(Json::Value jvi)
255 this->opath_.clear();