src/test8.cc

   1 #include <chrono>
   2 #include <iostream>
   3 #include <json/json.h>
   4
   5 #include "rrtce.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         if (jvi["init"] == Json::nullValue) {
  30                 std::cerr << "I need `init` in JSON input scenario";
  31                 std::cerr << std::endl;
  32                 return 1;
  33         }
  34         if (jvi["goal"] == Json::nullValue) {
  35                 std::cerr << "I need `goal` in JSON input scenario";
  36                 std::cerr << std::endl;
  37                 return 1;
  38         }
  39         if (jvi["goals"] == Json::nullValue) {
  40                 std::cerr << "I need `goals` in JSON input scenario";
  41                 std::cerr << std::endl;
  42                 return 1;
  43         }
  44         if (jvi["obst"] == Json::nullValue) {
  45                 std::cerr << "I need `obst` in JSON input scenario";
  46                 std::cerr << std::endl;
  47                 return 1;
  48         }
  49
  50         RRTCE5 rrts;
  51         rrts.nodes().front().x(jvi["init"][0].asDouble());
  52         rrts.nodes().front().y(jvi["init"][1].asDouble());
  53         rrts.nodes().front().h(jvi["init"][2].asDouble());
  54         {
  55                 RRTNode tmp_node;
  56                 tmp_node.x(jvi["goal"][0].asDouble());
  57                 tmp_node.y(jvi["goal"][1].asDouble());
  58                 tmp_node.h(jvi["goal"][2].asDouble());
  59                 rrts.goals().push_back(tmp_node);
  60                 for (auto g: jvi["goals"]) {
  61                         tmp_node.x(g[0].asDouble());
  62                         tmp_node.y(g[1].asDouble());
  63                         tmp_node.h(g[2].asDouble());
  64                         rrts.goals().push_back(tmp_node);
  65                 }
  66         }
  67         {
  68                 Obstacle tmp_obstacle;
  69                 for (auto o: jvi["obst"]) {
  70                         tmp_obstacle.poly().clear();
  71                         for (auto c: o) {
  72                                 double tmp_x = c[0].asDouble();
  73                                 double tmp_y = c[1].asDouble();
  74                                 auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
  75                                 tmp_obstacle.poly().push_back(tmp_tuple);
  76                         }
  77                         rrts.obstacles().push_back(tmp_obstacle);
  78                 }
  79         }
  80         {
  81                 double edist_init_goal = sqrt(
  82                         pow(
  83                                 rrts.nodes().front().x()
  84                                 - rrts.goals().front().x(),
  85                                 2
  86                         )
  87                         + pow(
  88                                 rrts.nodes().front().y()
  89                                 - rrts.goals().front().y(),
  90                                 2
  91                         )
  92                 );
  93                 rrts.set_sample(
  94                         rrts.nodes().front().x(), edist_init_goal,
  95                         rrts.nodes().front().y(), edist_init_goal,
  96                         0, 2 * M_PI
  97                 );
  98         }
  99
 100         rrts.init();
 101         TSTART();
 102         while (rrts.next()) {}
 103         TEND();
 104
 105         std::cout << rrts.json() << std::endl;
 106         return 0;
 107 }