src/test1.cc

   1 #include <chrono>
   2 #include <iostream>
   3 #include <jsoncpp/json/json.h>
   4
   5 #include "psp.h"
   6
   7 std::chrono::high_resolution_clock::time_point TSTART_;
   8 std::chrono::high_resolution_clock::time_point TEND_;
   9 inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
  10 inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
  11 inline double TDIFF()
  12 {
  13         std::chrono::duration<double> DT_;
  14         DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
  15                 TEND_ - TSTART_
  16         );
  17         return DT_.count();
  18 }
  19 inline void TPRINT(const char *what)
  20 {
  21         std::cerr << what << ": " << TDIFF() << std::endl;
  22 }
  23
  24 int main()
  25 {
  26         Json::Value jvi; // JSON input
  27         Json::Value jvo; // JSON output
  28         std::cin >> jvi;
  29
  30         if (jvi["slot"] == Json::nullValue) {
  31                 std::cerr << "I need `slot` in JSON input scenario";
  32                 std::cerr << std::endl;
  33                 return 1;
  34         }
  35
  36         PSPlanner psp;
  37         // use 1st slot (index 0)
  38         psp.ps().border(
  39                 jvi["slot"][0][0][0].asDouble(),
  40                 jvi["slot"][0][0][1].asDouble(),
  41                 jvi["slot"][0][1][0].asDouble(),
  42                 jvi["slot"][0][1][1].asDouble(),
  43                 jvi["slot"][0][2][0].asDouble(),
  44                 jvi["slot"][0][2][1].asDouble(),
  45                 jvi["slot"][0][3][0].asDouble(),
  46                 jvi["slot"][0][3][1].asDouble()
  47         );
  48         psp.guess_gc();
  49         psp.cc() = BicycleCar(psp.gc());
  50         TSTART();
  51         psp.fe();
  52         TEND();
  53
  54         {
  55                 jvo["time"] = TDIFF();
  56         }
  57         {
  58                 jvo["init"][0] = psp.cc().x();
  59                 jvo["init"][1] = psp.cc().y();
  60                 jvo["init"][2] = psp.cc().h();
  61         }
  62         {
  63                 jvo["goal"][0] = psp.gc().x();
  64                 jvo["goal"][1] = psp.gc().y();
  65                 jvo["goal"][2] = psp.gc().h();
  66         }
  67         {
  68                 jvo["slot"][0][0][0] = psp.ps().x1();
  69                 jvo["slot"][0][0][1] = psp.ps().y1();
  70                 jvo["slot"][0][1][0] = psp.ps().x2();
  71                 jvo["slot"][0][1][1] = psp.ps().y2();
  72                 jvo["slot"][0][2][0] = psp.ps().x3();
  73                 jvo["slot"][0][2][1] = psp.ps().y3();
  74                 jvo["slot"][0][3][0] = psp.ps().x4();
  75                 jvo["slot"][0][3][1] = psp.ps().y4();
  76         }
  77         std::cout << jvo << std::endl;
  78         return 0;
  79 }