src/template-with-reset.cc

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