src/test7.cc

   1 #include <chrono>
   2 #include <iostream>
   3 #include <jsoncpp/json/json.h>
   4
   5 #include "rrts.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         std::vector<RRTNode> final_path;
  51         double final_path_cost = 9999;
  52         double cost_from_orig_init = 0;
  53         RRTNode init_node;
  54         init_node.x(jvi["init"][0].asDouble());
  55         init_node.y(jvi["init"][1].asDouble());
  56         init_node.h(jvi["init"][2].asDouble());
  57         init_node.sp(2.7);
  58         init_node.st(M_PI / 32); // only for sc2_4
  59         RRTNode last_node(init_node);
  60         init_node.next();
  61
  62         RRTS rrts;
  63         cost_from_orig_init += rrts.cost_build(last_node, init_node);
  64         rrts.nodes().front().x(init_node.x());
  65         rrts.nodes().front().y(init_node.y());
  66         rrts.nodes().front().h(init_node.h());
  67         {
  68                 RRTNode tmp_node;
  69                 tmp_node.x(jvi["goal"][0].asDouble());
  70                 tmp_node.y(jvi["goal"][1].asDouble());
  71                 tmp_node.h(jvi["goal"][2].asDouble());
  72                 rrts.goals().push_back(tmp_node);
  73                 for (auto g: jvi["goals"]) {
  74                         tmp_node.x(g[0].asDouble());
  75                         tmp_node.y(g[1].asDouble());
  76                         tmp_node.h(g[2].asDouble());
  77                         rrts.goals().push_back(tmp_node);
  78                 }
  79         }
  80         {
  81                 Obstacle tmp_obstacle;
  82                 for (auto o: jvi["obst"]) {
  83                         tmp_obstacle.poly().clear();
  84                         for (auto c: o) {
  85                                 double tmp_x = c[0].asDouble();
  86                                 double tmp_y = c[1].asDouble();
  87                                 auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
  88                                 tmp_obstacle.poly().push_back(tmp_tuple);
  89                         }
  90                         rrts.obstacles().push_back(tmp_obstacle);
  91                 }
  92         }
  93         {
  94                 double edist_init_goal = sqrt(
  95                         pow(
  96                                 rrts.nodes().front().x()
  97                                 - rrts.goals().front().x(),
  98                                 2
  99                         )
 100                         + pow(
 101                                 rrts.nodes().front().y()
 102                                 - rrts.goals().front().y(),
 103                                 2
 104                         )
 105                 );
 106                 rrts.set_sample(
 107                         rrts.nodes().front().x(), edist_init_goal,
 108                         rrts.nodes().front().y(), edist_init_goal,
 109                         0, 2 * M_PI
 110                 );
 111         }
 112
 113         TSTART();
 114         while (rrts.next()) {}
 115         TEND();
 116         if (rrts.path().size() > 0) {
 117                 if (
 118                         cc(*rrts.path().back())
 119                         + cost_from_orig_init
 120                         < final_path_cost
 121                 ) {
 122                         final_path_cost = cc(*rrts.path().back());
 123                         final_path.clear();
 124                         for (auto n: rrts.path()) {
 125                                 final_path.push_back(RRTNode());
 126                                 final_path.back().x(n->x());
 127                                 final_path.back().y(n->y());
 128                                 final_path.back().h(n->h());
 129                         }
 130                 }
 131         }
 132         std::cerr << "finished 0, cost is " << final_path_cost << std::endl;
 133         int rcnt = 0;
 134         jvo[rcnt++] = rrts.json();
 135 for (int i = 1; i < 5; i++) {
 136         last_node = RRTNode(init_node);
 137         init_node.next();
 138         RRTS rrts2;
 139         cost_from_orig_init += rrts2.cost_build(last_node, init_node);
 140         rrts2.goals() = rrts.goals();
 141         rrts2.obstacles() = rrts.obstacles();
 142         rrts2.nodes().front().x(init_node.x());
 143         rrts2.nodes().front().y(init_node.y());
 144         rrts2.nodes().front().h(init_node.h());
 145         {
 146                 double edist_init_goal = sqrt(
 147                         pow(
 148                                 rrts2.nodes().front().x()
 149                                 - rrts2.goals().front().x(),
 150                                 2
 151                         )
 152                         + pow(
 153                                 rrts2.nodes().front().y()
 154                                 - rrts2.goals().front().y(),
 155                                 2
 156                         )
 157                 );
 158                 rrts2.set_sample(
 159                         rrts2.nodes().front().x(), edist_init_goal,
 160                         rrts2.nodes().front().y(), edist_init_goal,
 161                         0, 2 * M_PI
 162                 );
 163         }
 164         TSTART();
 165         while (rrts2.next()) {}
 166         TEND();
 167         if (rrts2.path().size() > 0) {
 168                 if (
 169                         cc(*rrts2.path().back()) < final_path_cost
 170                         + cost_from_orig_init
 171                         < final_path_cost
 172                 ) {
 173                         final_path_cost = cc(*rrts2.path().back());
 174                         final_path.clear();
 175                         for (auto n: rrts2.path()) {
 176                                 final_path.push_back(RRTNode());
 177                                 final_path.back().x(n->x());
 178                                 final_path.back().y(n->y());
 179                                 final_path.back().h(n->h());
 180                         }
 181                 }
 182         }
 183         std::cerr << "finished " << i << ", cost is " << final_path_cost;
 184         std::cerr << std::endl;
 185         jvo[rcnt++] = rrts2.json();
 186 }
 187         std::cout << jvo << std::endl;
 188         return 0;
 189 }