src/test10.cc

   1 #include <chrono>
   2 #include <iostream>
   3 #include <jsoncpp/json/json.h>
   4 #include <thread>
   5
   6 #include "BrainTree.h"
   7 #include "rrtce.h"
   8
   9 std::chrono::high_resolution_clock::time_point TSTART_;
  10 std::chrono::high_resolution_clock::time_point TEND_;
  11 inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
  12 inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
  13 inline double TDIFF()
  14 {
  15         std::chrono::duration<double> DT_;
  16         DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
  17                 TEND_ - TSTART_
  18         );
  19         return DT_.count();
  20 }
  21 inline void TPRINT(const char *what)
  22 {
  23         std::cerr << what << ": " << TDIFF() << std::endl;
  24 }
  25
  26 RRTNode init_node(RRTNode *oi, double *oic)
  27 {
  28         RRTNode r(*oi);
  29         TEND();
  30         r.sp(oi->sp() * TDIFF());
  31         r.next();
  32         if (oi->h() == r.h())
  33                 *oic = sqrt(pow(r.x() - oi->x(), 2) + pow(r.y() - oi->y(), 2));
  34         else
  35                 *oic = std::min(
  36                         std::abs(r.h() - oi->h())
  37                         , 2 * M_PI - std::abs(r.h() - oi->h())
  38                 ) * r.mtr();
  39         return r;
  40 }
  41 bool should_run_bt()
  42 {
  43         TEND();
  44         if (TDIFF() < 5)
  45                 return true;
  46         return false;
  47 }
  48
  49 #define EPP RRTCE7 // EPP stands for entry point planner.
  50 enum EppStat {
  51         RUNNING
  52         , FINISHED
  53 };
  54 EppStat epp_stat = FINISHED;
  55 std::vector<std::thread> epp_threads;
  56 void epp_proc(
  57         Json::Value jvi
  58         , std::vector<RRTNode> *fp
  59         , double *fc
  60         , double *oic
  61 )
  62 {
  63         epp_stat = RUNNING;
  64         EPP epp;
  65         epp.json(jvi);
  66         epp.init();
  67
  68         while (epp.next()) {}
  69
  70         if (epp.path().size() > 0) {
  71                 *fc = cc(*epp.path().back()) + *oic;
  72                 fp->clear();
  73                 for (auto n: epp.path()) {
  74                         fp->push_back(RRTNode());
  75                         fp->back().x(n->x());
  76                         fp->back().y(n->y());
  77                         fp->back().h(n->h());
  78                 }
  79         }
  80         epp_stat = FINISHED;
  81 }
  82
  83 class EppFinished : public BrainTree::Node {
  84         public:
  85                 Status update() override
  86                 {
  87                         if (epp_stat == FINISHED)
  88                                 return Status::Success;
  89                         return Status::Failure;
  90                 }
  91 };
  92 class EppGF : public BrainTree::Node {
  93         private:
  94                 std::vector<RRTNode> *fp;
  95         public:
  96                 EppGF(
  97                         std::vector<RRTNode> *feasible_path
  98                 )
  99                         : fp(feasible_path)
 100                 {
 101                 }
 102                 Status update() override
 103                 {
 104                         if (fp->size() == 0)
 105                                 return Status::Failure;
 106                         unsigned int cu = 0;
 107                         unsigned int co = 0;
 108                         unsigned int pcnt = 0;
 109                         for (auto n: *fp) {
 110                                 if (n.t(RRTNodeType::cusp))
 111                                         cu++;
 112                                 if (n.t(RRTNodeType::connected))
 113                                         co++;
 114                                 pcnt++;
 115                         }
 116                         if (cu > 5) // max cusps in feasible path
 117                                 return Status::Failure;
 118                         return Status::Success;
 119                 }
 120 };
 121 class EppRestart : public BrainTree::Node {
 122         private:
 123                 Json::Value jvi;
 124                 std::vector<RRTNode> *fp;
 125                 double *fpc;
 126                 RRTNode *oi;
 127                 double *oic;
 128         public:
 129                 EppRestart(
 130                         Json::Value jvi
 131                         , std::vector<RRTNode> *feasible_path
 132                         , double *feasible_path_cost
 133                         , RRTNode *orig_init
 134                         , double *orig_init_cost
 135                 )
 136                         : jvi(jvi)
 137                         , fp(feasible_path)
 138                         , fpc(feasible_path_cost)
 139                         , oi(orig_init)
 140                         , oic(orig_init_cost)
 141                 {
 142                 }
 143                 Status update() override
 144                 {
 145                         RRTNode in = init_node(oi, oic);
 146                         jvi["init"][0] = in.x();
 147                         jvi["init"][1] = in.y();
 148                         jvi["init"][2] = in.h();
 149                         epp_threads.push_back(std::thread(
 150                                 epp_proc
 151                                 , jvi
 152                                 , fp
 153                                 , fpc
 154                                 , oic
 155                         ));
 156                         return Status::Success;
 157                 }
 158 };
 159 class ShouldRunBT : public BrainTree::Node {
 160         public:
 161                 Status update() override
 162                 {
 163                         if (should_run_bt())
 164                                 return Status::Success;
 165                         return Status::Failure;
 166                 }
 167 };
 168 class UpdateFP : public BrainTree::Node {
 169         private:
 170                 std::vector<RRTNode> *fi;
 171                 double *fic;
 172                 std::vector<RRTNode> *fe;
 173                 double *fec;
 174         public:
 175                 UpdateFP(
 176                         std::vector<RRTNode> *final_path
 177                         , double *final_path_cost
 178                         , std::vector<RRTNode> *feasible_path
 179                         , double *feasible_path_cost
 180                 )
 181                         : fi(final_path)
 182                         , fic(final_path_cost)
 183                         , fe(feasible_path)
 184                         , fec(feasible_path_cost)
 185                 {
 186                 }
 187                 Status update() override
 188                 {
 189                         if (*fec < *fic) {
 190                                 *fic = *fec;
 191                                 fi->clear();
 192                                 for (auto n: *fe) {
 193                                         fi->push_back(RRTNode());
 194                                         fi->back().x(n.x());
 195                                         fi->back().y(n.y());
 196                                         fi->back().h(n.h());
 197                                 }
 198                                 return Status::Success;
 199                         }
 200                         return Status::Failure;
 201                 }
 202 };
 203
 204 int main()
 205 {
 206         Json::Value jvi; // JSON input
 207         std::cin >> jvi;
 208
 209         std::vector<RRTNode> final_path;
 210         double final_path_cost = 9999;
 211         std::vector<RRTNode> feasible_path;
 212         double feasible_path_cost = 9999;
 213         double orig_init_cost = 0;
 214         RRTNode orig_init;
 215         orig_init.x(jvi["init"][0].asDouble());
 216         orig_init.y(jvi["init"][1].asDouble());
 217         orig_init.h(jvi["init"][2].asDouble());
 218         orig_init.sp(2.7);
 219         //orig_init.st(M_PI / 32); // only for sc2_4
 220
 221         struct timespec sleeping_time;
 222         sleeping_time.tv_sec = 0;
 223         sleeping_time.tv_nsec = 50 * 1000000;
 224
 225         auto bt = BrainTree::Builder()
 226                 .composite<BrainTree::Sequence>()
 227                         .leaf<ShouldRunBT>()
 228                         .composite<BrainTree::Selector>()
 229                                 .composite<BrainTree::Sequence>()
 230                                         .leaf<EppFinished>()
 231                                         .leaf<EppGF>(
 232                                                 &feasible_path
 233                                         )
 234                                         .leaf<UpdateFP>(
 235                                                 &final_path
 236                                                 , &final_path_cost
 237                                                 , &feasible_path
 238                                                 , &feasible_path_cost
 239                                         )
 240                                         .leaf<EppRestart>(
 241                                                 jvi
 242                                                 , &feasible_path
 243                                                 , &feasible_path_cost
 244                                                 , &orig_init
 245                                                 , &orig_init_cost
 246                                         )
 247                                 .end()
 248                                 .composite<BrainTree::Sequence>()
 249                                         .leaf<EppFinished>()
 250                                         .leaf<EppRestart>(
 251                                                 jvi
 252                                                 , &feasible_path
 253                                                 , &feasible_path_cost
 254                                                 , &orig_init
 255                                                 , &orig_init_cost
 256                                         )
 257                                 .end()
 258                                 .decorator<BrainTree::Succeeder>()
 259                                         .leaf<ShouldRunBT>()
 260                                 .end()
 261                         .end()
 262                 .end()
 263         .build();
 264
 265         TSTART();
 266         while (bt->update() != BrainTree::Node::Status::Failure) {
 267                 nanosleep(&sleeping_time, (struct timespec *) NULL);
 268         }
 269         for (auto &t: epp_threads)
 270                 t.join();
 271
 272         Json::Value jvo = jvi; // JSON output
 273         {
 274                 jvo["time"] = TDIFF();
 275                 jvo["cost"] = final_path_cost;
 276                 jvo["path"][0][0] = orig_init.x();
 277                 jvo["path"][0][1] = orig_init.y();
 278                 jvo["path"][0][2] = orig_init.h();
 279                 unsigned int cu = 0;
 280                 unsigned int co = 0;
 281                 unsigned int pcnt = 1;
 282                 for (auto n: final_path) {
 283                         jvo["path"][pcnt][0] = n.x();
 284                         jvo["path"][pcnt][1] = n.y();
 285                         jvo["path"][pcnt][2] = n.h();
 286                         if (n.t(RRTNodeType::cusp))
 287                                 cu++;
 288                         if (n.t(RRTNodeType::connected))
 289                                 co++;
 290                         pcnt++;
 291                 }
 292                 jvo["cusps-in-path"] = cu;
 293                 jvo["connecteds-in-path"] = co;
 294         }
 295         std::cout << jvo << std::endl;
 296         return 0;
 297 }