src/test7.cc

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