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