src/uniform-sampling-with-reset-template.cc

   1 #include <algorithm>
   2 #include <chrono>
   3 #include <iostream>
   4 #include <json/json.h>
   5
   6 #include "psp.h"
   7 #include "rrtce.h"
   8 #ifndef EPP
   9         #define EPP RRTS
  10 #endif
  11
  12 double edist(double x1, double y1, double x2, double y2)
  13 {
  14         return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
  15 }
  16
  17 std::chrono::high_resolution_clock::time_point TSTART_;
  18 std::chrono::high_resolution_clock::time_point TEND_;
  19 inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
  20 inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
  21 inline double TDIFF()
  22 {
  23         std::chrono::duration<double> DT_;
  24         DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
  25                 TEND_ - TSTART_
  26         );
  27         return DT_.count();
  28 }
  29 inline void TPRINT(const char *what)
  30 {
  31         std::cerr << what << ": " << TDIFF() << std::endl;
  32 }
  33
  34 PSPlanner psp;
  35 EPP epp;
  36 int main()
  37 {
  38         Json::Value jvi; // JSON input
  39         Json::Value jvo; // JSON output
  40         Json::Value best_path; // JSON output
  41         std::cin >> jvi;
  42         unsigned int i = 0;
  43
  44         psp.ps().border(
  45                 jvi["slot"][0][0][0].asDouble(),
  46                 jvi["slot"][0][0][1].asDouble(),
  47
  48                 jvi["slot"][0][1][0].asDouble(),
  49                 jvi["slot"][0][1][1].asDouble(),
  50
  51                 jvi["slot"][0][2][0].asDouble(),
  52                 jvi["slot"][0][2][1].asDouble(),
  53
  54                 jvi["slot"][0][3][0].asDouble(),
  55                 jvi["slot"][0][3][1].asDouble()
  56         );
  57         psp.fe();
  58
  59         auto lm = psp.last_maneuver();
  60         std::reverse(lm.begin(), lm.end());
  61         i = 0;
  62         for (auto g: lm) {
  63                 jvi["goals"][i][0] = g.x();
  64                 jvi["goals"][i][1] = g.y();
  65                 jvi["goals"][i][2] = g.h();
  66                 i++;
  67         }
  68
  69         epp.json(jvi);
  70         epp.init();
  71         epp.sample_dist_type(2);
  72         // TODO the following has no meaning for uniform circle sampling
  73         epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI);
  74         unsigned int icnt = 0;
  75         unsigned int rcnt = 0;
  76         unsigned int bcnt = 0;
  77         double cost = 9999.9;
  78
  79         while (icnt < 1000) {
  80                 epp.icnt(icnt);
  81                 while (epp.next()) {
  82                 }
  83                 icnt = epp.icnt();
  84                 rcnt += 1;
  85                 auto epp_path = epp.path();
  86                 if (
  87                         epp.gf()
  88                         && epp_path.size() > 0
  89                         && epp_path.back()->cc < cost
  90                 ) {
  91                         cost = epp_path.back()->cc;
  92                         best_path = epp.json();
  93                         bcnt += 1;
  94                 }
  95                 epp.reset();
  96         }
  97
  98         jvo = epp.json();
  99         jvo["path"] = best_path["path"];
 100         jvo["goal"] = best_path["goal"];
 101         jvo["slot"] = jvi["slot"];
 102         jvo["bcnt"] = bcnt; // better
 103         jvo["rcnt"] = rcnt; // reset
 104         std::cout << jvo << std::endl;
 105         return 0;
 106 }