src/test3.cc

   1 #include <chrono>
   2 #include <iostream>
   3 #include <jsoncpp/json/json.h>
   4
   5 #include "rrtext.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["init"] == Json::nullValue) {
  31                 std::cerr << "I need `init` in JSON input scenario";
  32                 std::cerr << std::endl;
  33                 return 1;
  34         }
  35
  36         if (jvi["goal"] == Json::nullValue) {
  37                 std::cerr << "I need `goal` in JSON input scenario";
  38                 std::cerr << std::endl;
  39                 return 1;
  40         }
  41
  42         if (jvi["goals"] == Json::nullValue) {
  43                 std::cerr << "I need `goals` in JSON input scenario";
  44                 std::cerr << std::endl;
  45                 return 1;
  46         }
  47
  48         if (jvi["obst"] == Json::nullValue) {
  49                 std::cerr << "I need `obst` in JSON input scenario";
  50                 std::cerr << std::endl;
  51                 return 1;
  52         }
  53
  54         RRTExt2 rrts;
  55         rrts.nodes().front().x(jvi["init"][0].asDouble());
  56         rrts.nodes().front().y(jvi["init"][1].asDouble());
  57         rrts.nodes().front().h(jvi["init"][2].asDouble());
  58         {
  59                 RRTNode tmp_node;
  60                 tmp_node.x(jvi["goal"][0].asDouble());
  61                 tmp_node.y(jvi["goal"][1].asDouble());
  62                 tmp_node.h(jvi["goal"][2].asDouble());
  63                 rrts.goals().push_back(tmp_node);
  64                 for (auto g: jvi["goals"]) {
  65                         tmp_node.x(g[0].asDouble());
  66                         tmp_node.y(g[1].asDouble());
  67                         tmp_node.h(g[2].asDouble());
  68                         rrts.goals().push_back(tmp_node);
  69                 }
  70         }
  71         {
  72                 Obstacle tmp_obstacle;
  73                 for (auto o: jvi["obst"]) {
  74                         tmp_obstacle.poly().clear();
  75                         for (auto c: o) {
  76                                 double tmp_x = c[0].asDouble();
  77                                 double tmp_y = c[1].asDouble();
  78                                 auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
  79                                 tmp_obstacle.poly().push_back(tmp_tuple);
  80                         }
  81                         rrts.obstacles().push_back(tmp_obstacle);
  82                 }
  83         }
  84
  85         {
  86                 double edist_init_goal = sqrt(
  87                         pow(
  88                                 rrts.nodes().front().x()
  89                                 - rrts.goals().front().x(),
  90                                 2
  91                         )
  92                         + pow(
  93                                 rrts.nodes().front().y()
  94                                 - rrts.goals().front().y(),
  95                                 2
  96                         )
  97                 );
  98                 rrts.set_sample(
  99                         rrts.nodes().front().x(), edist_init_goal,
 100                         rrts.nodes().front().y(), edist_init_goal,
 101                         0, 2 * M_PI
 102                 );
 103         }
 104
 105         rrts.init();
 106         TSTART();
 107         while (rrts.next()) {}
 108         TEND();
 109
 110         {
 111                 jvo["time"] = TDIFF();
 112         }
 113         {
 114                 if (rrts.path().size() > 0) {
 115                         jvo["cost"] = cc(*rrts.path().back());
 116                 } else {
 117                         jvo["cost"] = -1;
 118                 }
 119         }
 120         {
 121                 jvo["init"][0] = rrts.nodes().front().x();
 122                 jvo["init"][1] = rrts.nodes().front().y();
 123                 jvo["init"][2] = rrts.nodes().front().h();
 124         }
 125         {
 126                 unsigned int ocnt = 0;
 127                 for (auto o: rrts.obstacles()) {
 128                         unsigned int ccnt = 0;
 129                         for (auto c: o.poly()) {
 130                                 jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
 131                                 jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
 132                                 ccnt++;
 133                         }
 134                         ocnt++;
 135                 }
 136         }
 137         //{
 138         //        unsigned int ncnt = 0;
 139         //        for (auto n: rrts.nodes()) {
 140         //                jvo["nodes_x"][ncnt] = n.x();
 141         //                jvo["nodes_y"][ncnt] = n.y();
 142         //                //jvo["nodes_h"][ncnt] = n.h();
 143         //                ncnt++;
 144         //        }
 145         //}
 146         {
 147                 unsigned int gcnt = 0;
 148                 for (auto g: rrts.goals()) {
 149                         jvo["goals"][gcnt][0] = g.x();
 150                         jvo["goals"][gcnt][1] = g.y();
 151                         jvo["goals"][gcnt][2] = g.h();
 152                         gcnt++;
 153                 }
 154         }
 155         {
 156                 if (rrts.path().size() > 0) {
 157                         jvo["goal"][0] = rrts.path().back()->x();
 158                         jvo["goal"][1] = rrts.path().back()->y();
 159                         jvo["goal"][2] = rrts.path().back()->h();
 160                 }
 161                 unsigned int cu = 0;
 162                 unsigned int co = 0;
 163                 unsigned int pcnt = 0;
 164                 for (auto n: rrts.path()) {
 165                         jvo["path"][pcnt][0] = n->x();
 166                         jvo["path"][pcnt][1] = n->y();
 167                         jvo["path"][pcnt][2] = n->h();
 168                         if (n->t(RRTNodeType::cusp))
 169                                 cu++;
 170                         if (n->t(RRTNodeType::connected))
 171                                 co++;
 172                         pcnt++;
 173                 }
 174                 jvo["path#cusp"] = cu;
 175                 jvo["path#connected"] = co;
 176         }
 177
 178         std::cout << jvo << std::endl;
 179         return 0;
 180 }