From: Jiri Vlasak Date: Tue, 14 Mar 2023 16:58:43 +0000 (+0100) Subject: Remove old and unused test* code X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/commitdiff_plain/9944ee3d06a11045f0428dec7232dc96b1360e88 Remove old and unused test* code --- diff --git a/src/test-template.cc b/src/test-template.cc deleted file mode 100644 index 693383b..0000000 --- a/src/test-template.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#ifndef EPP - #define EPP RRTS -#endif - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test1.cc b/src/test1.cc deleted file mode 100644 index aa713ee..0000000 --- a/src/test1.cc +++ /dev/null @@ -1,120 +0,0 @@ -#include -#include -#include - -#include "psp.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvo; // JSON output - - double offset = -1.256994; - double delta = 0.1; - std::cerr << "#dist,fe cusps,fe time,fer cusps,fer time"; - std::cerr << std::endl; - for (int i = 0; i < 14; i++) { - jvo["goals"] = Json::nullValue; - // setup - PSPlanner psp; - double ppsl = 0.01 + psp.cc().perfect_parking_slot_len(); - double psl = ppsl + offset + i * delta; - psp.ps().border( - 0, 0, - 2.2, 0, - 2.2, 0 + psl, - 0, 0 + psl - ); - // run fe planner - TSTART(); - psp.fe(); // or fer() - TEND(); - std::cerr << (psl - psp.cc().l() - 0.01); - std::cerr << ","; - std::cerr << psp.c(); - std::cerr << ","; - std::cerr << TDIFF(); - // json output - jvo["time"] = TDIFF(); - jvo["goal"][0] = psp.gc().x(); - jvo["goal"][1] = psp.gc().y(); - jvo["goal"][2] = psp.gc().h(); - unsigned int icnt = 0; - for (auto &i: psp.cusps()) { - jvo["goals"][icnt][0] = i.x(); - jvo["goals"][icnt][1] = i.y(); - jvo["goals"][icnt][2] = i.h(); - icnt++; - } - jvo["goals"][icnt][0] = psp.cc().x(); - jvo["goals"][icnt][1] = psp.cc().y(); - jvo["goals"][icnt][2] = psp.cc().h(); - jvo["slot"][0][0][0] = psp.ps().x1(); - jvo["slot"][0][0][1] = psp.ps().y1(); - jvo["slot"][0][1][0] = psp.ps().x2(); - jvo["slot"][0][1][1] = psp.ps().y2(); - jvo["slot"][0][2][0] = psp.ps().x3(); - jvo["slot"][0][2][1] = psp.ps().y3(); - jvo["slot"][0][3][0] = psp.ps().x4(); - jvo["slot"][0][3][1] = psp.ps().y4(); - jvo["cusp"] = psp.c(); - std::cout << jvo << std::endl; - psp.ps().border( - 0, 0, - 2.2, 0, - 2.2, 0 + psl, - 0, 0 + psl - ); - // run fer planner - TSTART(); - psp.fer(); // or fer() - TEND(); - std::cerr << ","; - std::cerr << psp.c(); - std::cerr << ","; - std::cerr << TDIFF(); - std::cerr << std::endl; - // json output - jvo["time"] = TDIFF(); - jvo["goal"][0] = psp.gc().x(); - jvo["goal"][1] = psp.gc().y(); - jvo["goal"][2] = psp.gc().h(); - icnt = 0; - for (auto &i: psp.cusps()) { - jvo["goals"][icnt][0] = i.x(); - jvo["goals"][icnt][1] = i.y(); - jvo["goals"][icnt][2] = i.h(); - icnt++; - } - jvo["goals"][icnt][0] = psp.cc().x(); - jvo["goals"][icnt][1] = psp.cc().y(); - jvo["goals"][icnt][2] = psp.cc().h(); - jvo["slot"][0][0][0] = psp.ps().x1(); - jvo["slot"][0][0][1] = psp.ps().y1(); - jvo["slot"][0][1][0] = psp.ps().x2(); - jvo["slot"][0][1][1] = psp.ps().y2(); - jvo["slot"][0][2][0] = psp.ps().x3(); - jvo["slot"][0][2][1] = psp.ps().y3(); - jvo["slot"][0][3][0] = psp.ps().x4(); - jvo["slot"][0][3][1] = psp.ps().y4(); - jvo["cusp"] = psp.c(); - std::cout << jvo << std::endl; - } - return 0; -} diff --git a/src/test10.cc b/src/test10.cc deleted file mode 100644 index bd2af5a..0000000 --- a/src/test10.cc +++ /dev/null @@ -1,297 +0,0 @@ -#include -#include -#include -#include - -#include "BrainTree.h" -#include "rrtce.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -RRTNode init_node(RRTNode *oi, double *oic) -{ - RRTNode r(*oi); - TEND(); - r.sp(oi->sp() * TDIFF()); - r.next(); - if (oi->h() == r.h()) - *oic = sqrt(pow(r.x() - oi->x(), 2) + pow(r.y() - oi->y(), 2)); - else - *oic = std::min( - std::abs(r.h() - oi->h()) - , 2 * M_PI - std::abs(r.h() - oi->h()) - ) * r.mtr(); - return r; -} -bool should_run_bt() -{ - TEND(); - if (TDIFF() < 5) - return true; - return false; -} - -#define EPP RRTCE7 // EPP stands for entry point planner. -enum EppStat { - RUNNING - , FINISHED -}; -EppStat epp_stat = FINISHED; -std::vector epp_threads; -void epp_proc( - Json::Value jvi - , std::vector *fp - , double *fc - , double *oic -) -{ - epp_stat = RUNNING; - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - if (epp.path().size() > 0) { - *fc = cc(*epp.path().back()) + *oic; - fp->clear(); - for (auto n: epp.path()) { - fp->push_back(RRTNode()); - fp->back().x(n->x()); - fp->back().y(n->y()); - fp->back().h(n->h()); - } - } - epp_stat = FINISHED; -} - -class EppFinished : public BrainTree::Node { - public: - Status update() override - { - if (epp_stat == FINISHED) - return Status::Success; - return Status::Failure; - } -}; -class EppGF : public BrainTree::Node { - private: - std::vector *fp; - public: - EppGF( - std::vector *feasible_path - ) - : fp(feasible_path) - { - } - Status update() override - { - if (fp->size() == 0) - return Status::Failure; - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: *fp) { - if (n.t(RRTNodeType::cusp)) - cu++; - if (n.t(RRTNodeType::connected)) - co++; - pcnt++; - } - if (cu > 5) // max cusps in feasible path - return Status::Failure; - return Status::Success; - } -}; -class EppRestart : public BrainTree::Node { - private: - Json::Value jvi; - std::vector *fp; - double *fpc; - RRTNode *oi; - double *oic; - public: - EppRestart( - Json::Value jvi - , std::vector *feasible_path - , double *feasible_path_cost - , RRTNode *orig_init - , double *orig_init_cost - ) - : jvi(jvi) - , fp(feasible_path) - , fpc(feasible_path_cost) - , oi(orig_init) - , oic(orig_init_cost) - { - } - Status update() override - { - RRTNode in = init_node(oi, oic); - jvi["init"][0] = in.x(); - jvi["init"][1] = in.y(); - jvi["init"][2] = in.h(); - epp_threads.push_back(std::thread( - epp_proc - , jvi - , fp - , fpc - , oic - )); - return Status::Success; - } -}; -class ShouldRunBT : public BrainTree::Node { - public: - Status update() override - { - if (should_run_bt()) - return Status::Success; - return Status::Failure; - } -}; -class UpdateFP : public BrainTree::Node { - private: - std::vector *fi; - double *fic; - std::vector *fe; - double *fec; - public: - UpdateFP( - std::vector *final_path - , double *final_path_cost - , std::vector *feasible_path - , double *feasible_path_cost - ) - : fi(final_path) - , fic(final_path_cost) - , fe(feasible_path) - , fec(feasible_path_cost) - { - } - Status update() override - { - if (*fec < *fic) { - *fic = *fec; - fi->clear(); - for (auto n: *fe) { - fi->push_back(RRTNode()); - fi->back().x(n.x()); - fi->back().y(n.y()); - fi->back().h(n.h()); - } - return Status::Success; - } - return Status::Failure; - } -}; - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - std::vector final_path; - double final_path_cost = 9999; - std::vector feasible_path; - double feasible_path_cost = 9999; - double orig_init_cost = 0; - RRTNode orig_init; - orig_init.x(jvi["init"][0].asDouble()); - orig_init.y(jvi["init"][1].asDouble()); - orig_init.h(jvi["init"][2].asDouble()); - orig_init.sp(2.7); - //orig_init.st(M_PI / 32); // only for sc2_4 - - struct timespec sleeping_time; - sleeping_time.tv_sec = 0; - sleeping_time.tv_nsec = 50 * 1000000; - - auto bt = BrainTree::Builder() - .composite() - .leaf() - .composite() - .composite() - .leaf() - .leaf( - &feasible_path - ) - .leaf( - &final_path - , &final_path_cost - , &feasible_path - , &feasible_path_cost - ) - .leaf( - jvi - , &feasible_path - , &feasible_path_cost - , &orig_init - , &orig_init_cost - ) - .end() - .composite() - .leaf() - .leaf( - jvi - , &feasible_path - , &feasible_path_cost - , &orig_init - , &orig_init_cost - ) - .end() - .decorator() - .leaf() - .end() - .end() - .end() - .build(); - - TSTART(); - while (bt->update() != BrainTree::Node::Status::Failure) { - nanosleep(&sleeping_time, (struct timespec *) NULL); - } - for (auto &t: epp_threads) - t.join(); - - Json::Value jvo = jvi; // JSON output - { - jvo["time"] = TDIFF(); - jvo["cost"] = final_path_cost; - jvo["path"][0][0] = orig_init.x(); - jvo["path"][0][1] = orig_init.y(); - jvo["path"][0][2] = orig_init.h(); - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 1; - for (auto n: final_path) { - jvo["path"][pcnt][0] = n.x(); - jvo["path"][pcnt][1] = n.y(); - jvo["path"][pcnt][2] = n.h(); - if (n.t(RRTNodeType::cusp)) - cu++; - if (n.t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["cusps-in-path"] = cu; - jvo["connecteds-in-path"] = co; - } - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test11.cc b/src/test11.cc deleted file mode 100644 index 539d2cd..0000000 --- a/src/test11.cc +++ /dev/null @@ -1,38 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE4 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - return 0; -} diff --git a/src/test12.cc b/src/test12.cc deleted file mode 100644 index f8141f3..0000000 --- a/src/test12.cc +++ /dev/null @@ -1,38 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE6 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - return 0; -} diff --git a/src/test13.cc b/src/test13.cc deleted file mode 100644 index ba6307e..0000000 --- a/src/test13.cc +++ /dev/null @@ -1,38 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE8 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - return 0; -} diff --git a/src/test14.cc b/src/test14.cc deleted file mode 100644 index 7076a46..0000000 --- a/src/test14.cc +++ /dev/null @@ -1,38 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE9 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - return 0; -} diff --git a/src/test15.cc b/src/test15.cc deleted file mode 100644 index f1bea6d..0000000 --- a/src/test15.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE10 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - epp.deinit(); - return 0; -} diff --git a/src/test16.cc b/src/test16.cc deleted file mode 100644 index 0620110..0000000 --- a/src/test16.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE11 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - epp.deinit(); - return 0; -} diff --git a/src/test17.cc b/src/test17.cc deleted file mode 100644 index c1c8a7c..0000000 --- a/src/test17.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE12 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - epp.deinit(); - return 0; -} diff --git a/src/test18.cc b/src/test18.cc deleted file mode 100644 index c5aab58..0000000 --- a/src/test18.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE13 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - epp.deinit(); - return 0; -} diff --git a/src/test19.cc b/src/test19.cc deleted file mode 100644 index 309ae9f..0000000 --- a/src/test19.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE14 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - epp.deinit(); - return 0; -} diff --git a/src/test2.cc b/src/test2.cc deleted file mode 100644 index 6f4f379..0000000 --- a/src/test2.cc +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include -#include - -#include "rrts.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - if (jvi["init"] == Json::nullValue) { - std::cerr << "I need `init` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goal"] == Json::nullValue) { - std::cerr << "I need `goal` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goals"] == Json::nullValue) { - std::cerr << "I need `goals` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["obst"] == Json::nullValue) { - std::cerr << "I need `obst` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - RRTS rrts; - rrts.nodes().front().x(jvi["init"][0].asDouble()); - rrts.nodes().front().y(jvi["init"][1].asDouble()); - rrts.nodes().front().h(jvi["init"][2].asDouble()); - { - RRTNode tmp_node; - tmp_node.x(jvi["goal"][0].asDouble()); - tmp_node.y(jvi["goal"][1].asDouble()); - tmp_node.h(jvi["goal"][2].asDouble()); - rrts.goals().push_back(tmp_node); - for (auto g: jvi["goals"]) { - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - rrts.goals().push_back(tmp_node); - } - } - { - Obstacle tmp_obstacle; - for (auto o: jvi["obst"]) { - tmp_obstacle.poly().clear(); - for (auto c: o) { - double tmp_x = c[0].asDouble(); - double tmp_y = c[1].asDouble(); - auto tmp_tuple = std::make_tuple(tmp_x, tmp_y); - tmp_obstacle.poly().push_back(tmp_tuple); - } - rrts.obstacles().push_back(tmp_obstacle); - } - } - - { - double edist_init_goal = sqrt( - pow( - rrts.nodes().front().x() - - rrts.goals().front().x(), - 2 - ) - + pow( - rrts.nodes().front().y() - - rrts.goals().front().y(), - 2 - ) - ); - rrts.set_sample( - rrts.nodes().front().x(), edist_init_goal, - rrts.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - - TSTART(); - while (rrts.next()) {} - TEND(); - - { - jvo["time"] = TDIFF(); - } - { - jvo["iterations"] = rrts.icnt(); - } - { - if (rrts.path().size() > 0) { - jvo["cost"] = cc(*rrts.path().back()); - } else { - jvo["cost"] = -1; - } - } - { - jvo["init"][0] = rrts.nodes().front().x(); - jvo["init"][1] = rrts.nodes().front().y(); - jvo["init"][2] = rrts.nodes().front().h(); - } - { - unsigned int ocnt = 0; - for (auto o: rrts.obstacles()) { - unsigned int ccnt = 0; - for (auto c: o.poly()) { - jvo["obst"][ocnt][ccnt][0] = std::get<0>(c); - jvo["obst"][ocnt][ccnt][1] = std::get<1>(c); - ccnt++; - } - ocnt++; - } - } - { - unsigned int gcnt = 0; - for (auto g: rrts.goals()) { - jvo["goals"][gcnt][0] = g.x(); - jvo["goals"][gcnt][1] = g.y(); - jvo["goals"][gcnt][2] = g.h(); - gcnt++; - } - } - { - if (rrts.path().size() > 0) { - jvo["goal"][0] = rrts.path().back()->x(); - jvo["goal"][1] = rrts.path().back()->y(); - jvo["goal"][2] = rrts.path().back()->h(); - } - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: rrts.path()) { - jvo["path"][pcnt][0] = n->x(); - jvo["path"][pcnt][1] = n->y(); - jvo["path"][pcnt][2] = n->h(); - if (n->t(RRTNodeType::cusp)) - cu++; - if (n->t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["cusps-in-path"] = cu; - jvo["connecteds-in-path"] = co; - } - { - jvo["nodes"] = (unsigned int) rrts.nodes().size(); - } - - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test20.cc b/src/test20.cc deleted file mode 100644 index 0078df0..0000000 --- a/src/test20.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE15 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - epp.deinit(); - return 0; -} diff --git a/src/test21.cc b/src/test21.cc deleted file mode 100644 index 4abe957..0000000 --- a/src/test21.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" -#define EPP RRTCE16 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - std::cin >> jvi; - - EPP epp; - epp.json(jvi); - epp.init(); - - while (epp.next()) {} - - std::cout << epp.json() << std::endl; - epp.deinit(); - return 0; -} diff --git a/src/test22.cc b/src/test22.cc deleted file mode 100644 index edd985f..0000000 --- a/src/test22.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE13 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test23.cc b/src/test23.cc deleted file mode 100644 index 1ab9f59..0000000 --- a/src/test23.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE14 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test24.cc b/src/test24.cc deleted file mode 100644 index 1283099..0000000 --- a/src/test24.cc +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE15 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test25.cc b/src/test25.cc deleted file mode 100644 index 2f19c0a..0000000 --- a/src/test25.cc +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE16 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test26.cc b/src/test26.cc deleted file mode 100644 index 477e03e..0000000 --- a/src/test26.cc +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE11 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test27.cc b/src/test27.cc deleted file mode 100644 index e5ba370..0000000 --- a/src/test27.cc +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE7 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test28.cc b/src/test28.cc deleted file mode 100644 index 7ecddf1..0000000 --- a/src/test28.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE3 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test29.cc b/src/test29.cc deleted file mode 100644 index 2560a95..0000000 --- a/src/test29.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE2 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test3.cc b/src/test3.cc deleted file mode 100644 index dca5ff6..0000000 --- a/src/test3.cc +++ /dev/null @@ -1,186 +0,0 @@ -#include -#include -#include - -#include "rrtext.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - if (jvi["init"] == Json::nullValue) { - std::cerr << "I need `init` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goal"] == Json::nullValue) { - std::cerr << "I need `goal` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goals"] == Json::nullValue) { - std::cerr << "I need `goals` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["obst"] == Json::nullValue) { - std::cerr << "I need `obst` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - RRTExt2 rrts; - rrts.nodes().front().x(jvi["init"][0].asDouble()); - rrts.nodes().front().y(jvi["init"][1].asDouble()); - rrts.nodes().front().h(jvi["init"][2].asDouble()); - { - RRTNode tmp_node; - tmp_node.x(jvi["goal"][0].asDouble()); - tmp_node.y(jvi["goal"][1].asDouble()); - tmp_node.h(jvi["goal"][2].asDouble()); - rrts.goals().push_back(tmp_node); - for (auto g: jvi["goals"]) { - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - rrts.goals().push_back(tmp_node); - } - } - { - Obstacle tmp_obstacle; - for (auto o: jvi["obst"]) { - tmp_obstacle.poly().clear(); - for (auto c: o) { - double tmp_x = c[0].asDouble(); - double tmp_y = c[1].asDouble(); - auto tmp_tuple = std::make_tuple(tmp_x, tmp_y); - tmp_obstacle.poly().push_back(tmp_tuple); - } - rrts.obstacles().push_back(tmp_obstacle); - } - } - - { - double edist_init_goal = sqrt( - pow( - rrts.nodes().front().x() - - rrts.goals().front().x(), - 2 - ) - + pow( - rrts.nodes().front().y() - - rrts.goals().front().y(), - 2 - ) - ); - rrts.set_sample( - rrts.nodes().front().x(), edist_init_goal, - rrts.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - - rrts.init(); - TSTART(); - while (rrts.next()) {} - TEND(); - - { - jvo["time"] = TDIFF(); - } - { - jvo["iterations"] = rrts.icnt(); - } - { - if (rrts.path().size() > 0) { - jvo["cost"] = cc(*rrts.path().back()); - } else { - jvo["cost"] = -1; - } - } - { - jvo["init"][0] = rrts.nodes().front().x(); - jvo["init"][1] = rrts.nodes().front().y(); - jvo["init"][2] = rrts.nodes().front().h(); - } - { - unsigned int ocnt = 0; - for (auto o: rrts.obstacles()) { - unsigned int ccnt = 0; - for (auto c: o.poly()) { - jvo["obst"][ocnt][ccnt][0] = std::get<0>(c); - jvo["obst"][ocnt][ccnt][1] = std::get<1>(c); - ccnt++; - } - ocnt++; - } - } - //{ - // unsigned int ncnt = 0; - // for (auto n: rrts.nodes()) { - // jvo["nodes_x"][ncnt] = n.x(); - // jvo["nodes_y"][ncnt] = n.y(); - // //jvo["nodes_h"][ncnt] = n.h(); - // ncnt++; - // } - //} - { - jvo["nodes"] = (unsigned int) rrts.nodes().size(); - } - { - unsigned int gcnt = 0; - for (auto g: rrts.goals()) { - jvo["goals"][gcnt][0] = g.x(); - jvo["goals"][gcnt][1] = g.y(); - jvo["goals"][gcnt][2] = g.h(); - gcnt++; - } - } - { - if (rrts.path().size() > 0) { - jvo["goal"][0] = rrts.path().back()->x(); - jvo["goal"][1] = rrts.path().back()->y(); - jvo["goal"][2] = rrts.path().back()->h(); - } - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: rrts.path()) { - jvo["path"][pcnt][0] = n->x(); - jvo["path"][pcnt][1] = n->y(); - jvo["path"][pcnt][2] = n->h(); - if (n->t(RRTNodeType::cusp)) - cu++; - if (n->t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["cusps-in-path"] = cu; - jvo["connecteds-in-path"] = co; - } - - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test30.cc b/src/test30.cc deleted file mode 100644 index 59c3cf4..0000000 --- a/src/test30.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE1 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test31.cc b/src/test31.cc deleted file mode 100644 index 8b9ae73..0000000 --- a/src/test31.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include - -#include "psp.h" -#include "rrtce.h" -#define EPP RRTCE17 - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -PSPlanner psp; -EPP epp; -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - - psp.fe(); - - jvi["goal"][0] = psp.cc().x(); - jvi["goal"][1] = psp.cc().y(); - jvi["goal"][2] = psp.cc().h(); - unsigned int icnt = 0; - for (auto &i: psp.possible_goals()) { - jvi["goals"][icnt][0] = i.x(); - jvi["goals"][icnt][1] = i.y(); - jvi["goals"][icnt][2] = i.h(); - icnt++; - } - jvi["goals"][icnt][0] = psp.cc().x(); - jvi["goals"][icnt][1] = psp.cc().y(); - jvi["goals"][icnt][2] = psp.cc().h(); - - epp.json(jvi); - epp.init(); - epp.sample_dist_type(1); - epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI); - - while (epp.next()) {} - - jvo = epp.json(); - jvo["slot"] = jvi["slot"]; - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test4.cc b/src/test4.cc deleted file mode 100644 index 732a5b5..0000000 --- a/src/test4.cc +++ /dev/null @@ -1,186 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - if (jvi["init"] == Json::nullValue) { - std::cerr << "I need `init` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goal"] == Json::nullValue) { - std::cerr << "I need `goal` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goals"] == Json::nullValue) { - std::cerr << "I need `goals` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["obst"] == Json::nullValue) { - std::cerr << "I need `obst` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - RRTCE1 rrts; - rrts.nodes().front().x(jvi["init"][0].asDouble()); - rrts.nodes().front().y(jvi["init"][1].asDouble()); - rrts.nodes().front().h(jvi["init"][2].asDouble()); - { - RRTNode tmp_node; - tmp_node.x(jvi["goal"][0].asDouble()); - tmp_node.y(jvi["goal"][1].asDouble()); - tmp_node.h(jvi["goal"][2].asDouble()); - rrts.goals().push_back(tmp_node); - for (auto g: jvi["goals"]) { - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - rrts.goals().push_back(tmp_node); - } - } - { - Obstacle tmp_obstacle; - for (auto o: jvi["obst"]) { - tmp_obstacle.poly().clear(); - for (auto c: o) { - double tmp_x = c[0].asDouble(); - double tmp_y = c[1].asDouble(); - auto tmp_tuple = std::make_tuple(tmp_x, tmp_y); - tmp_obstacle.poly().push_back(tmp_tuple); - } - rrts.obstacles().push_back(tmp_obstacle); - } - } - - { - double edist_init_goal = sqrt( - pow( - rrts.nodes().front().x() - - rrts.goals().front().x(), - 2 - ) - + pow( - rrts.nodes().front().y() - - rrts.goals().front().y(), - 2 - ) - ); - rrts.set_sample( - rrts.nodes().front().x(), edist_init_goal, - rrts.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - - rrts.init(); - TSTART(); - while (rrts.next()) {} - TEND(); - - { - jvo["time"] = TDIFF(); - } - { - jvo["iterations"] = rrts.icnt(); - } - { - if (rrts.path().size() > 0) { - jvo["cost"] = cc(*rrts.path().back()); - } else { - jvo["cost"] = -1; - } - } - { - jvo["init"][0] = rrts.nodes().front().x(); - jvo["init"][1] = rrts.nodes().front().y(); - jvo["init"][2] = rrts.nodes().front().h(); - } - { - unsigned int ocnt = 0; - for (auto o: rrts.obstacles()) { - unsigned int ccnt = 0; - for (auto c: o.poly()) { - jvo["obst"][ocnt][ccnt][0] = std::get<0>(c); - jvo["obst"][ocnt][ccnt][1] = std::get<1>(c); - ccnt++; - } - ocnt++; - } - } - //{ - // unsigned int ncnt = 0; - // for (auto n: rrts.nodes()) { - // jvo["nodes_x"][ncnt] = n.x(); - // jvo["nodes_y"][ncnt] = n.y(); - // //jvo["nodes_h"][ncnt] = n.h(); - // ncnt++; - // } - //} - { - jvo["nodes"] = (unsigned int) rrts.nodes().size(); - } - { - unsigned int gcnt = 0; - for (auto g: rrts.goals()) { - jvo["goals"][gcnt][0] = g.x(); - jvo["goals"][gcnt][1] = g.y(); - jvo["goals"][gcnt][2] = g.h(); - gcnt++; - } - } - { - if (rrts.path().size() > 0) { - jvo["goal"][0] = rrts.path().back()->x(); - jvo["goal"][1] = rrts.path().back()->y(); - jvo["goal"][2] = rrts.path().back()->h(); - } - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: rrts.path()) { - jvo["path"][pcnt][0] = n->x(); - jvo["path"][pcnt][1] = n->y(); - jvo["path"][pcnt][2] = n->h(); - if (n->t(RRTNodeType::cusp)) - cu++; - if (n->t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["cusps-in-path"] = cu; - jvo["connecteds-in-path"] = co; - } - - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test5.cc b/src/test5.cc deleted file mode 100644 index 553d07f..0000000 --- a/src/test5.cc +++ /dev/null @@ -1,186 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - if (jvi["init"] == Json::nullValue) { - std::cerr << "I need `init` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goal"] == Json::nullValue) { - std::cerr << "I need `goal` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goals"] == Json::nullValue) { - std::cerr << "I need `goals` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["obst"] == Json::nullValue) { - std::cerr << "I need `obst` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - RRTCE2 rrts; - rrts.nodes().front().x(jvi["init"][0].asDouble()); - rrts.nodes().front().y(jvi["init"][1].asDouble()); - rrts.nodes().front().h(jvi["init"][2].asDouble()); - { - RRTNode tmp_node; - tmp_node.x(jvi["goal"][0].asDouble()); - tmp_node.y(jvi["goal"][1].asDouble()); - tmp_node.h(jvi["goal"][2].asDouble()); - rrts.goals().push_back(tmp_node); - for (auto g: jvi["goals"]) { - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - rrts.goals().push_back(tmp_node); - } - } - { - Obstacle tmp_obstacle; - for (auto o: jvi["obst"]) { - tmp_obstacle.poly().clear(); - for (auto c: o) { - double tmp_x = c[0].asDouble(); - double tmp_y = c[1].asDouble(); - auto tmp_tuple = std::make_tuple(tmp_x, tmp_y); - tmp_obstacle.poly().push_back(tmp_tuple); - } - rrts.obstacles().push_back(tmp_obstacle); - } - } - - { - double edist_init_goal = sqrt( - pow( - rrts.nodes().front().x() - - rrts.goals().front().x(), - 2 - ) - + pow( - rrts.nodes().front().y() - - rrts.goals().front().y(), - 2 - ) - ); - rrts.set_sample( - rrts.nodes().front().x(), edist_init_goal, - rrts.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - - rrts.init(); - TSTART(); - while (rrts.next()) {} - TEND(); - - { - jvo["time"] = TDIFF(); - } - { - jvo["iterations"] = rrts.icnt(); - } - { - if (rrts.path().size() > 0) { - jvo["cost"] = cc(*rrts.path().back()); - } else { - jvo["cost"] = -1; - } - } - { - jvo["init"][0] = rrts.nodes().front().x(); - jvo["init"][1] = rrts.nodes().front().y(); - jvo["init"][2] = rrts.nodes().front().h(); - } - { - unsigned int ocnt = 0; - for (auto o: rrts.obstacles()) { - unsigned int ccnt = 0; - for (auto c: o.poly()) { - jvo["obst"][ocnt][ccnt][0] = std::get<0>(c); - jvo["obst"][ocnt][ccnt][1] = std::get<1>(c); - ccnt++; - } - ocnt++; - } - } - //{ - // unsigned int ncnt = 0; - // for (auto n: rrts.nodes()) { - // jvo["nodes_x"][ncnt] = n.x(); - // jvo["nodes_y"][ncnt] = n.y(); - // //jvo["nodes_h"][ncnt] = n.h(); - // ncnt++; - // } - //} - { - jvo["nodes"] = (unsigned int) rrts.nodes().size(); - } - { - unsigned int gcnt = 0; - for (auto g: rrts.goals()) { - jvo["goals"][gcnt][0] = g.x(); - jvo["goals"][gcnt][1] = g.y(); - jvo["goals"][gcnt][2] = g.h(); - gcnt++; - } - } - { - if (rrts.path().size() > 0) { - jvo["goal"][0] = rrts.path().back()->x(); - jvo["goal"][1] = rrts.path().back()->y(); - jvo["goal"][2] = rrts.path().back()->h(); - } - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: rrts.path()) { - jvo["path"][pcnt][0] = n->x(); - jvo["path"][pcnt][1] = n->y(); - jvo["path"][pcnt][2] = n->h(); - if (n->t(RRTNodeType::cusp)) - cu++; - if (n->t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["cusps-in-path"] = cu; - jvo["connecteds-in-path"] = co; - } - - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test6.cc b/src/test6.cc deleted file mode 100644 index 7913a51..0000000 --- a/src/test6.cc +++ /dev/null @@ -1,186 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - - if (jvi["init"] == Json::nullValue) { - std::cerr << "I need `init` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goal"] == Json::nullValue) { - std::cerr << "I need `goal` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["goals"] == Json::nullValue) { - std::cerr << "I need `goals` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - if (jvi["obst"] == Json::nullValue) { - std::cerr << "I need `obst` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - RRTCE3 rrts; - rrts.nodes().front().x(jvi["init"][0].asDouble()); - rrts.nodes().front().y(jvi["init"][1].asDouble()); - rrts.nodes().front().h(jvi["init"][2].asDouble()); - { - RRTNode tmp_node; - tmp_node.x(jvi["goal"][0].asDouble()); - tmp_node.y(jvi["goal"][1].asDouble()); - tmp_node.h(jvi["goal"][2].asDouble()); - rrts.goals().push_back(tmp_node); - for (auto g: jvi["goals"]) { - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - rrts.goals().push_back(tmp_node); - } - } - { - Obstacle tmp_obstacle; - for (auto o: jvi["obst"]) { - tmp_obstacle.poly().clear(); - for (auto c: o) { - double tmp_x = c[0].asDouble(); - double tmp_y = c[1].asDouble(); - auto tmp_tuple = std::make_tuple(tmp_x, tmp_y); - tmp_obstacle.poly().push_back(tmp_tuple); - } - rrts.obstacles().push_back(tmp_obstacle); - } - } - - { - double edist_init_goal = sqrt( - pow( - rrts.nodes().front().x() - - rrts.goals().front().x(), - 2 - ) - + pow( - rrts.nodes().front().y() - - rrts.goals().front().y(), - 2 - ) - ); - rrts.set_sample( - rrts.nodes().front().x(), edist_init_goal, - rrts.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - - rrts.init(); - TSTART(); - while (rrts.next()) {} - TEND(); - - { - jvo["time"] = TDIFF(); - } - { - jvo["iterations"] = rrts.icnt(); - } - { - if (rrts.path().size() > 0) { - jvo["cost"] = cc(*rrts.path().back()); - } else { - jvo["cost"] = -1; - } - } - { - jvo["init"][0] = rrts.nodes().front().x(); - jvo["init"][1] = rrts.nodes().front().y(); - jvo["init"][2] = rrts.nodes().front().h(); - } - { - unsigned int ocnt = 0; - for (auto o: rrts.obstacles()) { - unsigned int ccnt = 0; - for (auto c: o.poly()) { - jvo["obst"][ocnt][ccnt][0] = std::get<0>(c); - jvo["obst"][ocnt][ccnt][1] = std::get<1>(c); - ccnt++; - } - ocnt++; - } - } - //{ - // unsigned int ncnt = 0; - // for (auto n: rrts.nodes()) { - // jvo["nodes_x"][ncnt] = n.x(); - // jvo["nodes_y"][ncnt] = n.y(); - // //jvo["nodes_h"][ncnt] = n.h(); - // ncnt++; - // } - //} - { - jvo["nodes"] = (unsigned int) rrts.nodes().size(); - } - { - unsigned int gcnt = 0; - for (auto g: rrts.goals()) { - jvo["goals"][gcnt][0] = g.x(); - jvo["goals"][gcnt][1] = g.y(); - jvo["goals"][gcnt][2] = g.h(); - gcnt++; - } - } - { - if (rrts.path().size() > 0) { - jvo["goal"][0] = rrts.path().back()->x(); - jvo["goal"][1] = rrts.path().back()->y(); - jvo["goal"][2] = rrts.path().back()->h(); - } - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: rrts.path()) { - jvo["path"][pcnt][0] = n->x(); - jvo["path"][pcnt][1] = n->y(); - jvo["path"][pcnt][2] = n->h(); - if (n->t(RRTNodeType::cusp)) - cu++; - if (n->t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["cusps-in-path"] = cu; - jvo["connecteds-in-path"] = co; - } - - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test7.cc b/src/test7.cc deleted file mode 100644 index 3f7e3ff..0000000 --- a/src/test7.cc +++ /dev/null @@ -1,235 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - if (jvi["init"] == Json::nullValue) { - std::cerr << "I need `init` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["goal"] == Json::nullValue) { - std::cerr << "I need `goal` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["goals"] == Json::nullValue) { - std::cerr << "I need `goals` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["obst"] == Json::nullValue) { - std::cerr << "I need `obst` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - std::vector final_path; - double final_path_cost = 9999; - unsigned int final_path_steps = 0; - double cost_from_orig_init = 0; - unsigned int steps_from_orig_init = 0; - RRTNode init_node; - init_node.x(jvi["init"][0].asDouble()); - init_node.y(jvi["init"][1].asDouble()); - init_node.h(jvi["init"][2].asDouble()); - init_node.sp(2.7); - init_node.st(M_PI / 32); // only for sc2_4 - RRTNode last_node(init_node); - RRTNode orig_init(init_node); - init_node.next(); - steps_from_orig_init++; - { - double angl_diff = std::abs(last_node.h() - init_node.h()); - if (angl_diff == 0) - cost_from_orig_init += sqrt( - pow(last_node.x() - init_node.x(), 2) - + pow(last_node.y() - init_node.y(), 2) - ); - else - cost_from_orig_init += angl_diff * init_node.mtr(); - } - - RRTS rrts; - rrts.nodes().front().x(init_node.x()); - rrts.nodes().front().y(init_node.y()); - rrts.nodes().front().h(init_node.h()); - { - RRTNode tmp_node; - tmp_node.x(jvi["goal"][0].asDouble()); - tmp_node.y(jvi["goal"][1].asDouble()); - tmp_node.h(jvi["goal"][2].asDouble()); - rrts.goals().push_back(tmp_node); - for (auto g: jvi["goals"]) { - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - rrts.goals().push_back(tmp_node); - } - } - { - Obstacle tmp_obstacle; - for (auto o: jvi["obst"]) { - tmp_obstacle.poly().clear(); - for (auto c: o) { - double tmp_x = c[0].asDouble(); - double tmp_y = c[1].asDouble(); - auto tmp_tuple = std::make_tuple(tmp_x, tmp_y); - tmp_obstacle.poly().push_back(tmp_tuple); - } - rrts.obstacles().push_back(tmp_obstacle); - } - } - { - double edist_init_goal = sqrt( - pow( - rrts.nodes().front().x() - - rrts.goals().front().x(), - 2 - ) - + pow( - rrts.nodes().front().y() - - rrts.goals().front().y(), - 2 - ) - ); - rrts.set_sample( - rrts.nodes().front().x(), edist_init_goal, - rrts.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - - TSTART(); - while (rrts.next()) {} - TEND(); - if (rrts.path().size() > 0) { - if ( - cc(*rrts.path().back()) - + cost_from_orig_init - < final_path_cost - ) { - final_path_cost = - cc(*rrts.path().back()) - + cost_from_orig_init - ; - final_path_steps = steps_from_orig_init; - final_path.clear(); - for (auto n: rrts.path()) { - final_path.push_back(RRTNode()); - final_path.back().x(n->x()); - final_path.back().y(n->y()); - final_path.back().h(n->h()); - } - } - } - int rcnt = 0; - jvo[rcnt++] = rrts.json(); -for (int i = 1; i < 5; i++) { - last_node = RRTNode(init_node); - init_node.next(); - steps_from_orig_init++; - { - double angl_diff = std::abs(last_node.h() - init_node.h()); - if (angl_diff < 0.001) - cost_from_orig_init += sqrt( - pow(last_node.x() - init_node.x(), 2) - + pow(last_node.y() - init_node.y(), 2) - ); - else - cost_from_orig_init += angl_diff * init_node.mtr(); - } - RRTS rrts2; - rrts2.goals() = rrts.goals(); - rrts2.obstacles() = rrts.obstacles(); - rrts2.nodes().front().x(init_node.x()); - rrts2.nodes().front().y(init_node.y()); - rrts2.nodes().front().h(init_node.h()); - { - double edist_init_goal = sqrt( - pow( - rrts2.nodes().front().x() - - rrts2.goals().front().x(), - 2 - ) - + pow( - rrts2.nodes().front().y() - - rrts2.goals().front().y(), - 2 - ) - ); - rrts2.set_sample( - rrts2.nodes().front().x(), edist_init_goal, - rrts2.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - TSTART(); - while (rrts2.next()) {} - TEND(); - if (rrts2.path().size() > 0) { - if ( - cc(*rrts2.path().back()) - + cost_from_orig_init - < final_path_cost - ) { - final_path_cost = - cc(*rrts2.path().back()) - + cost_from_orig_init - ; - final_path_steps = steps_from_orig_init; - final_path.clear(); - for (auto n: rrts2.path()) { - final_path.push_back(RRTNode()); - final_path.back().x(n->x()); - final_path.back().y(n->y()); - final_path.back().h(n->h()); - } - } - } - jvo[rcnt++] = rrts2.json(); -} - { - jvo[rcnt]["obst"] = jvo[rcnt - 1]["obst"]; - jvo[rcnt]["cost"] = final_path_cost; - unsigned int ncnt = 0; - for (unsigned int i = 0; i < final_path_steps; i++) { - jvo[rcnt]["path"][ncnt][0] = orig_init.x(); - jvo[rcnt]["path"][ncnt][1] = orig_init.y(); - jvo[rcnt]["path"][ncnt][2] = orig_init.h(); - orig_init.next(); - ncnt++; - } - for (auto n: final_path) { - jvo[rcnt]["path"][ncnt][0] = n.x(); - jvo[rcnt]["path"][ncnt][1] = n.y(); - jvo[rcnt]["path"][ncnt][2] = n.h(); - ncnt++; - } - } - std::cout << jvo << std::endl; - return 0; -} diff --git a/src/test8.cc b/src/test8.cc deleted file mode 100644 index d7b894c..0000000 --- a/src/test8.cc +++ /dev/null @@ -1,107 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - if (jvi["init"] == Json::nullValue) { - std::cerr << "I need `init` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["goal"] == Json::nullValue) { - std::cerr << "I need `goal` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["goals"] == Json::nullValue) { - std::cerr << "I need `goals` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["obst"] == Json::nullValue) { - std::cerr << "I need `obst` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - RRTCE5 rrts; - rrts.nodes().front().x(jvi["init"][0].asDouble()); - rrts.nodes().front().y(jvi["init"][1].asDouble()); - rrts.nodes().front().h(jvi["init"][2].asDouble()); - { - RRTNode tmp_node; - tmp_node.x(jvi["goal"][0].asDouble()); - tmp_node.y(jvi["goal"][1].asDouble()); - tmp_node.h(jvi["goal"][2].asDouble()); - rrts.goals().push_back(tmp_node); - for (auto g: jvi["goals"]) { - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - rrts.goals().push_back(tmp_node); - } - } - { - Obstacle tmp_obstacle; - for (auto o: jvi["obst"]) { - tmp_obstacle.poly().clear(); - for (auto c: o) { - double tmp_x = c[0].asDouble(); - double tmp_y = c[1].asDouble(); - auto tmp_tuple = std::make_tuple(tmp_x, tmp_y); - tmp_obstacle.poly().push_back(tmp_tuple); - } - rrts.obstacles().push_back(tmp_obstacle); - } - } - { - double edist_init_goal = sqrt( - pow( - rrts.nodes().front().x() - - rrts.goals().front().x(), - 2 - ) - + pow( - rrts.nodes().front().y() - - rrts.goals().front().y(), - 2 - ) - ); - rrts.set_sample( - rrts.nodes().front().x(), edist_init_goal, - rrts.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - - rrts.init(); - TSTART(); - while (rrts.next()) {} - TEND(); - - std::cout << rrts.json() << std::endl; - return 0; -} diff --git a/src/test9.cc b/src/test9.cc deleted file mode 100644 index e014e60..0000000 --- a/src/test9.cc +++ /dev/null @@ -1,107 +0,0 @@ -#include -#include -#include - -#include "rrtce.h" - -std::chrono::high_resolution_clock::time_point TSTART_; -std::chrono::high_resolution_clock::time_point TEND_; -inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); } -inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); } -inline double TDIFF() -{ - std::chrono::duration DT_; - DT_ = std::chrono::duration_cast>( - TEND_ - TSTART_ - ); - return DT_.count(); -} -inline void TPRINT(const char *what) -{ - std::cerr << what << ": " << TDIFF() << std::endl; -} - -int main() -{ - Json::Value jvi; // JSON input - Json::Value jvo; // JSON output - std::cin >> jvi; - if (jvi["init"] == Json::nullValue) { - std::cerr << "I need `init` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["goal"] == Json::nullValue) { - std::cerr << "I need `goal` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["goals"] == Json::nullValue) { - std::cerr << "I need `goals` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - if (jvi["obst"] == Json::nullValue) { - std::cerr << "I need `obst` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - RRTCE7 rrts; - rrts.nodes().front().x(jvi["init"][0].asDouble()); - rrts.nodes().front().y(jvi["init"][1].asDouble()); - rrts.nodes().front().h(jvi["init"][2].asDouble()); - { - RRTNode tmp_node; - tmp_node.x(jvi["goal"][0].asDouble()); - tmp_node.y(jvi["goal"][1].asDouble()); - tmp_node.h(jvi["goal"][2].asDouble()); - rrts.goals().push_back(tmp_node); - for (auto g: jvi["goals"]) { - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - rrts.goals().push_back(tmp_node); - } - } - { - Obstacle tmp_obstacle; - for (auto o: jvi["obst"]) { - tmp_obstacle.poly().clear(); - for (auto c: o) { - double tmp_x = c[0].asDouble(); - double tmp_y = c[1].asDouble(); - auto tmp_tuple = std::make_tuple(tmp_x, tmp_y); - tmp_obstacle.poly().push_back(tmp_tuple); - } - rrts.obstacles().push_back(tmp_obstacle); - } - } - { - double edist_init_goal = sqrt( - pow( - rrts.nodes().front().x() - - rrts.goals().front().x(), - 2 - ) - + pow( - rrts.nodes().front().y() - - rrts.goals().front().y(), - 2 - ) - ); - rrts.set_sample( - rrts.nodes().front().x(), edist_init_goal, - rrts.nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } - - rrts.init(); - TSTART(); - while (rrts.next()) {} - TEND(); - - std::cout << rrts.json() << std::endl; - return 0; -}