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         RRTNode init_node;
  53         init_node.x(jvi["init"][0].asDouble());
  54         init_node.y(jvi["init"][1].asDouble());
  55         init_node.h(jvi["init"][2].asDouble());
  56         init_node.sp(2.7);
  57         init_node.st(M_PI / 32); // only for sc2_4
  58         init_node.next();
  59
  60         RRTS rrts;
  61         rrts.nodes().front().x(init_node.x());
  62         rrts.nodes().front().y(init_node.y());
  63         rrts.nodes().front().h(init_node.h());
  64         {
  65                 RRTNode tmp_node;
  66                 tmp_node.x(jvi["goal"][0].asDouble());
  67                 tmp_node.y(jvi["goal"][1].asDouble());
  68                 tmp_node.h(jvi["goal"][2].asDouble());
  69                 rrts.goals().push_back(tmp_node);
  70                 for (auto g: jvi["goals"]) {
  71                         tmp_node.x(g[0].asDouble());
  72                         tmp_node.y(g[1].asDouble());
  73                         tmp_node.h(g[2].asDouble());
  74                         rrts.goals().push_back(tmp_node);
  75                 }
  76         }
  77         {
  78                 Obstacle tmp_obstacle;
  79                 for (auto o: jvi["obst"]) {
  80                         tmp_obstacle.poly().clear();
  81                         for (auto c: o) {
  82                                 double tmp_x = c[0].asDouble();
  83                                 double tmp_y = c[1].asDouble();
  84                                 auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
  85                                 tmp_obstacle.poly().push_back(tmp_tuple);
  86                         }
  87                         rrts.obstacles().push_back(tmp_obstacle);
  88                 }
  89         }
  90         {
  91                 double edist_init_goal = sqrt(
  92                         pow(
  93                                 rrts.nodes().front().x()
  94                                 - rrts.goals().front().x(),
  95                                 2
  96                         )
  97                         + pow(
  98                                 rrts.nodes().front().y()
  99                                 - rrts.goals().front().y(),
 100                                 2
 101                         )
 102                 );
 103                 rrts.set_sample(
 104                         rrts.nodes().front().x(), edist_init_goal,
 105                         rrts.nodes().front().y(), edist_init_goal,
 106                         0, 2 * M_PI
 107                 );
 108         }
 109
 110         TSTART();
 111         while (rrts.next()) {}
 112         TEND();
 113         if (rrts.path().size() > 0) {
 114                 if (cc(*rrts.path().back()) < final_path_cost) {
 115                         final_path_cost = cc(*rrts.path().back());
 116                         final_path.clear();
 117                         for (auto n: rrts.path()) {
 118                                 final_path.push_back(RRTNode());
 119                                 final_path.back().x(n->x());
 120                                 final_path.back().y(n->y());
 121                                 final_path.back().h(n->h());
 122                         }
 123                 }
 124         }
 125 {
 126         init_node.next();
 127         RRTS rrts2;
 128         rrts2.goals() = rrts.goals();
 129         rrts2.obstacles() = rrts.obstacles();
 130         rrts2.nodes().front().x(init_node.x());
 131         rrts2.nodes().front().y(init_node.y());
 132         rrts2.nodes().front().h(init_node.h());
 133         {
 134                 double edist_init_goal = sqrt(
 135                         pow(
 136                                 rrts2.nodes().front().x()
 137                                 - rrts2.goals().front().x(),
 138                                 2
 139                         )
 140                         + pow(
 141                                 rrts2.nodes().front().y()
 142                                 - rrts2.goals().front().y(),
 143                                 2
 144                         )
 145                 );
 146                 rrts2.set_sample(
 147                         rrts2.nodes().front().x(), edist_init_goal,
 148                         rrts2.nodes().front().y(), edist_init_goal,
 149                         0, 2 * M_PI
 150                 );
 151         }
 152         TSTART();
 153         while (rrts2.next()) {}
 154         TEND();
 155         if (rrts2.path().size() > 0) {
 156                 if (cc(*rrts2.path().back()) < final_path_cost) {
 157                         final_path_cost = cc(*rrts2.path().back());
 158                         final_path.clear();
 159                         for (auto n: rrts2.path()) {
 160                                 final_path.push_back(RRTNode());
 161                                 final_path.back().x(n->x());
 162                                 final_path.back().y(n->y());
 163                                 final_path.back().h(n->h());
 164                         }
 165                 }
 166         }
 167 }
 168
 169         {
 170                 jvo["time"] = TDIFF();
 171         }
 172         {
 173                 jvo["iterations"] = rrts.icnt();
 174         }
 175         {
 176                 jvo["init"][0] = rrts.nodes().front().x();
 177                 jvo["init"][1] = rrts.nodes().front().y();
 178                 jvo["init"][2] = rrts.nodes().front().h();
 179         }
 180         {
 181                 jvo["cost"] = final_path_cost;
 182         }
 183         {
 184                 if (final_path.size() > 0) {
 185                         jvo["goal"][0] = final_path.back().x();
 186                         jvo["goal"][1] = final_path.back().y();
 187                         jvo["goal"][2] = final_path.back().h();
 188                 }
 189                 unsigned int cu = 0;
 190                 unsigned int co = 0;
 191                 unsigned int pcnt = 0;
 192                 for (auto n: final_path) {
 193                         jvo["path"][pcnt][0] = n.x();
 194                         jvo["path"][pcnt][1] = n.y();
 195                         jvo["path"][pcnt][2] = n.h();
 196                         if (n.t(RRTNodeType::cusp))
 197                                 cu++;
 198                         if (n.t(RRTNodeType::connected))
 199                                 co++;
 200                         pcnt++;
 201                 }
 202                 jvo["cusps-in-path"] = cu;
 203                 jvo["connecteds-in-path"] = co;
 204         }
 205         {
 206                 unsigned int gcnt = 0;
 207                 for (auto g: rrts.goals()) {
 208                         jvo["goals"][gcnt][0] = g.x();
 209                         jvo["goals"][gcnt][1] = g.y();
 210                         jvo["goals"][gcnt][2] = g.h();
 211                         gcnt++;
 212                 }
 213         }
 214         {
 215                 unsigned int ocnt = 0;
 216                 for (auto o: rrts.obstacles()) {
 217                         unsigned int ccnt = 0;
 218                         for (auto c: o.poly()) {
 219                                 jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
 220                                 jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
 221                                 ccnt++;
 222                         }
 223                         ocnt++;
 224                 }
 225         }
 226         {
 227                 jvo["nodes"] = (unsigned int) rrts.nodes().size();
 228         }
 229         //{
 230         //        unsigned int ncnt = 0;
 231         //        for (auto n: rrts.nodes()) {
 232         //                jvo["nodes_x"][ncnt] = n.x();
 233         //                jvo["nodes_y"][ncnt] = n.y();
 234         //                //jvo["nodes_h"][ncnt] = n.h();
 235         //                ncnt++;
 236         //        }
 237         //}
 238         std::cout << jvo << std::endl;
 239         return 0;
 240 }