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         // wang 2017
  32         //c.ctc(8.411896337925237);
  33         //c.w(1.81);
  34         //c.wb(2.7);
  35         //c.df(3.7);
  36         //c.len(4.85);
  37         // jhang 2020 https://en.wikipedia.org/wiki/Chrysler_Pacifica_(minivan)
  38         //c.ctc(9.557619159602458);
  39         //c.w(2.022);
  40         //c.wb(3.089);
  41         //c.df(4.236);
  42         //c.len(5.171);
  43         // feng 2018
  44         //p.eta(0.1);
  45         // PES Porsche Cayenne
  46         //c.ctc(11.2531); // this is guess
  47         //c.w(1.983);
  48         //c.wb(2.895);
  49         //c.df(2.895 + 0.9); // this is guess
  50         //c.len(4.918);
  51         c.become("porsche cayenne");
  52
  53         bool swapped = false;
  54         if (s.parallel() && !s.right()) {
  55                 s.swap_side();
  56                 swapped = true;
  57         }
  58         auto pr = s.fe(c);
  59         if (swapped) {
  60                 s.swap_side();
  61                 pr.reflect(s.entry());
  62         }
  63         //std::cerr << ip_time.scnt() << std::endl;
  64         // The following uses precomputed entries and needs `"fiat_punto.h"` to
  65         // be included.
  66         //auto pr = get_fiat_punto_entry(s.len(), s.w());
  67         //pr = s.recompute_entry(pr);
  68         c.x(pr.x());
  69         c.y(pr.y());
  70         c.h(pr.h());
  71         c.sp(-0.1);
  72         c.st(0.0);
  73         std::vector<bcar::Pose> ispath;
  74         if (s.right()) {
  75                 while (!c.rf().on_right_side_of(s.entry())) {
  76                         c.next();
  77                         ispath.push_back(c);
  78                 }
  79         } else {
  80                 while (c.lf().on_right_side_of(s.entry())) {
  81                         c.next();
  82                         ispath.push_back(c);
  83                 }
  84         }
  85         if (s.parallel()) {
  86                 c.set_max_steer();
  87                 for (auto &n : s.drive_in_slot(c)) {
  88                         ispath.push_back(n);
  89                 }
  90         }
  91         jvi["goal"][0] = pr.x();
  92         jvi["goal"][1] = pr.y();
  93         jvi["goal"][2] = pr.b();
  94         jvi["goal"][3] = pr.e();
  95         jvi["goal_inside"][0] = ispath.back().x();
  96         jvi["goal_inside"][1] = ispath.back().y();
  97         jvi["goal_inside"][2] = ispath.back().h();
  98
  99         // Opel Corsa
 100         //p.bc().ctc(9.900);
 101         //p.bc().w(1.532);
 102         //p.bc().wb(2.343);
 103         //p.bc().df(3.212);
 104         //p.bc().len(3.622);
 105         // wang 2017
 106         //p.bc().ctc(8.411896337925237);
 107         //p.bc().w(1.81);
 108         //p.bc().wb(2.7);
 109         //p.bc().df(3.7);
 110         //p.bc().len(4.85);
 111         // jhang 2020 https://en.wikipedia.org/wiki/Chrysler_Pacifica_(minivan)
 112         //p.bc().ctc(9.557619159602458);
 113         //p.bc().w(2.022);
 114         //p.bc().wb(3.089);
 115         //p.bc().df(4.236);
 116         //p.bc().len(5.171);
 117         //p.eta(0.3);
 118         // feng 2018
 119         //p.eta(0.1);
 120         // PES Porsche Cayenne
 121         //p.bc().ctc(11.2531); // this is guess
 122         //p.bc().w(1.983);
 123         //p.bc().wb(2.895);
 124         //p.bc().df(2.895 + 0.9); // this is guess
 125         //p.bc().len(4.918);
 126         //p.eta(0.1);
 127         c.become("porsche cayenne");
 128
 129         p.json(jvi);
 130         unsigned int icnt = 0;
 131         unsigned int rcnt = 0;
 132         unsigned int bcnt = 0;
 133         unsigned int ncnt = 0; // not better counter
 134         Json::Value best_path;
 135         Json::Value pj;
 136         double cost = 0.0;
 137         p.reset();
 138         while (icnt < 1000) {
 139                 p.icnt(icnt);
 140                 while (p.next()) {}
 141                 icnt = p.icnt();
 142                 pj = p.json();
 143                 if (pj["path"] != Json::nullValue) {
 144                         double gc = pj["goal_cc"].asDouble();
 145                         assert(gc > 0.0);
 146                         if (cost == 0.0 || gc < cost) {
 147                                 if (gc < 0.75 * cost) {
 148                                         ncnt = 0;
 149                                 }
 150                                 best_path = pj["path"];
 151                                 cost = gc;
 152                                 bcnt += 1;
 153                         } else {
 154                                 ncnt++;
 155                         }
 156                 }
 157                 if (ncnt > 5) {
 158                         break;
 159                 }
 160                 p.reset();
 161                 rcnt += 1;
 162         }
 163         double elapsed = p.scnt();
 164         auto jvo = p.json();
 165         unsigned int i = 0;
 166         for (auto p: ispath) {
 167                 jvo["ispath"][i][0] = p.x();
 168                 jvo["ispath"][i][1] = p.y();
 169                 jvo["ispath"][i][2] = p.h();
 170                 i += 1;
 171         }
 172         jvo["init"] = jvi["init"];
 173         jvo["slot"] = jvi["slot"];
 174         jvo["obst"] = jvi["obst"];
 175         jvo["entry"] = jvi["goal"];
 176         jvo["goal"][0] = ispath.back().x();
 177         jvo["goal"][1] = ispath.back().y();
 178         jvo["goal"][2] = ispath.back().h();
 179         jvo["time"] = elapsed;
 180         jvo["icnt"] = p.icnt();
 181         jvo["rcnt"] = rcnt;
 182         jvo["ncnt"] = ncnt;
 183         jvo["bcnt"] = bcnt;
 184         jvo["path"] = best_path;
 185         jvo["goal_cc"] = cost;
 186         std::cout << jvo << std::endl;
 187         return 0;
 188 }