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