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                 if (!s.right()) {
  66                         c.st(c.st() * -1.0);
  67                 }
  68                 for (auto p: s.steer_in_slot(c)) {
  69                         c.next();
  70                         ispath.push_back(c);
  71                 }
  72         }
  73         jvi["goal"][0] = pr.x();
  74         jvi["goal"][1] = pr.y();
  75         jvi["goal"][2] = pr.b();
  76         jvi["goal"][3] = pr.e();
  77         jvi["goal_inside"][0] = ispath.back().x();
  78         jvi["goal_inside"][1] = ispath.back().y();
  79         jvi["goal_inside"][2] = ispath.back().h();
  80         p.json(jvi);
  81         unsigned int icnt = 0;
  82         unsigned int rcnt = 0;
  83         unsigned int bcnt = 0;
  84         unsigned int ncnt = 0; // not better counter
  85         Json::Value best_path;
  86         Json::Value pj;
  87         double cost = 0.0;
  88         while (icnt < 1000) {
  89                 p.icnt(icnt);
  90                 while (p.next()) {}
  91                 icnt = p.icnt();
  92                 pj = p.json();
  93                 if (pj["path"] != Json::nullValue) {
  94                         double gc = pj["goal_cc"].asDouble();
  95                         assert(gc > 0.0);
  96                         if (cost == 0.0 || gc < cost) {
  97                                 if (gc < 0.75 * cost) {
  98                                         ncnt = 0;
  99                                 }
 100                                 best_path = pj["path"];
 101                                 cost = gc;
 102                                 bcnt += 1;
 103                         } else {
 104                                 ncnt++;
 105                         }
 106                 }
 107                 if (ncnt > 5) {
 108                         break;
 109                 }
 110                 p.reset();
 111                 rcnt += 1;
 112         }
 113         double elapsed = p.scnt();
 114         auto jvo = p.json();
 115         unsigned int i = 0;
 116         for (auto p: ispath) {
 117                 jvo["ispath"][i][0] = p.x();
 118                 jvo["ispath"][i][1] = p.y();
 119                 jvo["ispath"][i][2] = p.h();
 120                 i += 1;
 121         }
 122         jvo["init"] = jvi["init"];
 123         jvo["slot"] = jvi["slot"];
 124         jvo["obst"] = jvi["obst"];
 125         jvo["entry"] = jvi["goal"];
 126         jvo["goal"][0] = ispath.back().x();
 127         jvo["goal"][1] = ispath.back().y();
 128         jvo["goal"][2] = ispath.back().h();
 129         jvo["time"] = elapsed;
 130         jvo["icnt"] = p.icnt();
 131         jvo["rcnt"] = rcnt;
 132         jvo["bcnt"] = bcnt;
 133         jvo["path"] = best_path;
 134         jvo["goal_cc"] = cost;
 135         std::cout << jvo << std::endl;
 136         return 0;
 137 }