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 RRTNode init_node(RRTNode *oi, double *oic)
  26 {
  27         RRTNode r(*oi);
  28         TEND();
  29         r.sp(oi->sp() * TDIFF());
  30         r.next();
  31         if (oi->h() == r.h())
  32                 *oic = sqrt(pow(r.x() - oi->x(), 2) + pow(r.y() - oi->y(), 2));
  33         else
  34                 *oic = std::min(
  35                         std::abs(r.h() - oi->h())
  36                         , 2 * M_PI - std::abs(r.h() - oi->h())
  37                 ) * r.mtr();
  38         return r;
  39 }
  40
  41 #define EPP RRTCE7 // EPP stands for entry point planner.
  42 enum EppStat {
  43         RUNNING
  44         , FINISHED
  45 };
  46 EppStat epp_stat = FINISHED;
  47 void epp_proc(
  48         Json::Value jvi
  49         , std::vector<RRTNode> *fp
  50         , double *fc
  51         , double *oic
  52 )
  53 {
  54         epp_stat = RUNNING;
  55         EPP epp;
  56         epp.json(jvi);
  57         epp.init();
  58
  59         while (epp.next()) {}
  60
  61         if (epp.path().size() > 0) {
  62                 *fc = cc(*epp.path().back()) + *oic;
  63                 fp->clear();
  64                 for (auto n: epp.path()) {
  65                         fp->push_back(RRTNode());
  66                         fp->back().x(n->x());
  67                         fp->back().y(n->y());
  68                         fp->back().h(n->h());
  69                 }
  70         }
  71         epp_stat = FINISHED;
  72 }
  73
  74 int main()
  75 {
  76         Json::Value jvi; // JSON input
  77         std::cin >> jvi;
  78
  79         std::vector<RRTNode> final_path;
  80         double final_path_cost = 9999;
  81         std::vector<RRTNode> feasible_path;
  82         double feasible_path_cost = 9999;
  83         double orig_init_cost = 0;
  84         RRTNode orig_init;
  85         orig_init.x(jvi["init"][0].asDouble());
  86         orig_init.y(jvi["init"][1].asDouble());
  87         orig_init.h(jvi["init"][2].asDouble());
  88         orig_init.sp(2.7);
  89         //orig_init.st(M_PI / 32); // only for sc2_4
  90
  91         std::thread epp_thread(
  92                 epp_proc
  93                 , jvi
  94                 , &final_path
  95                 , &final_path_cost
  96                 , &orig_init_cost
  97         );
  98         epp_thread.join();
  99
 100         Json::Value jvo = jvi; // JSON output
 101         {
 102                 jvo["path"][0][0] = orig_init.x();
 103                 jvo["path"][0][1] = orig_init.y();
 104                 jvo["path"][0][2] = orig_init.h();
 105                 unsigned int cu = 0;
 106                 unsigned int co = 0;
 107                 unsigned int pcnt = 1;
 108                 for (auto n: final_path) {
 109                         jvo["path"][pcnt][0] = n.x();
 110                         jvo["path"][pcnt][1] = n.y();
 111                         jvo["path"][pcnt][2] = n.h();
 112                         if (n.t(RRTNodeType::cusp))
 113                                 cu++;
 114                         if (n.t(RRTNodeType::connected))
 115                                 co++;
 116                         pcnt++;
 117                 }
 118                 jvo["cusps-in-path"] = cu;
 119                 jvo["connecteds-in-path"] = co;
 120         }
 121         std::cout << jvo << std::endl;
 122         return 0;
 123 }