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] = jvi["init"][0];
  59                 jvo["init"][1] = jvi["init"][1];
  60                 jvo["init"][2] = jvi["init"][2];
  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                 unsigned int icnt = 0;
  69                 for (auto &i: psp.possible_goals(10, 50)) {
  70                         jvo["goals"][icnt][0] = i.x();
  71                         jvo["goals"][icnt][1] = i.y();
  72                         jvo["goals"][icnt][2] = i.h();
  73                         icnt++;
  74                 }
  75                 jvo["goals"][icnt][0] = psp.cc().x();
  76                 jvo["goals"][icnt][1] = psp.cc().y();
  77                 jvo["goals"][icnt][2] = psp.cc().h();
  78         }
  79         {
  80                 jvo["slot"][0][0][0] = psp.ps().x1();
  81                 jvo["slot"][0][0][1] = psp.ps().y1();
  82                 jvo["slot"][0][1][0] = psp.ps().x2();
  83                 jvo["slot"][0][1][1] = psp.ps().y2();
  84                 jvo["slot"][0][2][0] = psp.ps().x3();
  85                 jvo["slot"][0][2][1] = psp.ps().y3();
  86                 jvo["slot"][0][3][0] = psp.ps().x4();
  87                 jvo["slot"][0][3][1] = psp.ps().y4();
  88         }
  89         {
  90                 unsigned int ocnt = 0;
  91                 for (auto o: jvi["obst"]) {
  92                         unsigned int ccnt = 0;
  93                         for (auto c: o) {
  94                                 jvo["obst"][ocnt][ccnt][0] = c[0];
  95                                 jvo["obst"][ocnt][ccnt][1] = c[1];
  96                                 ccnt++;
  97                         }
  98                         ocnt++;
  99                 }
 100         }
 101         std::cout << jvo << std::endl;
 102         return 0;
 103 }