src/template.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         while (p.next()) {}
  76         auto jvo = p.json();
  77         unsigned int i = 0;
  78         for (auto p: ispath) {
  79                 jvo["ispath"][i][0] = p.x();
  80                 jvo["ispath"][i][1] = p.y();
  81                 jvo["ispath"][i][2] = p.h();
  82                 i += 1;
  83         }
  84         jvo["icnt"] = p.icnt();
  85         jvo["init"] = jvi["init"];
  86         jvo["slot"] = jvi["slot"];
  87         jvo["obst"] = jvi["obst"];
  88         jvo["entry"] = jvi["goal"];
  89         jvo["goal"][0] = ispath.back().x();
  90         jvo["goal"][1] = ispath.back().y();
  91         jvo["goal"][2] = ispath.back().h();
  92         std::cout << jvo << std::endl;
  93         return 0;
  94 }