src/test10.cc

   1 #include <chrono>
   2 #include <iostream>
   3 #include <jsoncpp/json/json.h>
   4 #include <thread>
   5
   6 #include "rrtce.h"
   7
   8 std::chrono::high_resolution_clock::time_point TSTART_;
   9 std::chrono::high_resolution_clock::time_point TEND_;
  10 inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
  11 inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
  12 inline double TDIFF()
  13 {
  14         std::chrono::duration<double> DT_;
  15         DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
  16                 TEND_ - TSTART_
  17         );
  18         return DT_.count();
  19 }
  20 inline void TPRINT(const char *what)
  21 {
  22         std::cerr << what << ": " << TDIFF() << std::endl;
  23 }
  24
  25 #define EPP RRTCE7 // EPP stands for entry point planner.
  26 enum EppStat {
  27         RUNNING
  28         , FINISHED
  29 };
  30 EppStat epp_stat = FINISHED;
  31 void epp_proc(
  32         Json::Value jvi
  33         , std::vector<RRTNode> *fp
  34         , double *fc
  35         , double *oic
  36 )
  37 {
  38         epp_stat = RUNNING;
  39         EPP epp;
  40         epp.json(jvi);
  41         epp.init();
  42
  43         while (epp.next()) {}
  44
  45         if (epp.path().size() > 0) {
  46                 *fc = cc(*epp.path().back()) + *oic;
  47                 fp->clear();
  48                 for (auto n: epp.path()) {
  49                         fp->push_back(RRTNode());
  50                         fp->back().x(n->x());
  51                         fp->back().y(n->y());
  52                         fp->back().h(n->h());
  53                 }
  54         }
  55         epp_stat = FINISHED;
  56 }
  57
  58 int main()
  59 {
  60         Json::Value jvi; // JSON input
  61         std::cin >> jvi;
  62
  63         std::vector<RRTNode> final_path;
  64         double final_path_cost = 9999;
  65         std::vector<RRTNode> feasible_path;
  66         double feasible_path_cost = 9999;
  67         double orig_init_cost = 0;
  68         RRTNode orig_init;
  69         orig_init.x(jvi["init"][0].asDouble());
  70         orig_init.y(jvi["init"][1].asDouble());
  71         orig_init.h(jvi["init"][2].asDouble());
  72         orig_init.sp(2.7);
  73         //orig_init.st(M_PI / 32); // only for sc2_4
  74
  75         std::thread epp_thread(
  76                 epp_proc
  77                 , jvi
  78                 , &final_path
  79                 , &final_path_cost
  80                 , &orig_init_cost
  81         );
  82         epp_thread.join();
  83
  84         Json::Value jvo = jvi; // JSON output
  85         {
  86                 jvo["path"][0][0] = orig_init.x();
  87                 jvo["path"][0][1] = orig_init.y();
  88                 jvo["path"][0][2] = orig_init.h();
  89                 unsigned int cu = 0;
  90                 unsigned int co = 0;
  91                 unsigned int pcnt = 1;
  92                 for (auto n: final_path) {
  93                         jvo["path"][pcnt][0] = n.x();
  94                         jvo["path"][pcnt][1] = n.y();
  95                         jvo["path"][pcnt][2] = n.h();
  96                         if (n.t(RRTNodeType::cusp))
  97                                 cu++;
  98                         if (n.t(RRTNodeType::connected))
  99                                 co++;
 100                         pcnt++;
 101                 }
 102                 jvo["cusps-in-path"] = cu;
 103                 jvo["connecteds-in-path"] = co;
 104         }
 105         std::cout << jvo << std::endl;
 106         return 0;
 107 }