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 jvo; // JSON output
  27
  28         double offset = -1.256994;
  29         double delta = 0.1;
  30         std::cerr << "#dist,fe cusps,fe time,fer cusps,fer time";
  31         std::cerr << std::endl;
  32         for (int i = 0; i < 14; i++) {
  33                 jvo["goals"] = Json::nullValue;
  34                 // setup
  35                 PSPlanner psp;
  36                 double ppsl = 0.01 + psp.cc().perfect_parking_slot_len();
  37                 double psl = ppsl + offset + i * delta;
  38                 psp.ps().border(
  39                         0, 0,
  40                         2.2, 0,
  41                         2.2, 0 + psl,
  42                         0, 0 + psl
  43                 );
  44                 // run fe planner
  45                 TSTART();
  46                 psp.fe(); // or fer()
  47                 TEND();
  48                 std::cerr << (psl - psp.cc().l() - 0.01);
  49                 std::cerr << ",";
  50                 std::cerr << psp.c();
  51                 std::cerr << ",";
  52                 std::cerr << TDIFF();
  53                 // json output
  54                 jvo["time"] = TDIFF();
  55                 jvo["goal"][0] = psp.gc().x();
  56                 jvo["goal"][1] = psp.gc().y();
  57                 jvo["goal"][2] = psp.gc().h();
  58                 unsigned int icnt = 0;
  59                 for (auto &i: psp.cusps()) {
  60                         jvo["goals"][icnt][0] = i.x();
  61                         jvo["goals"][icnt][1] = i.y();
  62                         jvo["goals"][icnt][2] = i.h();
  63                         icnt++;
  64                 }
  65                 jvo["goals"][icnt][0] = psp.cc().x();
  66                 jvo["goals"][icnt][1] = psp.cc().y();
  67                 jvo["goals"][icnt][2] = psp.cc().h();
  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                 jvo["cusp"] = psp.c();
  77                 std::cout << jvo << std::endl;
  78                 psp.ps().border(
  79                         0, 0,
  80                         2.2, 0,
  81                         2.2, 0 + psl,
  82                         0, 0 + psl
  83                 );
  84                 // run fer planner
  85                 TSTART();
  86                 psp.fer(); // or fer()
  87                 TEND();
  88                 std::cerr << ",";
  89                 std::cerr << psp.c();
  90                 std::cerr << ",";
  91                 std::cerr << TDIFF();
  92                 std::cerr << std::endl;
  93                 // json output
  94                 jvo["time"] = TDIFF();
  95                 jvo["goal"][0] = psp.gc().x();
  96                 jvo["goal"][1] = psp.gc().y();
  97                 jvo["goal"][2] = psp.gc().h();
  98                 icnt = 0;
  99                 for (auto &i: psp.cusps()) {
 100                         jvo["goals"][icnt][0] = i.x();
 101                         jvo["goals"][icnt][1] = i.y();
 102                         jvo["goals"][icnt][2] = i.h();
 103                         icnt++;
 104                 }
 105                 jvo["goals"][icnt][0] = psp.cc().x();
 106                 jvo["goals"][icnt][1] = psp.cc().y();
 107                 jvo["goals"][icnt][2] = psp.cc().h();
 108                 jvo["slot"][0][0][0] = psp.ps().x1();
 109                 jvo["slot"][0][0][1] = psp.ps().y1();
 110                 jvo["slot"][0][1][0] = psp.ps().x2();
 111                 jvo["slot"][0][1][1] = psp.ps().y2();
 112                 jvo["slot"][0][2][0] = psp.ps().x3();
 113                 jvo["slot"][0][2][1] = psp.ps().y3();
 114                 jvo["slot"][0][3][0] = psp.ps().x4();
 115                 jvo["slot"][0][3][1] = psp.ps().y4();
 116                 jvo["cusp"] = psp.c();
 117                 std::cout << jvo << std::endl;
 118         }
 119         return 0;
 120 }