src/rrtext13.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <algorithm>
   8 #include <queue>
   9 #include "rrtext.hh"
  10
  11 namespace rrts {
  12
  13 RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
  14 {
  15 }
  16
  17 bool
  18 RRTExt13::DijkstraNode::vi()
  19 {
  20         if (this->v) {
  21                 return true;
  22         }
  23         this->v = true;
  24         return false;
  25 }
  26
  27 int
  28 RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
  29                 DijkstraNode const& n2)
  30 {
  31         return n1.node->cc() > n2.node->cc();
  32 }
  33
  34 int
  35 RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
  36                 DijkstraNode const& n2)
  37 {
  38         return n1.d > n2.d;
  39 }
  40
  41 void
  42 RRTExt13::interesting_forward()
  43 {
  44         this->dn_.clear();
  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;
  50         }
  51         return;
  52 #endif
  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;
  65                                 i = j;
  66                                 break;
  67                         }
  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;
  71                         }
  72                 }
  73         }
  74         this->dn_.push_back(DijkstraNode(this->opath_.back()));
  75         this->dn_.back().i = this->dn_.size() - 1;
  76 #endif
  77 }
  78
  79 void
  80 RRTExt13::interesting_backward()
  81 {
  82         this->dn_.clear();
  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;
  88         }
  89         std::reverse(this->dn_.begin(), this->dn_.end());
  90         return;
  91 #endif
  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();
 107                                 i = j;
 108                                 break;
 109                         }
 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();
 114                         }
 115                 }
 116         }
 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();
 120 #endif
 121 }
 122
 123 void
 124 RRTExt13::dijkstra_forward()
 125 {
 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;
 133                 pq.pop();
 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)) {
 138                                 continue;
 139                         }
 140                         double cost = f.cc() + this->cost_build(f, t);
 141                         this->steer(f, t);
 142                         if (this->steered_.size() == 0) {
 143                                 break;
 144                         }
 145                         unsigned int ss = this->steered_.size();
 146                         if (this->collide_steered()
 147                                         || ss != this->steered_.size()) {
 148                                 continue;
 149                         }
 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]);
 159                                 }
 160                         }
 161                 }
 162         }
 163 }
 164
 165 void
 166 RRTExt13::dijkstra_backward()
 167 {
 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;
 175                 pq.pop();
 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)) {
 181                                 continue;
 182                         }
 183                         double cost = td.d + this->cost_build(f, t);
 184                         this->steer(f, t);
 185                         if (this->steered_.size() == 0) {
 186                                 break;
 187                         }
 188                         unsigned int ss = this->steered_.size();
 189                         if (this->collide_steered()
 190                                         || ss != this->steered_.size()) {
 191                                 continue;
 192                         }
 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) {
 197                                 fd.d = cost;
 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]);
 204                                 }
 205                         }
 206                 }
 207         }
 208         this->recompute_path_cc();
 209 }
 210
 211 void
 212 RRTExt13::compute_path()
 213 {
 214         RRTS::compute_path();
 215         if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
 216                 return;
 217         }
 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) {
 221                 return;
 222         }
 223 #endif
 224         bool measure_time = false;
 225         if (this->opath_.size() == 0) {
 226                 this->opath_ = this->path_;
 227                 this->ogoal_cc_ = this->goal_.cc();
 228                 measure_time = true;
 229         }
 230         if (measure_time) {
 231                 this->otime_ = -this->ter_.scnt();
 232         }
 233         double curr_cc = this->goal_.cc();
 234         double last_cc = curr_cc + 1.0;
 235         while (curr_cc < last_cc) {
 236                 last_cc = curr_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();
 245         }
 246         if (measure_time) {
 247                 this->otime_ += this->ter_.scnt();
 248         }
 249 }
 250
 251 RRTExt13::RRTExt13()
 252 {
 253         this->opath_.reserve(10000);
 254         this->dn_.reserve(10000);
 255 }
 256
 257 Json::Value
 258 RRTExt13::json() const
 259 {
 260         auto jvo = RRTS::json();
 261         unsigned int i = 0;
 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                 i++;
 267         }
 268         jvo["ogoal_cc"] = this->ogoal_cc_;
 269         jvo["otime"] = this->otime_;
 270         return jvo;
 271 }
 272
 273 void
 274 RRTExt13::json(Json::Value jvi)
 275 {
 276         RRTS::json(jvi);
 277 }
 278
 279 void
 280 RRTExt13::reset()
 281 {
 282         RRTS::reset();
 283         this->opath_.clear();
 284 }
 285
 286 } // namespace rrts