src/template-with-reset.cc

   1 #include <cassert>
   2 #include <iostream>
   3 #include <json/json.h>
   4 #include <vector>
   5 // When the file with precomputed entries is ready.
   6 //#include "fiat_punto.h"
   7 #include "pslot.hh"
   8 #include "rrtsp.hh"
   9
  10 #ifndef OSP
  11         #define OSP P36
  12 #endif
  13
  14 rrts::OSP p;
  15
  16 int main()
  17 {
  18         Json::Value jvi;
  19         std::cin >> jvi;
  20         std::cout << std::fixed;
  21         std::cerr << std::fixed;
  22         assert(jvi["slot"] != Json::nullValue);
  23         rrts::Ter ip_time;
  24         ip_time.start();
  25         bcar::ParkingSlot s(
  26                 jvi["slot"][0][0].asDouble(), jvi["slot"][0][1].asDouble(),
  27                 jvi["slot"][1][0].asDouble(), jvi["slot"][1][1].asDouble(),
  28                 jvi["slot"][2][0].asDouble(), jvi["slot"][2][1].asDouble(),
  29                 jvi["slot"][3][0].asDouble(), jvi["slot"][3][1].asDouble());
  30         bcar::BicycleCar c;
  31         bool swapped = false;
  32         if (s.parallel() && !s.right()) {
  33                 s.swap_side();
  34                 swapped = true;
  35         }
  36         auto pr = s.fe(c);
  37         if (swapped) {
  38                 s.swap_side();
  39                 pr.reflect(s.entry());
  40         }
  41         //std::cerr << ip_time.scnt() << std::endl;
  42         // The following uses precomputed entries and needs `"fiat_punto.h"` to
  43         // be included.
  44         //auto pr = get_fiat_punto_entry(s.len(), s.w());
  45         //pr = s.recompute_entry(pr);
  46         c.x(pr.x());
  47         c.y(pr.y());
  48         c.h(pr.h());
  49         c.sp(-0.1);
  50         c.st(0.0);
  51         std::vector<bcar::Pose> ispath;
  52         if (s.right()) {
  53                 while (!c.rf().on_right_side_of(s.entry())) {
  54                         c.next();
  55                         ispath.push_back(c);
  56                 }
  57         } else {
  58                 while (c.lf().on_right_side_of(s.entry())) {
  59                         c.next();
  60                         ispath.push_back(c);
  61                 }
  62         }
  63         if (s.parallel()) {
  64                 c.set_max_steer();
  65                 for (auto &n : s.drive_in_slot(c)) {
  66                         ispath.push_back(n);
  67                 }
  68         }
  69         jvi["goal"][0] = pr.x();
  70         jvi["goal"][1] = pr.y();
  71         jvi["goal"][2] = pr.b();
  72         jvi["goal"][3] = pr.e();
  73         jvi["goal_inside"][0] = ispath.back().x();
  74         jvi["goal_inside"][1] = ispath.back().y();
  75         jvi["goal_inside"][2] = ispath.back().h();
  76         p.json(jvi);
  77         unsigned int icnt = 0;
  78         unsigned int rcnt = 0;
  79         unsigned int bcnt = 0;
  80         unsigned int ncnt = 0; // not better counter
  81         Json::Value best_path;
  82         Json::Value pj;
  83         double cost = 0.0;
  84         p.reset();
  85         while (icnt < 1000) {
  86                 p.icnt(icnt);
  87                 while (p.next()) {}
  88                 icnt = p.icnt();
  89                 pj = p.json();
  90                 if (pj["path"] != Json::nullValue) {
  91                         double gc = pj["goal_cc"].asDouble();
  92                         assert(gc > 0.0);
  93                         if (cost == 0.0 || gc < cost) {
  94                                 if (gc < 0.75 * cost) {
  95                                         ncnt = 0;
  96                                 }
  97                                 best_path = pj["path"];
  98                                 cost = gc;
  99                                 bcnt += 1;
 100                         } else {
 101                                 ncnt++;
 102                         }
 103                 }
 104                 if (ncnt > 5) {
 105                         break;
 106                 }
 107                 p.reset();
 108                 rcnt += 1;
 109         }
 110         double elapsed = p.scnt();
 111         auto jvo = p.json();
 112         unsigned int i = 0;
 113         for (auto p: ispath) {
 114                 jvo["ispath"][i][0] = p.x();
 115                 jvo["ispath"][i][1] = p.y();
 116                 jvo["ispath"][i][2] = p.h();
 117                 i += 1;
 118         }
 119         jvo["init"] = jvi["init"];
 120         jvo["slot"] = jvi["slot"];
 121         jvo["obst"] = jvi["obst"];
 122         jvo["entry"] = jvi["goal"];
 123         jvo["goal"][0] = ispath.back().x();
 124         jvo["goal"][1] = ispath.back().y();
 125         jvo["goal"][2] = ispath.back().h();
 126         jvo["time"] = elapsed;
 127         jvo["icnt"] = p.icnt();
 128         jvo["rcnt"] = rcnt;
 129         jvo["ncnt"] = ncnt;
 130         jvo["bcnt"] = bcnt;
 131         jvo["path"] = best_path;
 132         jvo["goal_cc"] = cost;
 133         std::cout << jvo << std::endl;
 134         return 0;
 135 }