From: Jiri Vlasak Date: Tue, 13 Jul 2021 10:49:40 +0000 (+0200) Subject: Change spacing X-Git-Tag: v0.8.0~1^2~20 X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/90d163f545ab59eb9a286093e4bc6993edfe93c1 Change spacing --- diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 0000000..549987b --- /dev/null +++ b/.editorconfig @@ -0,0 +1,19 @@ +root = true + +[*.{cc,hh}] +end_of_line = lf +insert_final_newline = true +charset = utf-8 +trim_trailing_whitespace = true +indent_style = tab +indent_size = 8 +max_line_length = 80 + +[*.{py,md}] +end_of_line = lf +insert_final_newline = true +charset = utf-8 +trim_trailing_whitespace = true +indent_style = space +indent_size = 4 +max_line_length = 80 diff --git a/api/rrtce.h b/api/rrtce.h index eadd1a8..de04476 100644 --- a/api/rrtce.h +++ b/api/rrtce.h @@ -12,635 +12,635 @@ Compound extensions have no implementation. #include "rrtext.h" class RRTCE35 - : public RRTExt2 - , public RRTExt11 - , public RRTExt13 - , public RRTExt8 - , public RRTExt10 + : public RRTExt2 + , public RRTExt11 + , public RRTExt13 + , public RRTExt8 + , public RRTExt10 { - public: - void init() - { - RRTExt2::init(); - RRTExt8::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt8::deinit(); - } - void reset() - { - RRTExt8::reset(); - RRTExt13::reset(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt8::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt8::deinit(); + } + void reset() + { + RRTExt8::reset(); + RRTExt13::reset(); + } }; class RRTCE34 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt8 - , public RRTExt10 - , public RRTExt12 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt8 + , public RRTExt10 + , public RRTExt12 { - public: - void init() - { - RRTExt2::init(); - RRTExt8::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt8::deinit(); - } - void reset() - { - RRTExt8::reset(); - RRTExt3::reset(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt8::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt8::deinit(); + } + void reset() + { + RRTExt8::reset(); + RRTExt3::reset(); + } }; class RRTCE33 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt8 - , public RRTExt10 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt8 + , public RRTExt10 { - public: - void init() - { - RRTExt2::init(); - RRTExt8::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt8::deinit(); - } - void reset() - { - RRTExt8::reset(); - RRTExt3::reset(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt8::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt8::deinit(); + } + void reset() + { + RRTExt8::reset(); + RRTExt3::reset(); + } }; class RRTCE32 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt8 - , public RRTExt1 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt8 + , public RRTExt1 { - public: - void init() - { - RRTExt2::init(); - RRTExt8::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt8::deinit(); - } - void reset() - { - RRTExt8::reset(); - RRTExt3::reset(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt8::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt8::deinit(); + } + void reset() + { + RRTExt8::reset(); + RRTExt3::reset(); + } }; class RRTCE31 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt8 - , public RRTExt5 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt8 + , public RRTExt5 { - public: - void init() - { - RRTExt2::init(); - RRTExt8::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt8::deinit(); - } - void reset() - { - RRTExt8::reset(); - RRTExt3::reset(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt8::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt8::deinit(); + } + void reset() + { + RRTExt8::reset(); + RRTExt3::reset(); + } }; class RRTCE30 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt8 - , public RRTExt6 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt8 + , public RRTExt6 { - public: - void init() - { - RRTExt2::init(); - RRTExt8::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt8::deinit(); - } - void reset() - { - RRTExt8::reset(); - RRTExt3::reset(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt8::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt8::deinit(); + } + void reset() + { + RRTExt8::reset(); + RRTExt3::reset(); + } }; class RRTCE29 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt9 - , public RRTExt10 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt9 + , public RRTExt10 { - public: - void init() - { - RRTExt2::init(); - RRTExt9::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt9::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt9::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt9::deinit(); + } }; class RRTCE28 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt9 - , public RRTExt1 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt9 + , public RRTExt1 { - public: - void init() - { - RRTExt2::init(); - RRTExt9::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt9::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt9::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt9::deinit(); + } }; class RRTCE27 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt9 - , public RRTExt5 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt9 + , public RRTExt5 { - public: - void init() - { - RRTExt2::init(); - RRTExt9::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt9::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt9::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt9::deinit(); + } }; class RRTCE26 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt9 - , public RRTExt6 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt9 + , public RRTExt6 { - public: - void init() - { - RRTExt2::init(); - RRTExt9::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt9::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt9::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt9::deinit(); + } }; class RRTCE25 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt7 - , public RRTExt10 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt7 + , public RRTExt10 { - public: - void init() - { - RRTExt2::init(); - RRTExt7::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt7::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt7::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt7::deinit(); + } }; class RRTCE24 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt7 - , public RRTExt1 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt7 + , public RRTExt1 { - public: - void init() - { - RRTExt2::init(); - RRTExt7::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt7::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt7::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt7::deinit(); + } }; class RRTCE23 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt7 - , public RRTExt5 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt7 + , public RRTExt5 { - public: - void init() - { - RRTExt2::init(); - RRTExt7::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt7::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt7::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt7::deinit(); + } }; class RRTCE22 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt7 - , public RRTExt6 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt7 + , public RRTExt6 { - public: - void init() - { - RRTExt2::init(); - RRTExt7::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt7::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt7::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt7::deinit(); + } }; class RRTCE21 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt4 - , public RRTExt10 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt4 + , public RRTExt10 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE20 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt4 - , public RRTExt1 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt4 + , public RRTExt1 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE19 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt4 - , public RRTExt5 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt4 + , public RRTExt5 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE18 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt4 - , public RRTExt6 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt4 + , public RRTExt6 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE17 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt10 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt10 {}; class RRTCE16 - : public RRTExt2 - , public RRTExt3 - , public RRTExt9 - , public RRTExt1 + : public RRTExt2 + , public RRTExt3 + , public RRTExt9 + , public RRTExt1 { - public: - void init() - { - RRTExt2::init(); - RRTExt9::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt9::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt9::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt9::deinit(); + } }; class RRTCE15 - : public RRTExt2 - , public RRTExt3 - , public RRTExt9 - , public RRTExt6 + : public RRTExt2 + , public RRTExt3 + , public RRTExt9 + , public RRTExt6 { - public: - void init() - { - RRTExt2::init(); - RRTExt9::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt9::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt9::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt9::deinit(); + } }; class RRTCE14 - : public RRTExt2 - , public RRTExt3 - , public RRTExt8 - , public RRTExt1 + : public RRTExt2 + , public RRTExt3 + , public RRTExt8 + , public RRTExt1 { - public: - void init() - { - RRTExt2::init(); - RRTExt8::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt8::deinit(); - } - void reset() - { - RRTExt8::reset(); - RRTExt3::reset(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt8::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt8::deinit(); + } + void reset() + { + RRTExt8::reset(); + RRTExt3::reset(); + } }; class RRTCE13 - : public RRTExt2 - , public RRTExt3 - , public RRTExt8 - , public RRTExt6 + : public RRTExt2 + , public RRTExt3 + , public RRTExt8 + , public RRTExt6 { - public: - void init() - { - RRTExt2::init(); - RRTExt8::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt8::deinit(); - } - void reset() - { - RRTExt8::reset(); - RRTExt3::reset(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt8::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt8::deinit(); + } + void reset() + { + RRTExt8::reset(); + RRTExt3::reset(); + } }; class RRTCE12 - : public RRTExt2 - , public RRTExt3 - , public RRTExt7 - , public RRTExt1 + : public RRTExt2 + , public RRTExt3 + , public RRTExt7 + , public RRTExt1 { - public: - void init() - { - RRTExt2::init(); - RRTExt7::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt7::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt7::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt7::deinit(); + } }; class RRTCE11 - : public RRTExt2 - , public RRTExt3 - , public RRTExt7 - , public RRTExt5 + : public RRTExt2 + , public RRTExt3 + , public RRTExt7 + , public RRTExt5 { - public: - void init() - { - RRTExt2::init(); - RRTExt7::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt7::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt7::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt7::deinit(); + } }; class RRTCE10 - : public RRTExt2 - , public RRTExt3 - , public RRTExt7 - , public RRTExt6 + : public RRTExt2 + , public RRTExt3 + , public RRTExt7 + , public RRTExt6 { - public: - void init() - { - RRTExt2::init(); - RRTExt7::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt7::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt7::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt7::deinit(); + } }; class RRTCE9 - : public RRTExt2 - , public RRTExt3 - , public RRTExt4 - , public RRTExt1 + : public RRTExt2 + , public RRTExt3 + , public RRTExt4 + , public RRTExt1 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE8 - : public RRTExt2 - , public RRTExt3 - , public RRTExt4 - , public RRTExt6 + : public RRTExt2 + , public RRTExt3 + , public RRTExt4 + , public RRTExt6 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE7 - : public RRTExt2 - , public RRTExt3 - , public RRTExt4 - , public RRTExt5 + : public RRTExt2 + , public RRTExt3 + , public RRTExt4 + , public RRTExt5 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE6 : public RRTExt2, public RRTExt6, public RRTExt4 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE5 : public RRTExt2, public RRTExt5, public RRTExt4 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE4 : public RRTExt1, public RRTExt2, public RRTExt4 { - public: - void init() - { - RRTExt2::init(); - RRTExt4::init(); - } - void deinit() - { - RRTExt2::deinit(); - RRTExt4::deinit(); - } + public: + void init() + { + RRTExt2::init(); + RRTExt4::init(); + } + void deinit() + { + RRTExt2::deinit(); + RRTExt4::deinit(); + } }; class RRTCE3 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt6 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt6 {}; class RRTCE2 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt5 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt5 {}; class RRTCE1 - : public RRTExt2 - , public RRTExt11 - , public RRTExt3 - , public RRTExt1 + : public RRTExt2 + , public RRTExt11 + , public RRTExt3 + , public RRTExt1 {}; #endif /* RRTCE_H */ diff --git a/api/rrtext.h b/api/rrtext.h index 4e1f825..8184e6a 100644 --- a/api/rrtext.h +++ b/api/rrtext.h @@ -18,36 +18,36 @@ /*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */ class RRTExt13 : public virtual RRTS { - private: - public: - void reset(); - std::vector orig_path_; - double orig_path_cost_ = 9999; - std::vector first_optimized_path_; - double first_optimized_path_cost_ = 9999; - void first_path_optimization(); - void second_path_optimization(); - void compute_path(); - Json::Value json(); - void json(Json::Value jvi); - - // getter, setter - std::vector &orig_path() - { - return this->orig_path_; - }; - double &orig_path_cost() { return this->orig_path_cost_; } - void orig_path_cost(double c) { this->orig_path_cost_ = c; } - std::vector &first_optimized_path() - { - return this->first_optimized_path_; - }; - double &first_optimized_path_cost() { - return this->first_optimized_path_cost_; - } - void first_optimized_path_cost(double c) { - this->first_optimized_path_cost_ = c; - } + private: + public: + void reset(); + std::vector orig_path_; + double orig_path_cost_ = 9999; + std::vector first_optimized_path_; + double first_optimized_path_cost_ = 9999; + void first_path_optimization(); + void second_path_optimization(); + void compute_path(); + Json::Value json(); + void json(Json::Value jvi); + + // getter, setter + std::vector &orig_path() + { + return this->orig_path_; + }; + double &orig_path_cost() { return this->orig_path_cost_; } + void orig_path_cost(double c) { this->orig_path_cost_ = c; } + std::vector &first_optimized_path() + { + return this->first_optimized_path_; + }; + double &first_optimized_path_cost() { + return this->first_optimized_path_cost_; + } + void first_optimized_path_cost(double c) { + this->first_optimized_path_cost_ = c; + } }; /*! \brief Different `steer` procedures. @@ -55,19 +55,19 @@ class RRTExt13 : public virtual RRTS { Use sampling in control input for `steer1`. Use R&S steering for `steer2`. */ class RRTExt12 : public virtual RRTS { - protected: - void steer1(RRTNode &f, RRTNode &t); - bool connect(); - public: - bool next(); + protected: + void steer1(RRTNode &f, RRTNode &t); + bool connect(); + public: + bool next(); }; /*! \brief Goal Zone. */ class RRTExt11 : public virtual RRTS { - protected: - bool goal_found(RRTNode &f); + protected: + bool goal_found(RRTNode &f); }; /*! \brief Different costs extension. @@ -80,13 +80,13 @@ IEEE Transactions on Intelligent Transportation Systems 17, no. 2 (February */ class RRTExt10 : public virtual RRTS { - public: - /*! \brief Reeds and Shepp path length. - */ - double cost_build(RRTNode &f, RRTNode &t); - /*! \brief Heuristics distance. - */ - double cost_search(RRTNode &f, RRTNode &t); + public: + /*! \brief Reeds and Shepp path length. + */ + double cost_build(RRTNode &f, RRTNode &t); + /*! \brief Heuristics distance. + */ + double cost_search(RRTNode &f, RRTNode &t); }; /*! \brief Use grid data structure to store RRT nodes. @@ -95,44 +95,44 @@ This approach speeds up the search process for the nearest neighbor and the near vertices procedures. */ class RRTExt9 : public virtual RRTS { - private: - class Cell { - private: - bool changed_ = false; - std::vector nodes_; - public: - void nn(RRTNode *t, RRTNode **nn, RRTS *p); - void store_node(RRTNode *n); - - // getter, setter - bool changed() const - { - return this->changed_; - } - std::vector &nodes() - { - return this->nodes_; - } - - Cell(); - }; - Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI]; - double x_min_ = 0; - double x_max_ = 0; - double y_min_ = 0; - double y_max_ = 0; - double h_min_ = 0; - double h_max_ = 2 * M_PI; - - unsigned int xi(RRTNode n); - unsigned int yi(RRTNode n); - unsigned int hi(RRTNode n); - public: - void init(); - void deinit(); - void store_node(RRTNode n); - RRTNode *nn(RRTNode &t); - std::vector nv(RRTNode &t); + private: + class Cell { + private: + bool changed_ = false; + std::vector nodes_; + public: + void nn(RRTNode *t, RRTNode **nn, RRTS *p); + void store_node(RRTNode *n); + + // getter, setter + bool changed() const + { + return this->changed_; + } + std::vector &nodes() + { + return this->nodes_; + } + + Cell(); + }; + Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI]; + double x_min_ = 0; + double x_max_ = 0; + double y_min_ = 0; + double y_max_ = 0; + double h_min_ = 0; + double h_max_ = 2 * M_PI; + + unsigned int xi(RRTNode n); + unsigned int yi(RRTNode n); + unsigned int hi(RRTNode n); + public: + void init(); + void deinit(); + void store_node(RRTNode n); + RRTNode *nn(RRTNode &t); + std::vector nv(RRTNode &t); }; /*! \brief Use k-d tree data structure to store RRT nodes. @@ -143,42 +143,42 @@ the near vertices procedures. This extension implements 3D K-d tree. \see https://en.wikipedia.org/wiki/K-d_tree */ class RRTExt8 : public virtual RRTS { - private: - class KdNode { - private: - RRTNode *node_ = nullptr; - KdNode *left_ = nullptr; - KdNode *right_ = nullptr; - public: - // getter, setter - RRTNode *node() const { return this->node_; } - KdNode *&left() { return this->left_; } - KdNode *&right() { return this->right_; } - KdNode(RRTNode *n); - }; - KdNode *kdroot_ = nullptr; - void delete_kd_nodes(KdNode *n); - void store_node(RRTNode *n, KdNode *&r, int l); - void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d); - void nv( - std::vector& n, - RRTNode &t, - KdNode *r, - int l, - double const& d - ); - public: - void delete_kd_nodes() - { - this->delete_kd_nodes(this->kdroot_); - this->kdroot_ = nullptr; - } - void init(); - void reset(); - void deinit(); - void store_node(RRTNode n); - RRTNode *nn(RRTNode &t); - std::vector nv(RRTNode &t); + private: + class KdNode { + private: + RRTNode *node_ = nullptr; + KdNode *left_ = nullptr; + KdNode *right_ = nullptr; + public: + // getter, setter + RRTNode *node() const { return this->node_; } + KdNode *&left() { return this->left_; } + KdNode *&right() { return this->right_; } + KdNode(RRTNode *n); + }; + KdNode *kdroot_ = nullptr; + void delete_kd_nodes(KdNode *n); + void store_node(RRTNode *n, KdNode *&r, int l); + void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d); + void nv( + std::vector& n, + RRTNode &t, + KdNode *r, + int l, + double const& d + ); + public: + void delete_kd_nodes() + { + this->delete_kd_nodes(this->kdroot_); + this->kdroot_ = nullptr; + } + void init(); + void reset(); + void deinit(); + void store_node(RRTNode n); + RRTNode *nn(RRTNode &t); + std::vector nv(RRTNode &t); }; /*! \brief Use k-d tree data structure to store RRT nodes. @@ -189,41 +189,41 @@ the near vertices procedures. This extension implements 2D K-d tree. \see https://en.wikipedia.org/wiki/K-d_tree */ class RRTExt7 : public virtual RRTS { - private: - class KdNode { - private: - RRTNode *node_ = nullptr; - KdNode *left_ = nullptr; - KdNode *right_ = nullptr; - public: - // getter, setter - RRTNode *node() const { return this->node_; } - KdNode *&left() { return this->left_; } - KdNode *&right() { return this->right_; } - KdNode(RRTNode *n); - }; - KdNode *kdroot_ = nullptr; - void delete_kd_nodes(KdNode *n); - void store_node(RRTNode *n, KdNode *&r, int l); - void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d); - public: - void init(); - void deinit(); - void store_node(RRTNode n); - RRTNode *nn(RRTNode &t); - std::vector nv(RRTNode &t); + private: + class KdNode { + private: + RRTNode *node_ = nullptr; + KdNode *left_ = nullptr; + KdNode *right_ = nullptr; + public: + // getter, setter + RRTNode *node() const { return this->node_; } + KdNode *&left() { return this->left_; } + KdNode *&right() { return this->right_; } + KdNode(RRTNode *n); + }; + KdNode *kdroot_ = nullptr; + void delete_kd_nodes(KdNode *n); + void store_node(RRTNode *n, KdNode *&r, int l); + void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d); + public: + void init(); + void deinit(); + void store_node(RRTNode n); + RRTNode *nn(RRTNode &t); + std::vector nv(RRTNode &t); }; /*! \brief Reeds and Shepp cost for building and search. */ class RRTExt6 : public virtual RRTS { - public: - /*! \brief Reeds and Shepp path length. - */ - double cost_build(RRTNode &f, RRTNode &t); - /*! \brief Reeds and Shepp path length. - */ - double cost_search(RRTNode &f, RRTNode &t); + public: + /*! \brief Reeds and Shepp path length. + */ + double cost_build(RRTNode &f, RRTNode &t); + /*! \brief Reeds and Shepp path length. + */ + double cost_search(RRTNode &f, RRTNode &t); }; /*! \brief Different costs extension. @@ -232,13 +232,13 @@ Use different cost for bulding tree data structure and searching in the structure. This one is complementary to `rrtext1.cc`. */ class RRTExt5 : public virtual RRTS { - public: - /*! \brief Reeds and Shepp path length. - */ - double cost_build(RRTNode &f, RRTNode &t); - /*! \brief Euclidean distance. - */ - double cost_search(RRTNode &f, RRTNode &t); + public: + /*! \brief Reeds and Shepp path length. + */ + double cost_build(RRTNode &f, RRTNode &t); + /*! \brief Euclidean distance. + */ + double cost_search(RRTNode &f, RRTNode &t); }; /*! \brief Use grid data structure to store RRT nodes. @@ -247,76 +247,76 @@ This approach speeds up the search process for the nearest neighbor and the near vertices procedures. */ class RRTExt4 : public virtual RRTS { - private: - class Cell { - private: - bool changed_ = false; - std::vector nodes_; - public: - void nn(RRTNode *t, RRTNode **nn, RRTS *p); - void store_node(RRTNode *n); - - // getter, setter - bool changed() const - { - return this->changed_; - } - std::vector &nodes() - { - return this->nodes_; - } - - Cell(); - }; - Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left - double x_min_ = 0; - double x_max_ = 0; - double y_min_ = 0; - double y_max_ = 0; - - unsigned int xi(RRTNode n); - unsigned int yi(RRTNode n); - public: - void init(); - void deinit(); - void store_node(RRTNode n); - RRTNode *nn(RRTNode &t); - std::vector nv(RRTNode &t); + private: + class Cell { + private: + bool changed_ = false; + std::vector nodes_; + public: + void nn(RRTNode *t, RRTNode **nn, RRTS *p); + void store_node(RRTNode *n); + + // getter, setter + bool changed() const + { + return this->changed_; + } + std::vector &nodes() + { + return this->nodes_; + } + + Cell(); + }; + Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left + double x_min_ = 0; + double x_max_ = 0; + double y_min_ = 0; + double y_max_ = 0; + + unsigned int xi(RRTNode n); + unsigned int yi(RRTNode n); + public: + void init(); + void deinit(); + void store_node(RRTNode n); + RRTNode *nn(RRTNode &t); + std::vector nv(RRTNode &t); }; /*! \brief Use Dijkstra algorithm to find the shorter path. */ class RRTExt3 : public virtual RRTS { - private: - public: - void reset(); - std::vector orig_path_; - double orig_path_cost_ = 9999; - std::vector first_optimized_path_; - double first_optimized_path_cost_ = 9999; - void first_path_optimization(); - void second_path_optimization(); - void compute_path(); - Json::Value json(); - void json(Json::Value jvi); - - // getter, setter - std::vector &orig_path() - { - return this->orig_path_; - }; - double &orig_path_cost() { return this->orig_path_cost_; } - void orig_path_cost(double c) { this->orig_path_cost_ = c; } - std::vector &first_optimized_path() - { - return this->first_optimized_path_; - }; - double &first_optimized_path_cost() { - return this->first_optimized_path_cost_; - } - void first_optimized_path_cost(double c) { - this->first_optimized_path_cost_ = c; - } + private: + public: + void reset(); + std::vector orig_path_; + double orig_path_cost_ = 9999; + std::vector first_optimized_path_; + double first_optimized_path_cost_ = 9999; + void first_path_optimization(); + void second_path_optimization(); + void compute_path(); + Json::Value json(); + void json(Json::Value jvi); + + // getter, setter + std::vector &orig_path() + { + return this->orig_path_; + }; + double &orig_path_cost() { return this->orig_path_cost_; } + void orig_path_cost(double c) { this->orig_path_cost_ = c; } + std::vector &first_optimized_path() + { + return this->first_optimized_path_; + }; + double &first_optimized_path_cost() { + return this->first_optimized_path_cost_; + } + void first_optimized_path_cost(double c) { + this->first_optimized_path_cost_ = c; + } }; /*! \brief Use cute_c2 for collision detection. @@ -324,29 +324,29 @@ class RRTExt3 : public virtual RRTS { \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h */ class RRTExt2 : public virtual RRTS { - private: - c2Poly c2_bc_; - c2x c2x_bc_; - std::vector c2_obstacles_; - public: - void init(); - void deinit(); - - // Collide RRT procedures - std::tuple - collide_steered_from(RRTNode &f); - std::tuple - collide_tmp_steered_from(RRTNode &f); - - std::tuple - collide_two_nodes(RRTNode &f, RRTNode &t); - - // getters, setters - c2Poly &c2_bc() { return this->c2_bc_; } - c2x &c2x_bc() { return this->c2x_bc_; } - std::vector &c2_obstacles() { - return this->c2_obstacles_; - }; + private: + c2Poly c2_bc_; + c2x c2x_bc_; + std::vector c2_obstacles_; + public: + void init(); + void deinit(); + + // Collide RRT procedures + std::tuple + collide_steered_from(RRTNode &f); + std::tuple + collide_tmp_steered_from(RRTNode &f); + + std::tuple + collide_two_nodes(RRTNode &f, RRTNode &t); + + // getters, setters + c2Poly &c2_bc() { return this->c2_bc_; } + c2x &c2x_bc() { return this->c2x_bc_; } + std::vector &c2_obstacles() { + return this->c2_obstacles_; + }; }; /*! \brief Different costs extension. @@ -355,13 +355,13 @@ Use different cost for bulding tree data structure and searching in the structure. */ class RRTExt1 : public virtual RRTS { - public: - /*! \brief Reeds and Shepp path length. - */ - double cost_build(RRTNode &f, RRTNode &t); - /*! \brief Matej's heuristics. - */ - double cost_search(RRTNode &f, RRTNode &t); + public: + /*! \brief Reeds and Shepp path length. + */ + double cost_build(RRTNode &f, RRTNode &t); + /*! \brief Matej's heuristics. + */ + double cost_search(RRTNode &f, RRTNode &t); }; #endif /* RRTEXT_H */ diff --git a/api/rrts.h b/api/rrts.h index 42724d7..f7f704b 100644 --- a/api/rrts.h +++ b/api/rrts.h @@ -10,8 +10,8 @@ #define ETA 1.0 // for steer, nv #define GAMMA(cV) ({ \ - __typeof__ (cV) _cV = (cV); \ - pow(log(_cV) / _cV, 1.0 / 3.0); \ + __typeof__ (cV) _cV = (cV); \ + pow(log(_cV) / _cV, 1.0 / 3.0); \ }) /*! \brief Possible type of RRT node. @@ -20,9 +20,9 @@ \param connected The node that branches generated steered path. */ class RRTNodeType { - public: - static const unsigned int cusp = 1 << 0; - static const unsigned int connected = 1 << 1; + public: + static const unsigned int cusp = 1 << 0; + static const unsigned int connected = 1 << 1; }; /*! \brief RRT node basic class. @@ -38,73 +38,73 @@ class RRTNodeType { \param st Steering of the car. */ class RRTNode { - private: - RRTNode *p_ = nullptr; - unsigned int t_ = 0; - // -- from BicycleCar - // coordinates - double x_ = 0; - double y_ = 0; - double h_ = 0; - // moving - double sp_ = 0; - double st_ = 0; - public: - double c_ = 0; - double cc = 0.0; - // getters, setters - double c() const { return this->c_; } - void c(double c) - { - this->c_ = c; - this->cc = this->p_->cc + this->c_; - } - - RRTNode *p() const { return this->p_; } - void p(RRTNode *p) { this->p_ = p; } - - bool t(unsigned int flag) { return this->t_ & flag; } - void set_t(unsigned int flag) { this->t_ |= flag; } - void clear_t(unsigned int flag) { this->t_ &= ~flag; } - - // -- from BicycleCar - // getters, setters - double x() const { return this->x_; } - void x(double x) { this->x_ = x; } - - double y() const { return this->y_; } - void y(double y) { this->y_ = y; } - - double h() const { return this->h_; } - void h(double h) - { - while (h < -M_PI) - h += 2 * M_PI; - while (h > +M_PI) - h -= 2 * M_PI; - this->h_ = h; - } - - double sp() const { return this->sp_; } - void sp(double sp) { this->sp_ = sp; } - - double st() const { return this->st_; } - void st(double st) { this->st_ = st; } - - RRTNode(); - RRTNode(const BicycleCar &bc); - bool operator==(const RRTNode& n); - friend std::ostream &operator<<( - std::ostream &out, - const RRTNode &bc - ) - { - out << "[" << bc.x(); - out << "," << bc.y(); - out << "," << bc.h(); - out << "]"; - return out; - } + private: + RRTNode *p_ = nullptr; + unsigned int t_ = 0; + // -- from BicycleCar + // coordinates + double x_ = 0; + double y_ = 0; + double h_ = 0; + // moving + double sp_ = 0; + double st_ = 0; + public: + double c_ = 0; + double cc = 0.0; + // getters, setters + double c() const { return this->c_; } + void c(double c) + { + this->c_ = c; + this->cc = this->p_->cc + this->c_; + } + + RRTNode *p() const { return this->p_; } + void p(RRTNode *p) { this->p_ = p; } + + bool t(unsigned int flag) { return this->t_ & flag; } + void set_t(unsigned int flag) { this->t_ |= flag; } + void clear_t(unsigned int flag) { this->t_ &= ~flag; } + + // -- from BicycleCar + // getters, setters + double x() const { return this->x_; } + void x(double x) { this->x_ = x; } + + double y() const { return this->y_; } + void y(double y) { this->y_ = y; } + + double h() const { return this->h_; } + void h(double h) + { + while (h < -M_PI) + h += 2 * M_PI; + while (h > +M_PI) + h -= 2 * M_PI; + this->h_ = h; + } + + double sp() const { return this->sp_; } + void sp(double sp) { this->sp_ = sp; } + + double st() const { return this->st_; } + void st(double st) { this->st_ = st; } + + RRTNode(); + RRTNode(const BicycleCar &bc); + bool operator==(const RRTNode& n); + friend std::ostream &operator<<( + std::ostream &out, + const RRTNode &bc + ) + { + out << "[" << bc.x(); + out << "," << bc.y(); + out << "," << bc.h(); + out << "]"; + return out; + } }; /*! \brief Polygon obstacle basic class. @@ -112,16 +112,16 @@ class RRTNode { \param poly Border polygon of the obstacle. */ class Obstacle { - private: - std::vector> poly_; - public: - // getters, setters - std::vector> &poly() - { - return this->poly_; - } - - Obstacle(); + private: + std::vector> poly_; + public: + // getters, setters + std::vector> &poly() + { + return this->poly_; + } + + Obstacle(); }; /*! \brief RRT* algorithm basic class. @@ -134,204 +134,204 @@ class Obstacle { normal, 1 - uniform, 2 - uniform circle) */ class RRTS { - protected: - unsigned int icnt_ = 0; - std::chrono::high_resolution_clock::time_point tstart_; - double scnt_ = 0; - double pcnt_ = 0; - bool gf_ = false; - int sample_dist_type_ = 0; - - std::vector goals_; - std::vector nodes_; - std::vector obstacles_; - std::vector samples_; - std::vector steered_; - std::vector path_; - double log_path_time_ = 0.1; - unsigned int log_path_iter_ = 20; - - /*! \brief Update and return elapsed time. - */ - double elapsed(); - /*! \brief Log current path cost. - */ - void log_path_cost(); - /*! \brief Set normal distribution for sampling. - */ - void set_sample_normal( - double x1, double x2, - double y1, double y2, - double h1, double h2 - ); - /*! \brief Set uniform distribution for sampling. - */ - void set_sample_uniform( - double x1, double x2, - double y1, double y2, - double h1, double h2 - ); - /*! \brief Set uniform circle distribution for sampling. - */ - void set_sample_uniform_circle(); - RRTNode* use_nn; // Used for RRTExt12. - std::vector tmp_steered_; - bool finishit = false; - double path_cost_before_opt_ = 9999; - - BicycleCar bc; - /*! \brief Store RRT node to tree data structure. - */ - virtual void store_node(RRTNode n); - - // RRT procedures - std::tuple - collide(std::vector> &poly); - virtual std::tuple - collide_steered_from(RRTNode &f); - virtual std::tuple - collide_tmp_steered_from(RRTNode &f); - virtual std::tuple - collide_two_nodes(RRTNode &f, RRTNode &t); - void sample(); - std::default_random_engine gen_; - std::normal_distribution ndx_; - std::normal_distribution ndy_; - std::normal_distribution ndh_; - std::uniform_real_distribution udx_; - std::uniform_real_distribution udy_; - std::uniform_real_distribution udh_; - std::uniform_int_distribution udi1_; - std::uniform_int_distribution udi2_; - virtual RRTNode *nn(RRTNode &t); - virtual std::vector nv(RRTNode &t); - void steer(RRTNode &f, RRTNode &t); - void tmp_steer(RRTNode &f, RRTNode &t); - virtual void steer1(RRTNode &f, RRTNode &t); - virtual void steer2(RRTNode &f, RRTNode &t); - /*! \brief Join steered nodes to RRT data structure - - \param f RRT node to join steered nodes to. - */ - void join_steered(RRTNode *f); - void join_tmp_steered(RRTNode *f); - virtual bool goal_found(RRTNode &f); - // RRT* procedures - virtual bool connect(); - void rewire(); - public: - /// --- - std::vector log_opt_time_; - std::vector log_path_cost_; - struct { double x=0; double y=0; double b=0; double e=0; } entry; - bool entry_set = false; - struct { double x=0; double y=0; double h=0; } entry1; - struct { double x=0; double y=0; double h=0; } entry2; - bool entries_set = false; - std::vector steered1_; - std::vector steered2_; - /// --- - /*! \brief Initialize RRT algorithm if needed. - */ - virtual void init(); - virtual void reset(); - /*! \brief Deinitialize RRT algorithm if needed. - */ - virtual void deinit(); - /*! \brief Return path found by RRT*. - */ - virtual std::vector& path() - { - return this->path_; - } - virtual void compute_path(); - /*! \brief Return ``true`` if algorithm should stop. - - Update counters (iteration, seconds, ...) and return if - the current iteration should be the last one. - */ - bool should_stop(); - /*! \brief Return ``true`` if the algorithm should finish. - - Finish means that the algorithm will not be resumed. - */ - bool should_finish(); - /*! \brief Return ``true`` if the algorithm shoud break. - - Break means that the algorithm can be resumed. - */ - bool should_break(); - /*! \brief Return ``true`` if algorithm should continue. - - `pcnt_` is set to `scnt_`, so the difference is 0 and it can - start from scratch. After the `should_continue` is called, - there must be `while (rrts.next()) {}` loop. - */ - bool should_continue(); - /*! \brief Run next RRT* iteration. - */ - virtual bool next(); - /*! \brief Set sampling info. - - Based on `sample_dist_type`, set proper distribution - parameters. The distribution parameters are relative to `front` - node in `nodes` (init). - - For normal sampling: - \param x1 Mean x value. - \param x2 Standard deviation of x. - \param y1 Mean y value. - \param y2 Standard deviation of y. - \param h1 Mean h value. - \param h2 Standard deviation of h. - - For uniform sampling: - \param x1 Minimum x value. - \param x2 Maximum x value. - \param y1 Minimum y value. - \param y2 Maximum y value. - \param h1 Minimum h value. - \param h2 Maximum h value. - - For uniform circle sampling: - \param x1 Ignored. - \param x2 Ignored. - \param y1 Ignored. - \param y2 Ignored. - \param h1 Ignored. - \param h2 Ignored. - */ - void set_sample( - double x1, double x2, - double y1, double y2, - double h1, double h2 - ); - /*! \brief Generate JSON output. - */ - Json::Value json(); - /*! \brief Load JSON input. - */ - void json(Json::Value jvi); - - // RRT procedures - virtual double cost_build(RRTNode &f, RRTNode &t); - virtual double cost_search(RRTNode &f, RRTNode &t); - - // getters, setters - unsigned int icnt() const { return this->icnt_; } - void icnt(unsigned int i) { this->icnt_ = i; } - double scnt() const { return this->scnt_; } - bool gf() const { return this->gf_; } - void gf(bool f) { this->gf_ = f; } - int sample_dist_type() const { return this->sample_dist_type_;} - void sample_dist_type(int t) { this->sample_dist_type_ = t; } - std::vector &goals() { return this->goals_; } - std::vector &nodes() { return this->nodes_; } - std::vector &obstacles() { return this->obstacles_; } - std::vector &samples() { return this->samples_; } - std::vector &steered() { return this->steered_; } - - RRTS(); + protected: + unsigned int icnt_ = 0; + std::chrono::high_resolution_clock::time_point tstart_; + double scnt_ = 0; + double pcnt_ = 0; + bool gf_ = false; + int sample_dist_type_ = 0; + + std::vector goals_; + std::vector nodes_; + std::vector obstacles_; + std::vector samples_; + std::vector steered_; + std::vector path_; + double log_path_time_ = 0.1; + unsigned int log_path_iter_ = 20; + + /*! \brief Update and return elapsed time. + */ + double elapsed(); + /*! \brief Log current path cost. + */ + void log_path_cost(); + /*! \brief Set normal distribution for sampling. + */ + void set_sample_normal( + double x1, double x2, + double y1, double y2, + double h1, double h2 + ); + /*! \brief Set uniform distribution for sampling. + */ + void set_sample_uniform( + double x1, double x2, + double y1, double y2, + double h1, double h2 + ); + /*! \brief Set uniform circle distribution for sampling. + */ + void set_sample_uniform_circle(); + RRTNode* use_nn; // Used for RRTExt12. + std::vector tmp_steered_; + bool finishit = false; + double path_cost_before_opt_ = 9999; + + BicycleCar bc; + /*! \brief Store RRT node to tree data structure. + */ + virtual void store_node(RRTNode n); + + // RRT procedures + std::tuple + collide(std::vector> &poly); + virtual std::tuple + collide_steered_from(RRTNode &f); + virtual std::tuple + collide_tmp_steered_from(RRTNode &f); + virtual std::tuple + collide_two_nodes(RRTNode &f, RRTNode &t); + void sample(); + std::default_random_engine gen_; + std::normal_distribution ndx_; + std::normal_distribution ndy_; + std::normal_distribution ndh_; + std::uniform_real_distribution udx_; + std::uniform_real_distribution udy_; + std::uniform_real_distribution udh_; + std::uniform_int_distribution udi1_; + std::uniform_int_distribution udi2_; + virtual RRTNode *nn(RRTNode &t); + virtual std::vector nv(RRTNode &t); + void steer(RRTNode &f, RRTNode &t); + void tmp_steer(RRTNode &f, RRTNode &t); + virtual void steer1(RRTNode &f, RRTNode &t); + virtual void steer2(RRTNode &f, RRTNode &t); + /*! \brief Join steered nodes to RRT data structure + + \param f RRT node to join steered nodes to. + */ + void join_steered(RRTNode *f); + void join_tmp_steered(RRTNode *f); + virtual bool goal_found(RRTNode &f); + // RRT* procedures + virtual bool connect(); + void rewire(); + public: + /// --- + std::vector log_opt_time_; + std::vector log_path_cost_; + struct { double x=0; double y=0; double b=0; double e=0; } entry; + bool entry_set = false; + struct { double x=0; double y=0; double h=0; } entry1; + struct { double x=0; double y=0; double h=0; } entry2; + bool entries_set = false; + std::vector steered1_; + std::vector steered2_; + /// --- + /*! \brief Initialize RRT algorithm if needed. + */ + virtual void init(); + virtual void reset(); + /*! \brief Deinitialize RRT algorithm if needed. + */ + virtual void deinit(); + /*! \brief Return path found by RRT*. + */ + virtual std::vector& path() + { + return this->path_; + } + virtual void compute_path(); + /*! \brief Return ``true`` if algorithm should stop. + + Update counters (iteration, seconds, ...) and return if + the current iteration should be the last one. + */ + bool should_stop(); + /*! \brief Return ``true`` if the algorithm should finish. + + Finish means that the algorithm will not be resumed. + */ + bool should_finish(); + /*! \brief Return ``true`` if the algorithm shoud break. + + Break means that the algorithm can be resumed. + */ + bool should_break(); + /*! \brief Return ``true`` if algorithm should continue. + + `pcnt_` is set to `scnt_`, so the difference is 0 and it can + start from scratch. After the `should_continue` is called, + there must be `while (rrts.next()) {}` loop. + */ + bool should_continue(); + /*! \brief Run next RRT* iteration. + */ + virtual bool next(); + /*! \brief Set sampling info. + + Based on `sample_dist_type`, set proper distribution + parameters. The distribution parameters are relative to `front` + node in `nodes` (init). + + For normal sampling: + \param x1 Mean x value. + \param x2 Standard deviation of x. + \param y1 Mean y value. + \param y2 Standard deviation of y. + \param h1 Mean h value. + \param h2 Standard deviation of h. + + For uniform sampling: + \param x1 Minimum x value. + \param x2 Maximum x value. + \param y1 Minimum y value. + \param y2 Maximum y value. + \param h1 Minimum h value. + \param h2 Maximum h value. + + For uniform circle sampling: + \param x1 Ignored. + \param x2 Ignored. + \param y1 Ignored. + \param y2 Ignored. + \param h1 Ignored. + \param h2 Ignored. + */ + void set_sample( + double x1, double x2, + double y1, double y2, + double h1, double h2 + ); + /*! \brief Generate JSON output. + */ + Json::Value json(); + /*! \brief Load JSON input. + */ + void json(Json::Value jvi); + + // RRT procedures + virtual double cost_build(RRTNode &f, RRTNode &t); + virtual double cost_search(RRTNode &f, RRTNode &t); + + // getters, setters + unsigned int icnt() const { return this->icnt_; } + void icnt(unsigned int i) { this->icnt_ = i; } + double scnt() const { return this->scnt_; } + bool gf() const { return this->gf_; } + void gf(bool f) { this->gf_ = f; } + int sample_dist_type() const { return this->sample_dist_type_;} + void sample_dist_type(int t) { this->sample_dist_type_ = t; } + std::vector &goals() { return this->goals_; } + std::vector &nodes() { return this->nodes_; } + std::vector &obstacles() { return this->obstacles_; } + std::vector &samples() { return this->samples_; } + std::vector &steered() { return this->steered_; } + + RRTS(); }; /*! \brief Compute cumulative cost of RRT node. diff --git a/src/rrtext1.cc b/src/rrtext1.cc index bb55ac9..bdc43fd 100644 --- a/src/rrtext1.cc +++ b/src/rrtext1.cc @@ -3,21 +3,21 @@ double RRTExt1::cost_build(RRTNode &f, RRTNode &t) { - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - return rsss.distance(q0, q1); + double q0[] = {f.x(), f.y(), f.h()}; + double q1[] = {t.x(), t.y(), t.h()}; + ReedsSheppStateSpace rsss(this->bc.mtr()); + return rsss.distance(q0, q1); } double RRTExt1::cost_search(RRTNode &f, RRTNode &t) { - double cost = 0; - cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); - double heur = std::min( - std::abs(t.h() - f.h()), - 2 * M_PI - std::abs(t.h() - f.h()) - ); - heur *= this->bc.mtr(); - cost = std::max(cost, heur); - return cost; + double cost = 0; + cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); + double heur = std::min( + std::abs(t.h() - f.h()), + 2 * M_PI - std::abs(t.h() - f.h()) + ); + heur *= this->bc.mtr(); + cost = std::max(cost, heur); + return cost; } diff --git a/src/rrtext10.cc b/src/rrtext10.cc index c5b5602..cdb39b8 100644 --- a/src/rrtext10.cc +++ b/src/rrtext10.cc @@ -3,16 +3,16 @@ double RRTExt10::cost_build(RRTNode &f, RRTNode &t) { - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - return rsss.distance(q0, q1); + double q0[] = {f.x(), f.y(), f.h()}; + double q1[] = {t.x(), t.y(), t.h()}; + ReedsSheppStateSpace rsss(this->bc.mtr()); + return rsss.distance(q0, q1); } double RRTExt10::cost_search(RRTNode &f, RRTNode &t) { - double cost = 0; - cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); - cost += std::abs(t.h() - f.h()); - return cost; + double cost = 0; + cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); + cost += std::abs(t.h() - f.h()); + return cost; } diff --git a/src/rrtext11.cc b/src/rrtext11.cc index 76f790a..f349cc3 100644 --- a/src/rrtext11.cc +++ b/src/rrtext11.cc @@ -2,35 +2,35 @@ bool RRTExt11::goal_found(RRTNode &f) { - auto &g = this->goals().front(); - auto fbc = BicycleCar(); - fbc.x(f.x()); - fbc.y(f.y()); - fbc.h(f.h()); - auto gbc = BicycleCar(); - gbc.x(g.x()); - gbc.y(g.y()); - gbc.h(g.h()); - bool drivable = false; - if (this->entry_set) - drivable = gbc.drivable(fbc, this->entry.b, this->entry.e); - else - drivable = gbc.drivable(fbc); - if (drivable) { - this->steer(f, g); - if (std::get<0>(this->collide_steered_from(f))) - return false; - double cost = this->cost_build(f, g); - this->join_steered(&f); - if (g.p() == nullptr) { - g.p(&f); - g.c(cost); - this->path_cost_before_opt_ = g.cc; - } else if (f.cc + cost < g.cc) { - g.p(&f); - g.c(cost); - } - return true; - } - return false; + auto &g = this->goals().front(); + auto fbc = BicycleCar(); + fbc.x(f.x()); + fbc.y(f.y()); + fbc.h(f.h()); + auto gbc = BicycleCar(); + gbc.x(g.x()); + gbc.y(g.y()); + gbc.h(g.h()); + bool drivable = false; + if (this->entry_set) + drivable = gbc.drivable(fbc, this->entry.b, this->entry.e); + else + drivable = gbc.drivable(fbc); + if (drivable) { + this->steer(f, g); + if (std::get<0>(this->collide_steered_from(f))) + return false; + double cost = this->cost_build(f, g); + this->join_steered(&f); + if (g.p() == nullptr) { + g.p(&f); + g.c(cost); + this->path_cost_before_opt_ = g.cc; + } else if (f.cc + cost < g.cc) { + g.p(&f); + g.c(cost); + } + return true; + } + return false; } diff --git a/src/rrtext12.cc b/src/rrtext12.cc index 4dacd10..bb6babf 100644 --- a/src/rrtext12.cc +++ b/src/rrtext12.cc @@ -2,132 +2,132 @@ void RRTExt12::steer1(RRTNode &f, RRTNode &t) { - // assume that `t` is circle uniformly sampled - auto bc = BicycleCar(); - bc.x(f.x()); - bc.y(f.y()); - bc.h(f.h()); - bc.set_max_steer(); - auto max_steer = bc.st(); - // map t.h() in -PI...+PI to -max_steer...+max_steer - bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer); - bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5); - this->steered().clear(); - while (true) { - bc.next(); - auto n = RRTNode(bc); - if ( - this->steered().size() == 0 - && std::get<0>(this->collide_two_nodes(f, n)) - ) { - return; - } else if ( - ( - this->steered().size() > 0 - && std::get<0>(this->collide_two_nodes( - this->steered().back(), - n - )) - ) - || this->steered().size() > 25 - ) { - break; - } - this->steered().push_back(RRTNode(bc)); - } - bc.sp(-1*bc.sp()); - bc.next(); - bc.sp(-1*bc.sp()); - this->samples().back().x(bc.x()); - this->samples().back().y(bc.y()); - this->samples().back().h(bc.h()); + // assume that `t` is circle uniformly sampled + auto bc = BicycleCar(); + bc.x(f.x()); + bc.y(f.y()); + bc.h(f.h()); + bc.set_max_steer(); + auto max_steer = bc.st(); + // map t.h() in -PI...+PI to -max_steer...+max_steer + bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer); + bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5); + this->steered().clear(); + while (true) { + bc.next(); + auto n = RRTNode(bc); + if ( + this->steered().size() == 0 + && std::get<0>(this->collide_two_nodes(f, n)) + ) { + return; + } else if ( + ( + this->steered().size() > 0 + && std::get<0>(this->collide_two_nodes( + this->steered().back(), + n + )) + ) + || this->steered().size() > 25 + ) { + break; + } + this->steered().push_back(RRTNode(bc)); + } + bc.sp(-1*bc.sp()); + bc.next(); + bc.sp(-1*bc.sp()); + this->samples().back().x(bc.x()); + this->samples().back().y(bc.y()); + this->samples().back().h(bc.h()); } bool RRTExt12::connect() { - RRTNode *t = &this->steered().front(); - RRTNode *f = this->use_nn; - double cost = this->cost_search(*f, *t); - // steer from f->t and then continue with the steered. - this->tmp_steer(*f, *t); - if (this->tmp_steered_.size() > 0) { - auto col = this->collide_tmp_steered_from(*f); - if (std::get<0>(col)) - return false; - this->join_tmp_steered(f); - f = &this->nodes().back(); - } - if (sqrt( - pow(f->x() - t->x(), 2) - + pow(f->y() - t->y(), 2) - ) > 0.5) - return false; - // cont. - this->store_node(this->steered().front()); - t = &this->nodes().back(); - t->p(f); - t->c(this->cost_build(*f, *t)); - t->set_t(RRTNodeType::connected); - return true; + RRTNode *t = &this->steered().front(); + RRTNode *f = this->use_nn; + double cost = this->cost_search(*f, *t); + // steer from f->t and then continue with the steered. + this->tmp_steer(*f, *t); + if (this->tmp_steered_.size() > 0) { + auto col = this->collide_tmp_steered_from(*f); + if (std::get<0>(col)) + return false; + this->join_tmp_steered(f); + f = &this->nodes().back(); + } + if (sqrt( + pow(f->x() - t->x(), 2) + + pow(f->y() - t->y(), 2) + ) > 0.5) + return false; + // cont. + this->store_node(this->steered().front()); + t = &this->nodes().back(); + t->p(f); + t->c(this->cost_build(*f, *t)); + t->set_t(RRTNodeType::connected); + return true; } bool RRTExt12::next() { - if (this->icnt_ == 0) - this->tstart_ = std::chrono::high_resolution_clock::now(); - bool next = true; - if (this->icnt_ > this->log_path_iter_) - this->log_path_cost(); - if (this->should_stop()) - return false; - if (this->samples().size() == 0) { - this->samples().push_back(RRTNode()); - this->samples().back().x(this->goals().front().x()); - this->samples().back().y(this->goals().front().y()); - this->samples().back().h(this->goals().front().h()); - this->use_nn = &this->nodes().front(); - } else { - this->sample(); - } - this->steer1( - *this->use_nn, - this->samples().back() - ); - if (this->steered().size() == 0) - return next; - auto col = this->collide_steered_from(*this->use_nn); - if (std::get<0>(col)) { - auto rcnt = this->steered().size() - std::get<1>(col); - while (rcnt-- > 0) { - this->steered().pop_back(); - } - } - if (!this->connect()) - return next; - this->rewire(); - unsigned scnt = this->steered().size(); - this->join_steered(&this->nodes().back()); - RRTNode *just_added = &this->nodes().back(); - while (scnt > 0) { - // store all the steered1 nodes - this->steered1_.push_back(just_added); - scnt--; - auto &g = this->goals().front(); - this->steer2(*just_added, g); - auto col = this->collide_steered_from(*just_added); - if (std::get<0>(col)) { - auto rcnt = this->steered().size() - std::get<1>(col); - while (rcnt-- > 0) { - this->steered().pop_back(); - } - } - this->join_steered(just_added); - // store all the steered2 nodes - RRTNode* jap = &this->nodes().back(); - while (jap != just_added) { - this->steered2_.push_back(jap); - jap = jap->p(); - } - this->gf(this->goal_found(this->nodes().back())); - just_added = just_added->p(); - } - return next; + if (this->icnt_ == 0) + this->tstart_ = std::chrono::high_resolution_clock::now(); + bool next = true; + if (this->icnt_ > this->log_path_iter_) + this->log_path_cost(); + if (this->should_stop()) + return false; + if (this->samples().size() == 0) { + this->samples().push_back(RRTNode()); + this->samples().back().x(this->goals().front().x()); + this->samples().back().y(this->goals().front().y()); + this->samples().back().h(this->goals().front().h()); + this->use_nn = &this->nodes().front(); + } else { + this->sample(); + } + this->steer1( + *this->use_nn, + this->samples().back() + ); + if (this->steered().size() == 0) + return next; + auto col = this->collide_steered_from(*this->use_nn); + if (std::get<0>(col)) { + auto rcnt = this->steered().size() - std::get<1>(col); + while (rcnt-- > 0) { + this->steered().pop_back(); + } + } + if (!this->connect()) + return next; + this->rewire(); + unsigned scnt = this->steered().size(); + this->join_steered(&this->nodes().back()); + RRTNode *just_added = &this->nodes().back(); + while (scnt > 0) { + // store all the steered1 nodes + this->steered1_.push_back(just_added); + scnt--; + auto &g = this->goals().front(); + this->steer2(*just_added, g); + auto col = this->collide_steered_from(*just_added); + if (std::get<0>(col)) { + auto rcnt = this->steered().size() - std::get<1>(col); + while (rcnt-- > 0) { + this->steered().pop_back(); + } + } + this->join_steered(just_added); + // store all the steered2 nodes + RRTNode* jap = &this->nodes().back(); + while (jap != just_added) { + this->steered2_.push_back(jap); + jap = jap->p(); + } + this->gf(this->goal_found(this->nodes().back())); + just_added = just_added->p(); + } + return next; } diff --git a/src/rrtext13.cc b/src/rrtext13.cc index 563432e..172ef6a 100644 --- a/src/rrtext13.cc +++ b/src/rrtext13.cc @@ -3,312 +3,312 @@ void RRTExt13::reset() { - RRTS::reset(); - this->orig_path().clear(); - this->first_optimized_path().clear(); - this->orig_path_cost_ = 9999.9; - this->first_optimized_path_cost_ = 9999.9; + RRTS::reset(); + this->orig_path().clear(); + this->first_optimized_path().clear(); + this->orig_path_cost_ = 9999.9; + this->first_optimized_path_cost_ = 9999.9; } void RRTExt13::first_path_optimization() { - if (this->orig_path().size() == 0) { - this->orig_path_ = RRTS::path(); - if (this->orig_path().size() == 0) - return; - else - this->orig_path_cost(this->orig_path().back()->cc); - } - class DijkstraNode : public RRTNode { - public: - DijkstraNode *s = nullptr; - RRTNode *n = nullptr; - unsigned int i = 0; - double cc = 9999; - bool v = false; - bool vi() - { - if (this->v) - return true; - this->v = true; - return false; - } - DijkstraNode(const RRTNode& n) { - this->x(n.x()); - this->y(n.y()); - this->h(n.h()); - this->sp(n.sp()); - this->st(n.st()); - } - // override - DijkstraNode *p_ = nullptr; - DijkstraNode *p() const { return this->p_; } - void p(DijkstraNode *p) { this->p_ = p; } - }; - class DijkstraNodeComparator { - public: - int operator() ( - const DijkstraNode &n1, - const DijkstraNode &n2 - ) - { - return n1.cc > n2.cc; - } - }; - std::vector dn; - unsigned int ops = this->orig_path().size(); - auto op = this->orig_path(); - dn.reserve(ops); - unsigned int dncnt = 0; - dn.push_back(DijkstraNode(*this->orig_path().front())); - dn.back().cc = this->orig_path().front()->cc; - dn.back().s = &dn.back(); - dn.back().n = this->orig_path().front(); - dn.back().i = dncnt++; - for (unsigned int i = 0; i < ops - 1; i++) { - auto ibc = BicycleCar(); - ibc.x(op[i]->x()); - ibc.y(op[i]->y()); - ibc.h(op[i]->h()); - for (unsigned int j = i + 1; j < ops; j++) { - auto jbc = BicycleCar(); - jbc.x(op[j]->x()); - jbc.y(op[j]->y()); - jbc.h(op[j]->h()); - if (!jbc.drivable(ibc)) { - dn.push_back(DijkstraNode(*op[j-1])); - dn.back().cc = op[j-1]->cc; - dn.back().s = &dn.back(); - dn.back().n = op[j-1]; - dn.back().i = dncnt++; - i = j; - break; - } - } - } - dn.push_back(DijkstraNode(*this->orig_path().back())); - dn.back().cc = this->orig_path().back()->cc; - dn.back().s = &dn.back(); - dn.back().n = this->orig_path().back(); - dn.back().i = dncnt++; - std::priority_queue< - DijkstraNode, - std::vector, - DijkstraNodeComparator - > pq; - dn.front().vi(); - pq.push(dn.front()); - while (!pq.empty()) { - DijkstraNode f = pq.top(); - pq.pop(); - for (unsigned int i = f.i + 1; i < dncnt; i++) { - double cost = f.cc + this->cost_search(f, dn[i]); - this->steer(f, dn[i]); - if (this->steered().size() == 0) - break; // TODO why not continue? - if (std::get<0>(this->collide_steered_from(f))) - continue; - if (cost < dn[i].cc) { - dn[i].cc = cost; - dn[i].p(f.s); - if (!dn[i].vi()) - pq.push(dn[i]); - } - } - } - DijkstraNode *ln = nullptr; - for (auto &n: dn) { - if (n.v) - ln = &n; - } - if (ln == nullptr || ln->p() == nullptr) - return; - while (ln->p() != nullptr) { - RRTNode *t = ln->n; - RRTNode *f = ln->p()->n; - this->steer(*f, *t); - if (std::get<0>(this->collide_steered_from(*f))) - return; - this->join_steered(f); - t->p(&this->nodes().back()); - t->c(this->cost_build(this->nodes().back(), *t)); - ln = ln->p(); - } - RRTS::compute_path(); + if (this->orig_path().size() == 0) { + this->orig_path_ = RRTS::path(); + if (this->orig_path().size() == 0) + return; + else + this->orig_path_cost(this->orig_path().back()->cc); + } + class DijkstraNode : public RRTNode { + public: + DijkstraNode *s = nullptr; + RRTNode *n = nullptr; + unsigned int i = 0; + double cc = 9999; + bool v = false; + bool vi() + { + if (this->v) + return true; + this->v = true; + return false; + } + DijkstraNode(const RRTNode& n) { + this->x(n.x()); + this->y(n.y()); + this->h(n.h()); + this->sp(n.sp()); + this->st(n.st()); + } + // override + DijkstraNode *p_ = nullptr; + DijkstraNode *p() const { return this->p_; } + void p(DijkstraNode *p) { this->p_ = p; } + }; + class DijkstraNodeComparator { + public: + int operator() ( + const DijkstraNode &n1, + const DijkstraNode &n2 + ) + { + return n1.cc > n2.cc; + } + }; + std::vector dn; + unsigned int ops = this->orig_path().size(); + auto op = this->orig_path(); + dn.reserve(ops); + unsigned int dncnt = 0; + dn.push_back(DijkstraNode(*this->orig_path().front())); + dn.back().cc = this->orig_path().front()->cc; + dn.back().s = &dn.back(); + dn.back().n = this->orig_path().front(); + dn.back().i = dncnt++; + for (unsigned int i = 0; i < ops - 1; i++) { + auto ibc = BicycleCar(); + ibc.x(op[i]->x()); + ibc.y(op[i]->y()); + ibc.h(op[i]->h()); + for (unsigned int j = i + 1; j < ops; j++) { + auto jbc = BicycleCar(); + jbc.x(op[j]->x()); + jbc.y(op[j]->y()); + jbc.h(op[j]->h()); + if (!jbc.drivable(ibc)) { + dn.push_back(DijkstraNode(*op[j-1])); + dn.back().cc = op[j-1]->cc; + dn.back().s = &dn.back(); + dn.back().n = op[j-1]; + dn.back().i = dncnt++; + i = j; + break; + } + } + } + dn.push_back(DijkstraNode(*this->orig_path().back())); + dn.back().cc = this->orig_path().back()->cc; + dn.back().s = &dn.back(); + dn.back().n = this->orig_path().back(); + dn.back().i = dncnt++; + std::priority_queue< + DijkstraNode, + std::vector, + DijkstraNodeComparator + > pq; + dn.front().vi(); + pq.push(dn.front()); + while (!pq.empty()) { + DijkstraNode f = pq.top(); + pq.pop(); + for (unsigned int i = f.i + 1; i < dncnt; i++) { + double cost = f.cc + this->cost_search(f, dn[i]); + this->steer(f, dn[i]); + if (this->steered().size() == 0) + break; // TODO why not continue? + if (std::get<0>(this->collide_steered_from(f))) + continue; + if (cost < dn[i].cc) { + dn[i].cc = cost; + dn[i].p(f.s); + if (!dn[i].vi()) + pq.push(dn[i]); + } + } + } + DijkstraNode *ln = nullptr; + for (auto &n: dn) { + if (n.v) + ln = &n; + } + if (ln == nullptr || ln->p() == nullptr) + return; + while (ln->p() != nullptr) { + RRTNode *t = ln->n; + RRTNode *f = ln->p()->n; + this->steer(*f, *t); + if (std::get<0>(this->collide_steered_from(*f))) + return; + this->join_steered(f); + t->p(&this->nodes().back()); + t->c(this->cost_build(this->nodes().back(), *t)); + ln = ln->p(); + } + RRTS::compute_path(); } void RRTExt13::second_path_optimization() { - if (this->first_optimized_path().size() == 0) { - return; - } - class DijkstraNode : public RRTNode { - public: - DijkstraNode *s = nullptr; - RRTNode *n = nullptr; - unsigned int i = 0; - double cc = 9999; - bool v = false; - bool vi() - { - if (this->v) - return true; - this->v = true; - return false; - } - DijkstraNode(const RRTNode& n) { - this->x(n.x()); - this->y(n.y()); - this->h(n.h()); - this->sp(n.sp()); - this->st(n.st()); - } - // override - DijkstraNode *p_ = nullptr; - DijkstraNode *p() const { return this->p_; } - void p(DijkstraNode *p) { this->p_ = p; } - }; - class DijkstraNodeComparator { - public: - int operator() ( - const DijkstraNode &n1, - const DijkstraNode &n2 - ) - { - return n1.cc > n2.cc; - } - }; - std::vector dn; - unsigned int ops = this->orig_path().size(); - auto op = this->orig_path(); - dn.reserve(ops); - unsigned int dncnt = 0; - dn.push_back(DijkstraNode(*this->orig_path().front())); - dn.back().cc = this->orig_path().front()->cc; - dn.back().s = &dn.back(); - dn.back().n = this->orig_path().front(); - dn.back().i = dncnt++; - for (unsigned int i = ops - 1; i > 1; i--) { - auto ibc = BicycleCar(); - ibc.x(op[i]->x()); - ibc.y(op[i]->y()); - ibc.h(op[i]->h()); - for (unsigned int j = i - 1; j > 0; j--) { - auto jbc = BicycleCar(); - jbc.x(op[j]->x()); - jbc.y(op[j]->y()); - jbc.h(op[j]->h()); - if (!jbc.drivable(ibc)) { - dn.push_back(DijkstraNode(*op[j-1])); - dn.back().cc = op[j-1]->cc; - dn.back().s = &dn.back(); - dn.back().n = op[j-1]; - dn.back().i = dncnt++; - i = j; - break; - } - } - } - dn.push_back(DijkstraNode(*this->orig_path().back())); - dn.back().cc = this->orig_path().back()->cc; - dn.back().s = &dn.back(); - dn.back().n = this->orig_path().back(); - dn.back().i = dncnt++; - std::priority_queue< - DijkstraNode, - std::vector, - DijkstraNodeComparator - > pq; - // TODO rewrite - dn.back().vi(); - pq.push(dn.back()); - while (!pq.empty()) { - DijkstraNode t = pq.top(); - pq.pop(); - for (unsigned int i = t.i - 1; i > 0; i--) { - double cost = dn[i].cc + this->cost_search(dn[i], t); - this->steer(dn[i], t); - if (this->steered().size() == 0) - break; // TODO why not continue? - if (std::get<0>(this->collide_steered_from(dn[i]))) - continue; - if (cost < t.cc) { - t.cc = cost; - t.p(dn[i].s); - if (!dn[i].vi()) - pq.push(dn[i]); - } - } - } - DijkstraNode *fn = nullptr; - for (auto &n: dn) { - if (n.v) { - fn = &n; - break; - } - } - if (fn == nullptr || fn->p() == nullptr) - return; - DijkstraNode *ln = &dn.back(); - while (ln->p() != fn) { - RRTNode *t = ln->n; - RRTNode *f = ln->p()->n; - this->steer(*f, *t); - if (std::get<0>(this->collide_steered_from(*f))) - return; - this->join_steered(f); - t->p(&this->nodes().back()); - t->c(this->cost_build(this->nodes().back(), *t)); - ln = ln->p(); - } - RRTS::compute_path(); + if (this->first_optimized_path().size() == 0) { + return; + } + class DijkstraNode : public RRTNode { + public: + DijkstraNode *s = nullptr; + RRTNode *n = nullptr; + unsigned int i = 0; + double cc = 9999; + bool v = false; + bool vi() + { + if (this->v) + return true; + this->v = true; + return false; + } + DijkstraNode(const RRTNode& n) { + this->x(n.x()); + this->y(n.y()); + this->h(n.h()); + this->sp(n.sp()); + this->st(n.st()); + } + // override + DijkstraNode *p_ = nullptr; + DijkstraNode *p() const { return this->p_; } + void p(DijkstraNode *p) { this->p_ = p; } + }; + class DijkstraNodeComparator { + public: + int operator() ( + const DijkstraNode &n1, + const DijkstraNode &n2 + ) + { + return n1.cc > n2.cc; + } + }; + std::vector dn; + unsigned int ops = this->orig_path().size(); + auto op = this->orig_path(); + dn.reserve(ops); + unsigned int dncnt = 0; + dn.push_back(DijkstraNode(*this->orig_path().front())); + dn.back().cc = this->orig_path().front()->cc; + dn.back().s = &dn.back(); + dn.back().n = this->orig_path().front(); + dn.back().i = dncnt++; + for (unsigned int i = ops - 1; i > 1; i--) { + auto ibc = BicycleCar(); + ibc.x(op[i]->x()); + ibc.y(op[i]->y()); + ibc.h(op[i]->h()); + for (unsigned int j = i - 1; j > 0; j--) { + auto jbc = BicycleCar(); + jbc.x(op[j]->x()); + jbc.y(op[j]->y()); + jbc.h(op[j]->h()); + if (!jbc.drivable(ibc)) { + dn.push_back(DijkstraNode(*op[j-1])); + dn.back().cc = op[j-1]->cc; + dn.back().s = &dn.back(); + dn.back().n = op[j-1]; + dn.back().i = dncnt++; + i = j; + break; + } + } + } + dn.push_back(DijkstraNode(*this->orig_path().back())); + dn.back().cc = this->orig_path().back()->cc; + dn.back().s = &dn.back(); + dn.back().n = this->orig_path().back(); + dn.back().i = dncnt++; + std::priority_queue< + DijkstraNode, + std::vector, + DijkstraNodeComparator + > pq; + // TODO rewrite + dn.back().vi(); + pq.push(dn.back()); + while (!pq.empty()) { + DijkstraNode t = pq.top(); + pq.pop(); + for (unsigned int i = t.i - 1; i > 0; i--) { + double cost = dn[i].cc + this->cost_search(dn[i], t); + this->steer(dn[i], t); + if (this->steered().size() == 0) + break; // TODO why not continue? + if (std::get<0>(this->collide_steered_from(dn[i]))) + continue; + if (cost < t.cc) { + t.cc = cost; + t.p(dn[i].s); + if (!dn[i].vi()) + pq.push(dn[i]); + } + } + } + DijkstraNode *fn = nullptr; + for (auto &n: dn) { + if (n.v) { + fn = &n; + break; + } + } + if (fn == nullptr || fn->p() == nullptr) + return; + DijkstraNode *ln = &dn.back(); + while (ln->p() != fn) { + RRTNode *t = ln->n; + RRTNode *f = ln->p()->n; + this->steer(*f, *t); + if (std::get<0>(this->collide_steered_from(*f))) + return; + this->join_steered(f); + t->p(&this->nodes().back()); + t->c(this->cost_build(this->nodes().back(), *t)); + ln = ln->p(); + } + RRTS::compute_path(); } void RRTExt13::compute_path() { - RRTS::compute_path(); - auto tstart = std::chrono::high_resolution_clock::now(); - this->first_path_optimization(); - this->first_optimized_path_ = RRTS::path(); - if (this->first_optimized_path_.size() > 0) { - this->first_optimized_path_cost( - this->first_optimized_path_.back()->cc - ); - } else { - return; - } - this->second_path_optimization(); - auto tend = std::chrono::high_resolution_clock::now(); - auto tdiff = std::chrono::duration_cast>( - tend - tstart); - this->log_opt_time_.push_back(tdiff.count()); + RRTS::compute_path(); + auto tstart = std::chrono::high_resolution_clock::now(); + this->first_path_optimization(); + this->first_optimized_path_ = RRTS::path(); + if (this->first_optimized_path_.size() > 0) { + this->first_optimized_path_cost( + this->first_optimized_path_.back()->cc + ); + } else { + return; + } + this->second_path_optimization(); + auto tend = std::chrono::high_resolution_clock::now(); + auto tdiff = std::chrono::duration_cast>( + tend - tstart); + this->log_opt_time_.push_back(tdiff.count()); } Json::Value RRTExt13::json() { - Json::Value jvo = RRTS::json(); - jvo["orig_path_cost"] = this->orig_path_cost(); - { - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: this->orig_path()) { - jvo["orig_path"][pcnt][0] = n->x(); - jvo["orig_path"][pcnt][1] = n->y(); - jvo["orig_path"][pcnt][2] = n->h(); - if (n->t(RRTNodeType::cusp)) - cu++; - if (n->t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["orig_cusps-in-path"] = cu; - jvo["orig_connecteds-in-path"] = co; - } - return jvo; + Json::Value jvo = RRTS::json(); + jvo["orig_path_cost"] = this->orig_path_cost(); + { + unsigned int cu = 0; + unsigned int co = 0; + unsigned int pcnt = 0; + for (auto n: this->orig_path()) { + jvo["orig_path"][pcnt][0] = n->x(); + jvo["orig_path"][pcnt][1] = n->y(); + jvo["orig_path"][pcnt][2] = n->h(); + if (n->t(RRTNodeType::cusp)) + cu++; + if (n->t(RRTNodeType::connected)) + co++; + pcnt++; + } + jvo["orig_cusps-in-path"] = cu; + jvo["orig_connecteds-in-path"] = co; + } + return jvo; } void RRTExt13::json(Json::Value jvi) { - return RRTS::json(jvi); + return RRTS::json(jvi); } diff --git a/src/rrtext2.cc b/src/rrtext2.cc index 1b74169..77ac2e5 100644 --- a/src/rrtext2.cc +++ b/src/rrtext2.cc @@ -4,134 +4,134 @@ void RRTExt2::init() { - BicycleCar bc; - this->c2_bc().count = 4; - this->c2_bc().verts[0].x = bc.lfx(); - this->c2_bc().verts[0].y = bc.lfy(); - this->c2_bc().verts[1].x = bc.lrx(); - this->c2_bc().verts[1].y = bc.lry(); - this->c2_bc().verts[2].x = bc.rrx(); - this->c2_bc().verts[2].y = bc.rry(); - this->c2_bc().verts[3].x = bc.rfx(); - this->c2_bc().verts[3].y = bc.rfy(); + BicycleCar bc; + this->c2_bc().count = 4; + this->c2_bc().verts[0].x = bc.lfx(); + this->c2_bc().verts[0].y = bc.lfy(); + this->c2_bc().verts[1].x = bc.lrx(); + this->c2_bc().verts[1].y = bc.lry(); + this->c2_bc().verts[2].x = bc.rrx(); + this->c2_bc().verts[2].y = bc.rry(); + this->c2_bc().verts[3].x = bc.rfx(); + this->c2_bc().verts[3].y = bc.rfy(); - for (auto o: this->obstacles()) { - c2Poly c2tmp; - c2tmp.count = (o.poly().size() < C2_MAX_POLYGON_VERTS) - ? o.poly().size() - : C2_MAX_POLYGON_VERTS - ; - int i = 0; - for (auto c: o.poly()) { - if (i < C2_MAX_POLYGON_VERTS) { - c2tmp.verts[i].x = std::get<0>(c); - c2tmp.verts[i].y = std::get<1>(c); - } - i++; - } - this->c2_obstacles().push_back(c2tmp); - } + for (auto o: this->obstacles()) { + c2Poly c2tmp; + c2tmp.count = (o.poly().size() < C2_MAX_POLYGON_VERTS) + ? o.poly().size() + : C2_MAX_POLYGON_VERTS + ; + int i = 0; + for (auto c: o.poly()) { + if (i < C2_MAX_POLYGON_VERTS) { + c2tmp.verts[i].x = std::get<0>(c); + c2tmp.verts[i].y = std::get<1>(c); + } + i++; + } + this->c2_obstacles().push_back(c2tmp); + } } void RRTExt2::deinit() { - this->c2_obstacles().clear(); - RRTS::deinit(); + this->c2_obstacles().clear(); + RRTS::deinit(); } std::tuple RRTExt2::collide_steered_from(RRTNode &f) { - this->c2x_bc().p.x = f.x(); - this->c2x_bc().p.y = f.y(); - this->c2x_bc().r.c = cos(f.h()); - this->c2x_bc().r.s = sin(f.h()); - for (auto &o: this->c2_obstacles()) { - if (c2PolytoPoly( - &this->c2_bc(), &this->c2x_bc(), - &o, NULL - )) { - return std::make_tuple(true, 0, 0); - } - } - unsigned int i = 0; - for (auto &n: this->steered()) { - this->c2x_bc().p.x = n.x(); - this->c2x_bc().p.y = n.y(); - this->c2x_bc().r.c = cos(n.h()); - this->c2x_bc().r.s = sin(n.h()); - for (auto &o: this->c2_obstacles()) { - if (c2PolytoPoly( - &this->c2_bc(), &this->c2x_bc(), - &o, NULL - )) { - return std::make_tuple(true, i, 0); - } - } - i++; - } - return std::make_tuple(false, 0, 0); + this->c2x_bc().p.x = f.x(); + this->c2x_bc().p.y = f.y(); + this->c2x_bc().r.c = cos(f.h()); + this->c2x_bc().r.s = sin(f.h()); + for (auto &o: this->c2_obstacles()) { + if (c2PolytoPoly( + &this->c2_bc(), &this->c2x_bc(), + &o, NULL + )) { + return std::make_tuple(true, 0, 0); + } + } + unsigned int i = 0; + for (auto &n: this->steered()) { + this->c2x_bc().p.x = n.x(); + this->c2x_bc().p.y = n.y(); + this->c2x_bc().r.c = cos(n.h()); + this->c2x_bc().r.s = sin(n.h()); + for (auto &o: this->c2_obstacles()) { + if (c2PolytoPoly( + &this->c2_bc(), &this->c2x_bc(), + &o, NULL + )) { + return std::make_tuple(true, i, 0); + } + } + i++; + } + return std::make_tuple(false, 0, 0); } std::tuple RRTExt2::collide_tmp_steered_from(RRTNode &f) { - this->c2x_bc().p.x = f.x(); - this->c2x_bc().p.y = f.y(); - this->c2x_bc().r.c = cos(f.h()); - this->c2x_bc().r.s = sin(f.h()); - for (auto &o: this->c2_obstacles()) { - if (c2PolytoPoly( - &this->c2_bc(), &this->c2x_bc(), - &o, NULL - )) { - return std::make_tuple(true, 0, 0); - } - } - unsigned int i = 0; - for (auto n: this->tmp_steered_) { - this->c2x_bc().p.x = n.x(); - this->c2x_bc().p.y = n.y(); - this->c2x_bc().r.c = cos(n.h()); - this->c2x_bc().r.s = sin(n.h()); - for (auto &o: this->c2_obstacles()) { - if (c2PolytoPoly( - &this->c2_bc(), &this->c2x_bc(), - &o, NULL - )) { - return std::make_tuple(true, i, 0); - } - } - i++; - } - return std::make_tuple(false, 0, 0); + this->c2x_bc().p.x = f.x(); + this->c2x_bc().p.y = f.y(); + this->c2x_bc().r.c = cos(f.h()); + this->c2x_bc().r.s = sin(f.h()); + for (auto &o: this->c2_obstacles()) { + if (c2PolytoPoly( + &this->c2_bc(), &this->c2x_bc(), + &o, NULL + )) { + return std::make_tuple(true, 0, 0); + } + } + unsigned int i = 0; + for (auto n: this->tmp_steered_) { + this->c2x_bc().p.x = n.x(); + this->c2x_bc().p.y = n.y(); + this->c2x_bc().r.c = cos(n.h()); + this->c2x_bc().r.s = sin(n.h()); + for (auto &o: this->c2_obstacles()) { + if (c2PolytoPoly( + &this->c2_bc(), &this->c2x_bc(), + &o, NULL + )) { + return std::make_tuple(true, i, 0); + } + } + i++; + } + return std::make_tuple(false, 0, 0); } std::tuple RRTExt2::collide_two_nodes(RRTNode &f, RRTNode &t) { - this->c2x_bc().p.x = f.x(); - this->c2x_bc().p.y = f.y(); - this->c2x_bc().r.c = cos(f.h()); - this->c2x_bc().r.s = sin(f.h()); - for (auto &o: this->c2_obstacles()) { - if (c2PolytoPoly( - &this->c2_bc(), &this->c2x_bc(), - &o, NULL - )) { - return std::make_tuple(true, 1, 0); - } - } - this->c2x_bc().p.x = t.x(); - this->c2x_bc().p.y = t.y(); - this->c2x_bc().r.c = cos(t.h()); - this->c2x_bc().r.s = sin(t.h()); - for (auto &o: this->c2_obstacles()) { - if (c2PolytoPoly( - &this->c2_bc(), &this->c2x_bc(), - &o, NULL - )) { - return std::make_tuple(true, 2, 0); - } - } - return std::make_tuple(false, 0, 0); + this->c2x_bc().p.x = f.x(); + this->c2x_bc().p.y = f.y(); + this->c2x_bc().r.c = cos(f.h()); + this->c2x_bc().r.s = sin(f.h()); + for (auto &o: this->c2_obstacles()) { + if (c2PolytoPoly( + &this->c2_bc(), &this->c2x_bc(), + &o, NULL + )) { + return std::make_tuple(true, 1, 0); + } + } + this->c2x_bc().p.x = t.x(); + this->c2x_bc().p.y = t.y(); + this->c2x_bc().r.c = cos(t.h()); + this->c2x_bc().r.s = sin(t.h()); + for (auto &o: this->c2_obstacles()) { + if (c2PolytoPoly( + &this->c2_bc(), &this->c2x_bc(), + &o, NULL + )) { + return std::make_tuple(true, 2, 0); + } + } + return std::make_tuple(false, 0, 0); } diff --git a/src/rrtext3.cc b/src/rrtext3.cc index f2d342e..0432bb2 100644 --- a/src/rrtext3.cc +++ b/src/rrtext3.cc @@ -3,280 +3,280 @@ void RRTExt3::reset() { - RRTS::reset(); - this->orig_path().clear(); - this->first_optimized_path().clear(); - this->orig_path_cost_ = 9999.9; - this->first_optimized_path_cost_ = 9999.9; + RRTS::reset(); + this->orig_path().clear(); + this->first_optimized_path().clear(); + this->orig_path_cost_ = 9999.9; + this->first_optimized_path_cost_ = 9999.9; } void RRTExt3::first_path_optimization() { - if (this->orig_path().size() == 0) { - this->orig_path_ = RRTS::path(); - if (this->orig_path().size() == 0) - return; - else - this->orig_path_cost(this->orig_path().back()->cc); - } - class DijkstraNode : public RRTNode { - public: - DijkstraNode *s = nullptr; - RRTNode *n = nullptr; - unsigned int i = 0; - double cc = 9999; - bool v = false; - bool vi() - { - if (this->v) - return true; - this->v = true; - return false; - } - DijkstraNode(const RRTNode& n) { - this->x(n.x()); - this->y(n.y()); - this->h(n.h()); - this->sp(n.sp()); - this->st(n.st()); - } - // override - DijkstraNode *p_ = nullptr; - DijkstraNode *p() const { return this->p_; } - void p(DijkstraNode *p) { this->p_ = p; } - }; - class DijkstraNodeComparator { - public: - int operator() ( - const DijkstraNode &n1, - const DijkstraNode &n2 - ) - { - return n1.cc > n2.cc; - } - }; - std::vector dn; - dn.reserve(this->orig_path().size()); - unsigned int dncnt = 0; - for (auto n: this->orig_path()) { - if ( - n->t(RRTNodeType::cusp) - || n->t(RRTNodeType::connected) - ) { - dn.push_back(DijkstraNode(*n)); - dn.back().cc = n->cc; - dn.back().s = &dn.back(); - dn.back().n = n; - dn.back().i = dncnt++; - } - } - dn.push_back(DijkstraNode(*this->orig_path().back())); - dn.back().cc = this->orig_path().back()->cc; - dn.back().s = &dn.back(); - dn.back().n = this->orig_path().back(); - dn.back().i = dncnt++; - std::priority_queue< - DijkstraNode, - std::vector, - DijkstraNodeComparator - > pq; - dn.front().vi(); - pq.push(dn.front()); - while (!pq.empty()) { - DijkstraNode f = pq.top(); - pq.pop(); - for (unsigned int i = f.i + 1; i < dncnt; i++) { - double cost = f.cc + this->cost_search(f, dn[i]); - this->steer(f, dn[i]); - if (this->steered().size() == 0) - break; // TODO why not continue? - if (std::get<0>(this->collide_steered_from(f))) - continue; - if (cost < dn[i].cc) { - dn[i].cc = cost; - dn[i].p(f.s); - if (!dn[i].vi()) - pq.push(dn[i]); - } - } - } - DijkstraNode *ln = nullptr; - for (auto &n: dn) { - if (n.v) - ln = &n; - } - if (ln == nullptr || ln->p() == nullptr) - return; - while (ln->p() != nullptr) { - RRTNode *t = ln->n; - RRTNode *f = ln->p()->n; - this->steer(*f, *t); - if (std::get<0>(this->collide_steered_from(*f))) - return; - this->join_steered(f); - t->p(&this->nodes().back()); - t->c(this->cost_build(this->nodes().back(), *t)); - ln = ln->p(); - } - RRTS::compute_path(); + if (this->orig_path().size() == 0) { + this->orig_path_ = RRTS::path(); + if (this->orig_path().size() == 0) + return; + else + this->orig_path_cost(this->orig_path().back()->cc); + } + class DijkstraNode : public RRTNode { + public: + DijkstraNode *s = nullptr; + RRTNode *n = nullptr; + unsigned int i = 0; + double cc = 9999; + bool v = false; + bool vi() + { + if (this->v) + return true; + this->v = true; + return false; + } + DijkstraNode(const RRTNode& n) { + this->x(n.x()); + this->y(n.y()); + this->h(n.h()); + this->sp(n.sp()); + this->st(n.st()); + } + // override + DijkstraNode *p_ = nullptr; + DijkstraNode *p() const { return this->p_; } + void p(DijkstraNode *p) { this->p_ = p; } + }; + class DijkstraNodeComparator { + public: + int operator() ( + const DijkstraNode &n1, + const DijkstraNode &n2 + ) + { + return n1.cc > n2.cc; + } + }; + std::vector dn; + dn.reserve(this->orig_path().size()); + unsigned int dncnt = 0; + for (auto n: this->orig_path()) { + if ( + n->t(RRTNodeType::cusp) + || n->t(RRTNodeType::connected) + ) { + dn.push_back(DijkstraNode(*n)); + dn.back().cc = n->cc; + dn.back().s = &dn.back(); + dn.back().n = n; + dn.back().i = dncnt++; + } + } + dn.push_back(DijkstraNode(*this->orig_path().back())); + dn.back().cc = this->orig_path().back()->cc; + dn.back().s = &dn.back(); + dn.back().n = this->orig_path().back(); + dn.back().i = dncnt++; + std::priority_queue< + DijkstraNode, + std::vector, + DijkstraNodeComparator + > pq; + dn.front().vi(); + pq.push(dn.front()); + while (!pq.empty()) { + DijkstraNode f = pq.top(); + pq.pop(); + for (unsigned int i = f.i + 1; i < dncnt; i++) { + double cost = f.cc + this->cost_search(f, dn[i]); + this->steer(f, dn[i]); + if (this->steered().size() == 0) + break; // TODO why not continue? + if (std::get<0>(this->collide_steered_from(f))) + continue; + if (cost < dn[i].cc) { + dn[i].cc = cost; + dn[i].p(f.s); + if (!dn[i].vi()) + pq.push(dn[i]); + } + } + } + DijkstraNode *ln = nullptr; + for (auto &n: dn) { + if (n.v) + ln = &n; + } + if (ln == nullptr || ln->p() == nullptr) + return; + while (ln->p() != nullptr) { + RRTNode *t = ln->n; + RRTNode *f = ln->p()->n; + this->steer(*f, *t); + if (std::get<0>(this->collide_steered_from(*f))) + return; + this->join_steered(f); + t->p(&this->nodes().back()); + t->c(this->cost_build(this->nodes().back(), *t)); + ln = ln->p(); + } + RRTS::compute_path(); } void RRTExt3::second_path_optimization() { - if (this->first_optimized_path().size() == 0) { - return; - } - class DijkstraNode : public RRTNode { - public: - DijkstraNode *s = nullptr; - RRTNode *n = nullptr; - unsigned int i = 0; - double cc = 9999; - bool v = false; - bool vi() - { - if (this->v) - return true; - this->v = true; - return false; - } - DijkstraNode(const RRTNode& n) { - this->x(n.x()); - this->y(n.y()); - this->h(n.h()); - this->sp(n.sp()); - this->st(n.st()); - } - // override - DijkstraNode *p_ = nullptr; - DijkstraNode *p() const { return this->p_; } - void p(DijkstraNode *p) { this->p_ = p; } - }; - class DijkstraNodeComparator { - public: - int operator() ( - const DijkstraNode &n1, - const DijkstraNode &n2 - ) - { - return n1.cc > n2.cc; - } - }; - std::vector dn; - dn.reserve(this->orig_path().size()); - unsigned int dncnt = 0; - for (auto n: this->orig_path()) { - if ( - n->t(RRTNodeType::cusp) - || n->t(RRTNodeType::connected) - ) { - dn.push_back(DijkstraNode(*n)); - dn.back().cc = n->cc; - dn.back().s = &dn.back(); - dn.back().n = n; - dn.back().i = dncnt++; - } - } - dn.push_back(DijkstraNode(*this->orig_path().back())); - dn.back().cc = this->orig_path().back()->cc; - dn.back().s = &dn.back(); - dn.back().n = this->orig_path().back(); - dn.back().i = dncnt++; - std::priority_queue< - DijkstraNode, - std::vector, - DijkstraNodeComparator - > pq; - // TODO rewrite - dn.back().vi(); - pq.push(dn.back()); - while (!pq.empty()) { - DijkstraNode t = pq.top(); - pq.pop(); - for (unsigned int i = t.i - 1; i > 0; i--) { - double cost = dn[i].cc + this->cost_search(dn[i], t); - this->steer(dn[i], t); - if (this->steered().size() == 0) - break; // TODO why not continue? - if (std::get<0>(this->collide_steered_from(dn[i]))) - continue; - if (cost < t.cc) { - t.cc = cost; - t.p(dn[i].s); - if (!dn[i].vi()) - pq.push(dn[i]); - } - } - } - DijkstraNode *fn = nullptr; - for (auto &n: dn) { - if (n.v) { - fn = &n; - break; - } - } - if (fn == nullptr || fn->p() == nullptr) - return; - DijkstraNode *ln = &dn.back(); - while (ln->p() != fn) { - RRTNode *t = ln->n; - RRTNode *f = ln->p()->n; - this->steer(*f, *t); - if (std::get<0>(this->collide_steered_from(*f))) - return; - this->join_steered(f); - t->p(&this->nodes().back()); - t->c(this->cost_build(this->nodes().back(), *t)); - ln = ln->p(); - } - RRTS::compute_path(); + if (this->first_optimized_path().size() == 0) { + return; + } + class DijkstraNode : public RRTNode { + public: + DijkstraNode *s = nullptr; + RRTNode *n = nullptr; + unsigned int i = 0; + double cc = 9999; + bool v = false; + bool vi() + { + if (this->v) + return true; + this->v = true; + return false; + } + DijkstraNode(const RRTNode& n) { + this->x(n.x()); + this->y(n.y()); + this->h(n.h()); + this->sp(n.sp()); + this->st(n.st()); + } + // override + DijkstraNode *p_ = nullptr; + DijkstraNode *p() const { return this->p_; } + void p(DijkstraNode *p) { this->p_ = p; } + }; + class DijkstraNodeComparator { + public: + int operator() ( + const DijkstraNode &n1, + const DijkstraNode &n2 + ) + { + return n1.cc > n2.cc; + } + }; + std::vector dn; + dn.reserve(this->orig_path().size()); + unsigned int dncnt = 0; + for (auto n: this->orig_path()) { + if ( + n->t(RRTNodeType::cusp) + || n->t(RRTNodeType::connected) + ) { + dn.push_back(DijkstraNode(*n)); + dn.back().cc = n->cc; + dn.back().s = &dn.back(); + dn.back().n = n; + dn.back().i = dncnt++; + } + } + dn.push_back(DijkstraNode(*this->orig_path().back())); + dn.back().cc = this->orig_path().back()->cc; + dn.back().s = &dn.back(); + dn.back().n = this->orig_path().back(); + dn.back().i = dncnt++; + std::priority_queue< + DijkstraNode, + std::vector, + DijkstraNodeComparator + > pq; + // TODO rewrite + dn.back().vi(); + pq.push(dn.back()); + while (!pq.empty()) { + DijkstraNode t = pq.top(); + pq.pop(); + for (unsigned int i = t.i - 1; i > 0; i--) { + double cost = dn[i].cc + this->cost_search(dn[i], t); + this->steer(dn[i], t); + if (this->steered().size() == 0) + break; // TODO why not continue? + if (std::get<0>(this->collide_steered_from(dn[i]))) + continue; + if (cost < t.cc) { + t.cc = cost; + t.p(dn[i].s); + if (!dn[i].vi()) + pq.push(dn[i]); + } + } + } + DijkstraNode *fn = nullptr; + for (auto &n: dn) { + if (n.v) { + fn = &n; + break; + } + } + if (fn == nullptr || fn->p() == nullptr) + return; + DijkstraNode *ln = &dn.back(); + while (ln->p() != fn) { + RRTNode *t = ln->n; + RRTNode *f = ln->p()->n; + this->steer(*f, *t); + if (std::get<0>(this->collide_steered_from(*f))) + return; + this->join_steered(f); + t->p(&this->nodes().back()); + t->c(this->cost_build(this->nodes().back(), *t)); + ln = ln->p(); + } + RRTS::compute_path(); } void RRTExt3::compute_path() { - RRTS::compute_path(); - auto tstart = std::chrono::high_resolution_clock::now(); - this->first_path_optimization(); - this->first_optimized_path_ = RRTS::path(); - if (this->first_optimized_path_.size() > 0) { - this->first_optimized_path_cost( - this->first_optimized_path_.back()->cc - ); - } else { - return; - } - this->second_path_optimization(); - auto tend = std::chrono::high_resolution_clock::now(); - auto tdiff = std::chrono::duration_cast>( - tend - tstart); - this->log_opt_time_.push_back(tdiff.count()); + RRTS::compute_path(); + auto tstart = std::chrono::high_resolution_clock::now(); + this->first_path_optimization(); + this->first_optimized_path_ = RRTS::path(); + if (this->first_optimized_path_.size() > 0) { + this->first_optimized_path_cost( + this->first_optimized_path_.back()->cc + ); + } else { + return; + } + this->second_path_optimization(); + auto tend = std::chrono::high_resolution_clock::now(); + auto tdiff = std::chrono::duration_cast>( + tend - tstart); + this->log_opt_time_.push_back(tdiff.count()); } Json::Value RRTExt3::json() { - Json::Value jvo = RRTS::json(); - jvo["orig_path_cost"] = this->orig_path_cost(); - { - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: this->orig_path()) { - jvo["orig_path"][pcnt][0] = n->x(); - jvo["orig_path"][pcnt][1] = n->y(); - jvo["orig_path"][pcnt][2] = n->h(); - if (n->t(RRTNodeType::cusp)) - cu++; - if (n->t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["orig_cusps-in-path"] = cu; - jvo["orig_connecteds-in-path"] = co; - } - return jvo; + Json::Value jvo = RRTS::json(); + jvo["orig_path_cost"] = this->orig_path_cost(); + { + unsigned int cu = 0; + unsigned int co = 0; + unsigned int pcnt = 0; + for (auto n: this->orig_path()) { + jvo["orig_path"][pcnt][0] = n->x(); + jvo["orig_path"][pcnt][1] = n->y(); + jvo["orig_path"][pcnt][2] = n->h(); + if (n->t(RRTNodeType::cusp)) + cu++; + if (n->t(RRTNodeType::connected)) + co++; + pcnt++; + } + jvo["orig_cusps-in-path"] = cu; + jvo["orig_connecteds-in-path"] = co; + } + return jvo; } void RRTExt3::json(Json::Value jvi) { - return RRTS::json(jvi); + return RRTS::json(jvi); } diff --git a/src/rrtext4.cc b/src/rrtext4.cc index 9b0073f..1277a4b 100644 --- a/src/rrtext4.cc +++ b/src/rrtext4.cc @@ -2,19 +2,19 @@ void RRTExt4::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p) { - double cost = p->cost_search(**nn, *t); - for (auto f: this->nodes()) { - if (p->cost_search(*f, *t) < cost) { - *nn = f; - cost = p->cost_search(*f, *t); - } - } + double cost = p->cost_search(**nn, *t); + for (auto f: this->nodes()) { + if (p->cost_search(*f, *t) < cost) { + *nn = f; + cost = p->cost_search(*f, *t); + } + } } void RRTExt4::Cell::store_node(RRTNode *n) { - this->nodes_.push_back(n); - this->changed_ = true; + this->nodes_.push_back(n); + this->changed_ = true; } RRTExt4::Cell::Cell() @@ -23,90 +23,90 @@ RRTExt4::Cell::Cell() unsigned int RRTExt4::xi(RRTNode n) { - if (n.x() >= this->x_max_) - return GRID_MAX_XI - 1; - if (n.x() <= this->x_min_) - return 0; - return (unsigned int) (floor(n.x() - this->x_min_) / GRID); + if (n.x() >= this->x_max_) + return GRID_MAX_XI - 1; + if (n.x() <= this->x_min_) + return 0; + return (unsigned int) (floor(n.x() - this->x_min_) / GRID); } unsigned int RRTExt4::yi(RRTNode n) { - if (n.y() > this->y_max_) - return GRID_MAX_YI - 1; - if (n.y() <= this->y_min_) - return 0; - return (unsigned int) (floor(n.y() - this->y_min_) / GRID); + if (n.y() > this->y_max_) + return GRID_MAX_YI - 1; + if (n.y() <= this->y_min_) + return 0; + return (unsigned int) (floor(n.y() - this->y_min_) / GRID); } // API void RRTExt4::init() { - this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2; - this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2; - this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2; - this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2; + this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2; + this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2; + this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2; + this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2; } void RRTExt4::deinit() { - for (unsigned int i = 0; i < GRID_MAX_XI; i++) - for (unsigned int j = 0; j < GRID_MAX_YI; j++) - this->grid_[i][j].nodes().clear(); + for (unsigned int i = 0; i < GRID_MAX_XI; i++) + for (unsigned int j = 0; j < GRID_MAX_YI; j++) + this->grid_[i][j].nodes().clear(); } void RRTExt4::store_node(RRTNode n) { - RRTS::store_node(n); - RRTNode *sn = &this->nodes().back(); - this->grid_[this->xi(n)][this->yi(n)].store_node(sn); + RRTS::store_node(n); + RRTNode *sn = &this->nodes().back(); + this->grid_[this->xi(n)][this->yi(n)].store_node(sn); } RRTNode *RRTExt4::nn(RRTNode &t) { - RRTNode *nn = &this->nodes().front(); - unsigned int txi = this->xi(t); - unsigned int tyi = this->yi(t); - unsigned int l = 0; - while (this->cost_search(*nn, t) > l * GRID) { - int xi_min = txi - l; - if (xi_min < 0) - xi_min = 0; - int xi_max = txi + l; - if (xi_max > GRID_MAX_XI - 1) - xi_max = GRID_MAX_XI - 1; - int yi_min = tyi - l; - if (yi_min < 0) - yi_min = 0; - int yi_max = tyi + l; - if (yi_max > GRID_MAX_YI - 1) - yi_max = GRID_MAX_YI - 1; - for (int xi = xi_min; xi <= xi_max; xi++) { - this->grid_[xi][yi_max].nn(&t, &nn, this); - } - for (int xi = xi_min; xi <= xi_max; xi++) { - this->grid_[xi][yi_min].nn(&t, &nn, this); - } - for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { - this->grid_[xi_min][yi].nn(&t, &nn, this); - } - for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { - this->grid_[xi_max][yi].nn(&t, &nn, this); - } - l++; - } - return nn; + RRTNode *nn = &this->nodes().front(); + unsigned int txi = this->xi(t); + unsigned int tyi = this->yi(t); + unsigned int l = 0; + while (this->cost_search(*nn, t) > l * GRID) { + int xi_min = txi - l; + if (xi_min < 0) + xi_min = 0; + int xi_max = txi + l; + if (xi_max > GRID_MAX_XI - 1) + xi_max = GRID_MAX_XI - 1; + int yi_min = tyi - l; + if (yi_min < 0) + yi_min = 0; + int yi_max = tyi + l; + if (yi_max > GRID_MAX_YI - 1) + yi_max = GRID_MAX_YI - 1; + for (int xi = xi_min; xi <= xi_max; xi++) { + this->grid_[xi][yi_max].nn(&t, &nn, this); + } + for (int xi = xi_min; xi <= xi_max; xi++) { + this->grid_[xi][yi_min].nn(&t, &nn, this); + } + for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { + this->grid_[xi_min][yi].nn(&t, &nn, this); + } + for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { + this->grid_[xi_max][yi].nn(&t, &nn, this); + } + l++; + } + return nn; } std::vector RRTExt4::nv(RRTNode &t) { - std::vector nv; - unsigned int txi = this->xi(t); - unsigned int tyi = this->yi(t); - unsigned int l = 0; - double cost = std::min(GAMMA(this->nodes().size()), ETA); - for (auto f: this->grid_[txi][tyi].nodes()) - if (this->cost_search(*f, t) < cost) - nv.push_back(f); - return nv; + std::vector nv; + unsigned int txi = this->xi(t); + unsigned int tyi = this->yi(t); + unsigned int l = 0; + double cost = std::min(GAMMA(this->nodes().size()), ETA); + for (auto f: this->grid_[txi][tyi].nodes()) + if (this->cost_search(*f, t) < cost) + nv.push_back(f); + return nv; } diff --git a/src/rrtext5.cc b/src/rrtext5.cc index b1fb544..f5ee50c 100644 --- a/src/rrtext5.cc +++ b/src/rrtext5.cc @@ -3,15 +3,15 @@ double RRTExt5::cost_build(RRTNode &f, RRTNode &t) { - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - return rsss.distance(q0, q1); + double q0[] = {f.x(), f.y(), f.h()}; + double q1[] = {t.x(), t.y(), t.h()}; + ReedsSheppStateSpace rsss(this->bc.mtr()); + return rsss.distance(q0, q1); } double RRTExt5::cost_search(RRTNode &f, RRTNode &t) { - double cost = 0; - cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); - return cost; + double cost = 0; + cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); + return cost; } diff --git a/src/rrtext6.cc b/src/rrtext6.cc index 0f4c35f..db7d661 100644 --- a/src/rrtext6.cc +++ b/src/rrtext6.cc @@ -3,16 +3,16 @@ double RRTExt6::cost_build(RRTNode &f, RRTNode &t) { - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - return rsss.distance(q0, q1); + double q0[] = {f.x(), f.y(), f.h()}; + double q1[] = {t.x(), t.y(), t.h()}; + ReedsSheppStateSpace rsss(this->bc.mtr()); + return rsss.distance(q0, q1); } double RRTExt6::cost_search(RRTNode &f, RRTNode &t) { - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - return rsss.distance(q0, q1); + double q0[] = {f.x(), f.y(), f.h()}; + double q1[] = {t.x(), t.y(), t.h()}; + ReedsSheppStateSpace rsss(this->bc.mtr()); + return rsss.distance(q0, q1); } diff --git a/src/rrtext7.cc b/src/rrtext7.cc index ab52db7..a6282db 100644 --- a/src/rrtext7.cc +++ b/src/rrtext7.cc @@ -1,62 +1,62 @@ #include "rrtext.h" RRTExt7::KdNode::KdNode(RRTNode *n) - : node_(n) - , left_(nullptr) - , right_(nullptr) + : node_(n) + , left_(nullptr) + , right_(nullptr) { } void RRTExt7::delete_kd_nodes(KdNode *n) { - if (!n) - return; - if (n->left() != nullptr) - delete_kd_nodes(n->left()); - if (n->right() != nullptr) - delete_kd_nodes(n->right()); - delete n; + if (!n) + return; + if (n->left() != nullptr) + delete_kd_nodes(n->left()); + if (n->right() != nullptr) + delete_kd_nodes(n->right()); + delete n; } void RRTExt7::store_node(RRTNode *n, KdNode *&r, int l) { - if (r == nullptr) - r = new KdNode(n); - else if (l % 2 == 0 && n->x() < r->node()->x()) - store_node(n, r->left(), l + 1); - else if (l % 2 == 0) - store_node(n, r->right(), l + 1); - else if (l % 2 == 1 && n->y() < r->node()->y()) - store_node(n, r->left(), l + 1); - else - store_node(n, r->right(), l + 1); + if (r == nullptr) + r = new KdNode(n); + else if (l % 2 == 0 && n->x() < r->node()->x()) + store_node(n, r->left(), l + 1); + else if (l % 2 == 0) + store_node(n, r->right(), l + 1); + else if (l % 2 == 1 && n->y() < r->node()->y()) + store_node(n, r->left(), l + 1); + else + store_node(n, r->right(), l + 1); } void RRTExt7::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d) { - if (r == nullptr) - return; - if (this->cost_search(*r->node(), t) < d) { - n = r->node(); - d = this->cost_search(*r->node(), t); - } - if (l % 2 == 0 && t.x() < r->node()->x()) { - nn(n, t, r->left(), l + 1, d); - if (r->node()->x() - t.x() < d) - nn(n, t, r->right(), l + 1, d); - } else if (l % 2 == 0) { - nn(n, t, r->right(), l + 1, d); - if (t.x() - r->node()->x() < d) - nn(n, t, r->left(), l + 1, d); - } else if (l % 2 == 1 && t.y() < r->node()->y()) { - nn(n, t, r->left(), l + 1, d); - if (r->node()->y() - t.y() < d) - nn(n, t, r->right(), l + 1, d); - } else { - nn(n, t, r->right(), l + 1, d); - if (t.y() - r->node()->y() < d) - nn(n, t, r->left(), l + 1, d); - } + if (r == nullptr) + return; + if (this->cost_search(*r->node(), t) < d) { + n = r->node(); + d = this->cost_search(*r->node(), t); + } + if (l % 2 == 0 && t.x() < r->node()->x()) { + nn(n, t, r->left(), l + 1, d); + if (r->node()->x() - t.x() < d) + nn(n, t, r->right(), l + 1, d); + } else if (l % 2 == 0) { + nn(n, t, r->right(), l + 1, d); + if (t.x() - r->node()->x() < d) + nn(n, t, r->left(), l + 1, d); + } else if (l % 2 == 1 && t.y() < r->node()->y()) { + nn(n, t, r->left(), l + 1, d); + if (r->node()->y() - t.y() < d) + nn(n, t, r->right(), l + 1, d); + } else { + nn(n, t, r->right(), l + 1, d); + if (t.y() - r->node()->y() < d) + nn(n, t, r->left(), l + 1, d); + } } // API @@ -66,26 +66,26 @@ void RRTExt7::init() void RRTExt7::deinit() { - this->delete_kd_nodes(this->kdroot_); + this->delete_kd_nodes(this->kdroot_); } void RRTExt7::store_node(RRTNode n) { - RRTS::store_node(n); - RRTNode *sn = &this->nodes().back(); - this->store_node(sn, this->kdroot_, 0); + RRTS::store_node(n); + RRTNode *sn = &this->nodes().back(); + this->store_node(sn, this->kdroot_, 0); } RRTNode *RRTExt7::nn(RRTNode &t) { - RRTNode *n = &this->nodes().front(); - double d = 9999; - this->nn(n, t, this->kdroot_, 0, d); - return n; + RRTNode *n = &this->nodes().front(); + double d = 9999; + this->nn(n, t, this->kdroot_, 0, d); + return n; } std::vector RRTExt7::nv(RRTNode &t) { - std::vector nv; - return nv; + std::vector nv; + return nv; } diff --git a/src/rrtext8.cc b/src/rrtext8.cc index 7101888..13032a2 100644 --- a/src/rrtext8.cc +++ b/src/rrtext8.cc @@ -1,112 +1,112 @@ #include "rrtext.h" RRTExt8::KdNode::KdNode(RRTNode *n) - : node_(n) - , left_(nullptr) - , right_(nullptr) + : node_(n) + , left_(nullptr) + , right_(nullptr) { } void RRTExt8::delete_kd_nodes(KdNode *n) { - if (n == nullptr) - return; - this->delete_kd_nodes(n->left()); - this->delete_kd_nodes(n->right()); - delete n; + if (n == nullptr) + return; + this->delete_kd_nodes(n->left()); + this->delete_kd_nodes(n->right()); + delete n; } void RRTExt8::store_node(RRTNode *n, KdNode *&r, int l) { - if (r == nullptr) - r = new KdNode(n); - else if (l % 3 == 0 && n->x() < r->node()->x()) - store_node(n, r->left(), l + 1); - else if (l % 3 == 0) - store_node(n, r->right(), l + 1); - else if (l % 3 == 1 && n->y() < r->node()->y()) - store_node(n, r->left(), l + 1); - else if (l % 3 == 1) - store_node(n, r->right(), l + 1); - else if (l % 3 == 2 && n->h() < r->node()->h()) - store_node(n, r->left(), l + 1); - else - store_node(n, r->right(), l + 1); + if (r == nullptr) + r = new KdNode(n); + else if (l % 3 == 0 && n->x() < r->node()->x()) + store_node(n, r->left(), l + 1); + else if (l % 3 == 0) + store_node(n, r->right(), l + 1); + else if (l % 3 == 1 && n->y() < r->node()->y()) + store_node(n, r->left(), l + 1); + else if (l % 3 == 1) + store_node(n, r->right(), l + 1); + else if (l % 3 == 2 && n->h() < r->node()->h()) + store_node(n, r->left(), l + 1); + else + store_node(n, r->right(), l + 1); } void RRTExt8::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d) { - if (r == nullptr) - return; - if (this->cost_search(*r->node(), t) < d) { - n = r->node(); - d = this->cost_search(*r->node(), t); - } - if (l % 3 == 0 && t.x() < r->node()->x()) { - nn(n, t, r->left(), l + 1, d); - if (r->node()->x() - t.x() < d) - nn(n, t, r->right(), l + 1, d); - } else if (l % 3 == 0) { - nn(n, t, r->right(), l + 1, d); - if (t.x() - r->node()->x() < d) - nn(n, t, r->left(), l + 1, d); - } else if (l % 3 == 1 && t.y() < r->node()->y()) { - nn(n, t, r->left(), l + 1, d); - if (r->node()->y() - t.y() < d) - nn(n, t, r->right(), l + 1, d); - } else if (l % 3 == 1) { - nn(n, t, r->right(), l + 1, d); - if (t.y() - r->node()->y() < d) - nn(n, t, r->left(), l + 1, d); - } else if (l % 3 == 2 && t.h() < r->node()->h()) { - nn(n, t, r->left(), l + 1, d); - if (r->node()->h() - t.h() < d) - nn(n, t, r->right(), l + 1, d); - } else { - nn(n, t, r->right(), l + 1, d); - if (t.h() - r->node()->h() < d) - nn(n, t, r->left(), l + 1, d); - } + if (r == nullptr) + return; + if (this->cost_search(*r->node(), t) < d) { + n = r->node(); + d = this->cost_search(*r->node(), t); + } + if (l % 3 == 0 && t.x() < r->node()->x()) { + nn(n, t, r->left(), l + 1, d); + if (r->node()->x() - t.x() < d) + nn(n, t, r->right(), l + 1, d); + } else if (l % 3 == 0) { + nn(n, t, r->right(), l + 1, d); + if (t.x() - r->node()->x() < d) + nn(n, t, r->left(), l + 1, d); + } else if (l % 3 == 1 && t.y() < r->node()->y()) { + nn(n, t, r->left(), l + 1, d); + if (r->node()->y() - t.y() < d) + nn(n, t, r->right(), l + 1, d); + } else if (l % 3 == 1) { + nn(n, t, r->right(), l + 1, d); + if (t.y() - r->node()->y() < d) + nn(n, t, r->left(), l + 1, d); + } else if (l % 3 == 2 && t.h() < r->node()->h()) { + nn(n, t, r->left(), l + 1, d); + if (r->node()->h() - t.h() < d) + nn(n, t, r->right(), l + 1, d); + } else { + nn(n, t, r->right(), l + 1, d); + if (t.h() - r->node()->h() < d) + nn(n, t, r->left(), l + 1, d); + } } void RRTExt8::nv( - std::vector& n, - RRTNode &t, - KdNode *r, - int l, - double const& d + std::vector& n, + RRTNode &t, + KdNode *r, + int l, + double const& d ) { - if (r == nullptr) - return; - if (this->cost_search(*r->node(), t) < d) { - n.push_back(r->node()); - } - if (l % 3 == 0 && t.x() < r->node()->x()) { - this->nv(n, t, r->left(), l + 1, d); - if (r->node()->x() - t.x() < d) - this->nv(n, t, r->right(), l + 1, d); - } else if (l % 3 == 0) { - this->nv(n, t, r->right(), l + 1, d); - if (t.x() - r->node()->x() < d) - this->nv(n, t, r->left(), l + 1, d); - } else if (l % 3 == 1 && t.y() < r->node()->y()) { - this->nv(n, t, r->left(), l + 1, d); - if (r->node()->y() - t.y() < d) - this->nv(n, t, r->right(), l + 1, d); - } else if (l % 3 == 1) { - this->nv(n, t, r->right(), l + 1, d); - if (t.y() - r->node()->y() < d) - this->nv(n, t, r->left(), l + 1, d); - } else if (l % 3 == 2 && t.h() < r->node()->h()) { - this->nv(n, t, r->left(), l + 1, d); - if (r->node()->h() - t.h() < d) - this->nv(n, t, r->right(), l + 1, d); - } else { - this->nv(n, t, r->right(), l + 1, d); - if (t.h() - r->node()->h() < d) - this->nv(n, t, r->left(), l + 1, d); - } + if (r == nullptr) + return; + if (this->cost_search(*r->node(), t) < d) { + n.push_back(r->node()); + } + if (l % 3 == 0 && t.x() < r->node()->x()) { + this->nv(n, t, r->left(), l + 1, d); + if (r->node()->x() - t.x() < d) + this->nv(n, t, r->right(), l + 1, d); + } else if (l % 3 == 0) { + this->nv(n, t, r->right(), l + 1, d); + if (t.x() - r->node()->x() < d) + this->nv(n, t, r->left(), l + 1, d); + } else if (l % 3 == 1 && t.y() < r->node()->y()) { + this->nv(n, t, r->left(), l + 1, d); + if (r->node()->y() - t.y() < d) + this->nv(n, t, r->right(), l + 1, d); + } else if (l % 3 == 1) { + this->nv(n, t, r->right(), l + 1, d); + if (t.y() - r->node()->y() < d) + this->nv(n, t, r->left(), l + 1, d); + } else if (l % 3 == 2 && t.h() < r->node()->h()) { + this->nv(n, t, r->left(), l + 1, d); + if (r->node()->h() - t.h() < d) + this->nv(n, t, r->right(), l + 1, d); + } else { + this->nv(n, t, r->right(), l + 1, d); + if (t.h() - r->node()->h() < d) + this->nv(n, t, r->left(), l + 1, d); + } } // API @@ -116,34 +116,34 @@ void RRTExt8::init() void RRTExt8::reset() { - RRTS::reset(); - this->delete_kd_nodes(); + RRTS::reset(); + this->delete_kd_nodes(); } void RRTExt8::deinit() { - this->delete_kd_nodes(this->kdroot_); + this->delete_kd_nodes(this->kdroot_); } void RRTExt8::store_node(RRTNode n) { - RRTS::store_node(n); - RRTNode *sn = &this->nodes().back(); - this->store_node(sn, this->kdroot_, 0); + RRTS::store_node(n); + RRTNode *sn = &this->nodes().back(); + this->store_node(sn, this->kdroot_, 0); } RRTNode *RRTExt8::nn(RRTNode &t) { - RRTNode *n = &this->nodes().front(); - double d = 9999; - this->nn(n, t, this->kdroot_, 0, d); - return n; + RRTNode *n = &this->nodes().front(); + double d = 9999; + this->nn(n, t, this->kdroot_, 0, d); + return n; } std::vector RRTExt8::nv(RRTNode &t) { - double cost = std::min(GAMMA(this->nodes().size()), 0.5); - std::vector n; - this->nv(n, t, this->kdroot_, 0, cost); - return n; + double cost = std::min(GAMMA(this->nodes().size()), 0.5); + std::vector n; + this->nv(n, t, this->kdroot_, 0, cost); + return n; } diff --git a/src/rrtext9.cc b/src/rrtext9.cc index 341bb32..a2cf6dc 100644 --- a/src/rrtext9.cc +++ b/src/rrtext9.cc @@ -2,19 +2,19 @@ void RRTExt9::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p) { - double cost = p->cost_search(**nn, *t); - for (auto f: this->nodes()) { - if (p->cost_search(*f, *t) < cost) { - *nn = f; - cost = p->cost_search(*f, *t); - } - } + double cost = p->cost_search(**nn, *t); + for (auto f: this->nodes()) { + if (p->cost_search(*f, *t) < cost) { + *nn = f; + cost = p->cost_search(*f, *t); + } + } } void RRTExt9::Cell::store_node(RRTNode *n) { - this->nodes_.push_back(n); - this->changed_ = true; + this->nodes_.push_back(n); + this->changed_ = true; } RRTExt9::Cell::Cell() @@ -23,110 +23,110 @@ RRTExt9::Cell::Cell() unsigned int RRTExt9::xi(RRTNode n) { - if (n.x() >= this->x_max_) - return GRID_MAX_XI - 1; - if (n.x() <= this->x_min_) - return 0; - return (unsigned int) (floor(n.x() - this->x_min_) / GRID); + if (n.x() >= this->x_max_) + return GRID_MAX_XI - 1; + if (n.x() <= this->x_min_) + return 0; + return (unsigned int) (floor(n.x() - this->x_min_) / GRID); } unsigned int RRTExt9::yi(RRTNode n) { - if (n.y() > this->y_max_) - return GRID_MAX_YI - 1; - if (n.y() <= this->y_min_) - return 0; - return (unsigned int) (floor(n.y() - this->y_min_) / GRID); + if (n.y() > this->y_max_) + return GRID_MAX_YI - 1; + if (n.y() <= this->y_min_) + return 0; + return (unsigned int) (floor(n.y() - this->y_min_) / GRID); } unsigned int RRTExt9::hi(RRTNode n) { - if (n.h() > this->h_max_) - return GRID_MAX_HI - 1; - if (n.h() <= this->h_min_) - return 0; - return (unsigned int) (n.h() * GRID_MAX_HI / (2 * M_PI)); + if (n.h() > this->h_max_) + return GRID_MAX_HI - 1; + if (n.h() <= this->h_min_) + return 0; + return (unsigned int) (n.h() * GRID_MAX_HI / (2 * M_PI)); } // API void RRTExt9::init() { - this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2; - this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2; - this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2; - this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2; + this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2; + this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2; + this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2; + this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2; } void RRTExt9::deinit() { - for (unsigned int i = 0; i < GRID_MAX_XI; i++) - for (unsigned int j = 0; j < GRID_MAX_YI; j++) - for (unsigned int k = 0; k < GRID_MAX_HI; k++) - this->grid_[i][j][k].nodes().clear(); + for (unsigned int i = 0; i < GRID_MAX_XI; i++) + for (unsigned int j = 0; j < GRID_MAX_YI; j++) + for (unsigned int k = 0; k < GRID_MAX_HI; k++) + this->grid_[i][j][k].nodes().clear(); } void RRTExt9::store_node(RRTNode n) { - RRTS::store_node(n); - RRTNode *sn = &this->nodes().back(); - this->grid_[this->xi(n)][this->yi(n)][this->hi(n)].store_node(sn); + RRTS::store_node(n); + RRTNode *sn = &this->nodes().back(); + this->grid_[this->xi(n)][this->yi(n)][this->hi(n)].store_node(sn); } RRTNode *RRTExt9::nn(RRTNode &t) { - RRTNode *nn = &this->nodes().front(); - unsigned int txi = this->xi(t); - unsigned int tyi = this->yi(t); - unsigned int thi = this->hi(t); - unsigned int l = 0; - while (this->cost_search(*nn, t) > l * GRID) { - int xi_min = txi - l; - if (xi_min < 0) - xi_min = 0; - int xi_max = txi + l; - if (xi_max > GRID_MAX_XI - 1) - xi_max = GRID_MAX_XI - 1; - int yi_min = tyi - l; - if (yi_min < 0) - yi_min = 0; - int yi_max = tyi + l; - if (yi_max > GRID_MAX_YI - 1) - yi_max = GRID_MAX_YI - 1; - int hi_min = thi - l; // TODO respect kinematic constraints - if (hi_min < 0) - hi_min = 0; - int hi_max = thi + l; - if (hi_max > GRID_MAX_HI - 1) - hi_max = GRID_MAX_HI - 1; - for (int hi = hi_min; hi <= hi_max; hi++) { - for (int xi = xi_min; xi <= xi_max; xi++) { - this->grid_[xi][yi_max][hi].nn(&t, &nn, this); - } - for (int xi = xi_min; xi <= xi_max; xi++) { - this->grid_[xi][yi_min][hi].nn(&t, &nn, this); - } - for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { - this->grid_[xi_min][yi][hi].nn(&t, &nn, this); - } - for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { - this->grid_[xi_max][yi][hi].nn(&t, &nn, this); - } - } - l++; - } - return nn; + RRTNode *nn = &this->nodes().front(); + unsigned int txi = this->xi(t); + unsigned int tyi = this->yi(t); + unsigned int thi = this->hi(t); + unsigned int l = 0; + while (this->cost_search(*nn, t) > l * GRID) { + int xi_min = txi - l; + if (xi_min < 0) + xi_min = 0; + int xi_max = txi + l; + if (xi_max > GRID_MAX_XI - 1) + xi_max = GRID_MAX_XI - 1; + int yi_min = tyi - l; + if (yi_min < 0) + yi_min = 0; + int yi_max = tyi + l; + if (yi_max > GRID_MAX_YI - 1) + yi_max = GRID_MAX_YI - 1; + int hi_min = thi - l; // TODO respect kinematic constraints + if (hi_min < 0) + hi_min = 0; + int hi_max = thi + l; + if (hi_max > GRID_MAX_HI - 1) + hi_max = GRID_MAX_HI - 1; + for (int hi = hi_min; hi <= hi_max; hi++) { + for (int xi = xi_min; xi <= xi_max; xi++) { + this->grid_[xi][yi_max][hi].nn(&t, &nn, this); + } + for (int xi = xi_min; xi <= xi_max; xi++) { + this->grid_[xi][yi_min][hi].nn(&t, &nn, this); + } + for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { + this->grid_[xi_min][yi][hi].nn(&t, &nn, this); + } + for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { + this->grid_[xi_max][yi][hi].nn(&t, &nn, this); + } + } + l++; + } + return nn; } std::vector RRTExt9::nv(RRTNode &t) { - std::vector nv; - unsigned int txi = this->xi(t); - unsigned int tyi = this->yi(t); - unsigned int thi = this->hi(t); - unsigned int l = 0; - double cost = std::min(GAMMA(this->nodes().size()), ETA); - for (auto f: this->grid_[txi][tyi][thi].nodes()) - if (this->cost_search(*f, t) < cost) - nv.push_back(f); - return nv; + std::vector nv; + unsigned int txi = this->xi(t); + unsigned int tyi = this->yi(t); + unsigned int thi = this->hi(t); + unsigned int l = 0; + double cost = std::min(GAMMA(this->nodes().size()), ETA); + for (auto f: this->grid_[txi][tyi][thi].nodes()) + if (this->cost_search(*f, t) < cost) + nv.push_back(f); + return nv; } diff --git a/src/rrts.cc b/src/rrts.cc index 2856dce..a6ef694 100644 --- a/src/rrts.cc +++ b/src/rrts.cc @@ -4,7 +4,7 @@ #include "reeds_shepp.h" template int sgn(T val) { - return (T(0) < val) - (val < T(0)); + return (T(0) < val) - (val < T(0)); } RRTNode::RRTNode() @@ -22,9 +22,9 @@ RRTNode::RRTNode(const BicycleCar &bc) bool RRTNode::operator==(const RRTNode& n) { - if (this == &n) - return true; - return false; + if (this == &n) + return true; + return false; } Obstacle::Obstacle() @@ -33,329 +33,329 @@ Obstacle::Obstacle() double RRTS::elapsed() { - std::chrono::duration dt; - dt = std::chrono::duration_cast>( - std::chrono::high_resolution_clock::now() - - this->tstart_ - ); - this->scnt_ = dt.count(); - return this->scnt_; + std::chrono::duration dt; + dt = std::chrono::duration_cast>( + std::chrono::high_resolution_clock::now() + - this->tstart_ + ); + this->scnt_ = dt.count(); + return this->scnt_; } void RRTS::log_path_cost() { - if (this->log_path_cost_.size() == 0) { - this->log_path_cost_.push_back(this->goals().front().cc); - } else { - auto lc = this->log_path_cost_.back(); - auto gc = this->goals().front().cc; - auto goal_is_better = this->goals().front().cc > 0 && lc < gc; - if ( - this->log_path_cost_.back() > 0 - && ( - this->goals().front().cc == 0 - || ( - this->goals().front().cc > 0 - && goal_is_better - ) - ) - ) { - this->log_path_cost_.push_back( - this->log_path_cost_.back() - ); - } else { - this->log_path_cost_.push_back( - this->goals().front().cc - ); - } - } - this->log_path_iter_ += 1; + if (this->log_path_cost_.size() == 0) { + this->log_path_cost_.push_back(this->goals().front().cc); + } else { + auto lc = this->log_path_cost_.back(); + auto gc = this->goals().front().cc; + auto goal_is_better = this->goals().front().cc > 0 && lc < gc; + if ( + this->log_path_cost_.back() > 0 + && ( + this->goals().front().cc == 0 + || ( + this->goals().front().cc > 0 + && goal_is_better + ) + ) + ) { + this->log_path_cost_.push_back( + this->log_path_cost_.back() + ); + } else { + this->log_path_cost_.push_back( + this->goals().front().cc + ); + } + } + this->log_path_iter_ += 1; } bool RRTS::should_stop() { - // the following counters must be updated, do not comment - this->icnt_++; - this->elapsed(); - // current iteration stop conditions - if (this->should_finish()) return true; - if (this->should_break()) return true; - // but continue by default - return false; + // the following counters must be updated, do not comment + this->icnt_++; + this->elapsed(); + // current iteration stop conditions + if (this->should_finish()) return true; + if (this->should_break()) return true; + // but continue by default + return false; } bool RRTS::should_finish() { - // decide finish conditions (maybe comment some lines) - if (this->icnt_ > 1000) return true; - //if (this->scnt_ > 2) return true; - if (this->finishit) return true; - if (this->gf()) return true; - // but continue by default - return false; + // decide finish conditions (maybe comment some lines) + if (this->icnt_ > 1000) return true; + //if (this->scnt_ > 2) return true; + if (this->finishit) return true; + if (this->gf()) return true; + // but continue by default + return false; } bool RRTS::should_break() { - // decide break conditions (maybe comment some lines) - //if (this->scnt_ - this->pcnt_ > 2) return true; - // but continue by default - return false; + // decide break conditions (maybe comment some lines) + //if (this->scnt_ - this->pcnt_ > 2) return true; + // but continue by default + return false; } bool RRTS::should_continue() { - // decide the stop conditions (maybe comment some lines) - // it is exact opposite of `should_stop` - //if (this->icnt_ > 999) return false; - if (this->scnt_ > 10) return false; - if (this->gf()) return false; - // and reset pause counter if should continue - this->pcnt_ = this->scnt_; - return true; + // decide the stop conditions (maybe comment some lines) + // it is exact opposite of `should_stop` + //if (this->icnt_ > 999) return false; + if (this->scnt_ > 10) return false; + if (this->gf()) return false; + // and reset pause counter if should continue + this->pcnt_ = this->scnt_; + return true; } void RRTS::store_node(RRTNode n) { - this->nodes().push_back(n); + this->nodes().push_back(n); } // RRT procedures std::tuple RRTS::collide(std::vector> &poly) { - for (auto &o: this->obstacles()) - if (std::get<0>(::collide(poly, o.poly()))) - return ::collide(poly, o.poly()); - return std::make_tuple(false, 0, 0); + for (auto &o: this->obstacles()) + if (std::get<0>(::collide(poly, o.poly()))) + return ::collide(poly, o.poly()); + return std::make_tuple(false, 0, 0); } std::tuple RRTS::collide_steered_from(RRTNode &f) { - auto fbc = BicycleCar(); - fbc.x(f.x()); - fbc.y(f.y()); - fbc.h(f.h()); - std::vector> s; - s.push_back(std::make_tuple(fbc.x(), fbc.y())); - for (auto &n: this->steered()) { - auto nbc = BicycleCar(); - nbc.x(n.x()); - nbc.y(n.y()); - nbc.h(n.h()); - s.push_back(std::make_tuple(nbc.lfx(), nbc.lfy())); - s.push_back(std::make_tuple(nbc.lrx(), nbc.lry())); - s.push_back(std::make_tuple(nbc.rrx(), nbc.rry())); - s.push_back(std::make_tuple(nbc.rfx(), nbc.rfy())); - } - auto col = this->collide(s); - auto strip_from = this->steered().size() - std::get<1>(col) / 4; - if (std::get<0>(col) && strip_from > 0) { - while (strip_from-- > 0) { - this->steered().pop_back(); - } - return this->collide_steered_from(f); - } - return col; + auto fbc = BicycleCar(); + fbc.x(f.x()); + fbc.y(f.y()); + fbc.h(f.h()); + std::vector> s; + s.push_back(std::make_tuple(fbc.x(), fbc.y())); + for (auto &n: this->steered()) { + auto nbc = BicycleCar(); + nbc.x(n.x()); + nbc.y(n.y()); + nbc.h(n.h()); + s.push_back(std::make_tuple(nbc.lfx(), nbc.lfy())); + s.push_back(std::make_tuple(nbc.lrx(), nbc.lry())); + s.push_back(std::make_tuple(nbc.rrx(), nbc.rry())); + s.push_back(std::make_tuple(nbc.rfx(), nbc.rfy())); + } + auto col = this->collide(s); + auto strip_from = this->steered().size() - std::get<1>(col) / 4; + if (std::get<0>(col) && strip_from > 0) { + while (strip_from-- > 0) { + this->steered().pop_back(); + } + return this->collide_steered_from(f); + } + return col; } std::tuple RRTS::collide_tmp_steered_from(RRTNode &f) { - return std::make_tuple(false, 0, 0); + return std::make_tuple(false, 0, 0); } std::tuple RRTS::collide_two_nodes(RRTNode &f, RRTNode &t) { - auto fbc = BicycleCar(); - fbc.x(f.x()); - fbc.y(f.y()); - fbc.h(f.h()); - auto tbc = BicycleCar(); - tbc.x(f.x()); - tbc.y(f.y()); - tbc.h(f.h()); - std::vector> p; - p.push_back(std::make_tuple(fbc.lfx(), fbc.lfy())); - p.push_back(std::make_tuple(fbc.lrx(), fbc.lry())); - p.push_back(std::make_tuple(fbc.rrx(), fbc.rry())); - p.push_back(std::make_tuple(fbc.rfx(), fbc.rfy())); - p.push_back(std::make_tuple(tbc.lfx(), tbc.lfy())); - p.push_back(std::make_tuple(tbc.lrx(), tbc.lry())); - p.push_back(std::make_tuple(tbc.rrx(), tbc.rry())); - p.push_back(std::make_tuple(tbc.rfx(), tbc.rfy())); - return this->collide(p); + auto fbc = BicycleCar(); + fbc.x(f.x()); + fbc.y(f.y()); + fbc.h(f.h()); + auto tbc = BicycleCar(); + tbc.x(f.x()); + tbc.y(f.y()); + tbc.h(f.h()); + std::vector> p; + p.push_back(std::make_tuple(fbc.lfx(), fbc.lfy())); + p.push_back(std::make_tuple(fbc.lrx(), fbc.lry())); + p.push_back(std::make_tuple(fbc.rrx(), fbc.rry())); + p.push_back(std::make_tuple(fbc.rfx(), fbc.rfy())); + p.push_back(std::make_tuple(tbc.lfx(), tbc.lfy())); + p.push_back(std::make_tuple(tbc.lrx(), tbc.lry())); + p.push_back(std::make_tuple(tbc.rrx(), tbc.rry())); + p.push_back(std::make_tuple(tbc.rfx(), tbc.rfy())); + return this->collide(p); } double RRTS::cost_build(RRTNode &f, RRTNode &t) { - double cost = 0; - cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); - return cost; + double cost = 0; + cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); + return cost; } double RRTS::cost_search(RRTNode &f, RRTNode &t) { - double cost = 0; - cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); - return cost; + double cost = 0; + cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); + return cost; } void RRTS::sample() { - double x = 0; - double y = 0; - double h = 0; - switch (this->sample_dist_type()) { - case 1: // uniform - x = this->udx_(this->gen_); - y = this->udy_(this->gen_); - h = this->udh_(this->gen_); - break; - case 2: // uniform circle - { - // see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409 - double R = sqrt( - pow( - this->nodes().front().x() - - this->goals().front().x(), - 2 - ) - + pow( - this->nodes().front().y() - - this->goals().front().y(), - 2 - ) - ); - double a = atan2( - this->goals().front().y() - this->nodes().front().y(), - this->goals().front().x() - this->nodes().front().x() - ); - double cx = this->goals().front().x() - R/2 * cos(a); - double cy = this->goals().front().y() - R/2 * sin(a); - double r = R * sqrt(this->udx_(this->gen_)); - double theta = this->udy_(this->gen_) * 2 * M_PI; - x = cx + r * cos(theta); - y = cy + r * sin(theta); - h = this->udh_(this->gen_); - } - break; - case 3: { - if ( - this->steered1_.size() == 0 - && this->steered2_.size() == 0 - ) { - x = this->nodes().front().x(); - y = this->nodes().front().y(); - h = this->nodes().front().h(); - this->use_nn = &this->nodes().front(); - } else { - this->udi1_ = std::uniform_int_distribution( - 0, - this->steered1_.size() - 1 - ); - this->udi2_ = std::uniform_int_distribution( - 0, - this->steered2_.size() - 1 - ); - auto ind1 = this->udi1_(this->gen_); - auto ind2 = this->udi2_(this->gen_); - if ( - this->steered2_.size() == 0 - ) { - auto n1 = this->steered1_[ind1]; - x = n1->x(); - y = n1->y(); - h = n1->h(); - this->use_nn = this->steered1_[ind1]; - } else if ( - this->steered1_.size() == 0 - ) { - auto n2 = this->steered2_[ind2]; - x = n2->x(); - y = n2->y(); - h = n2->h(); - this->use_nn = this->steered2_[ind2]; - } else { - auto n1 = this->steered1_[ind1]; - auto n2 = this->steered2_[ind2]; - auto which = this->udx_(this->gen_); - if (which > 0.5) { - x = n1->x(); - y = n1->y(); - h = n1->h(); - this->use_nn = this->steered1_[ind1]; - } else { - x = n2->x(); - y = n2->y(); - h = n2->h(); - this->use_nn = this->steered2_[ind2]; - } - } - } - break; - } - default: // normal - x = this->ndx_(this->gen_); - y = this->ndy_(this->gen_); - h = this->ndh_(this->gen_); - } - this->samples().push_back(RRTNode()); - this->samples().back().x(x); - this->samples().back().y(y); - this->samples().back().h(h); + double x = 0; + double y = 0; + double h = 0; + switch (this->sample_dist_type()) { + case 1: // uniform + x = this->udx_(this->gen_); + y = this->udy_(this->gen_); + h = this->udh_(this->gen_); + break; + case 2: // uniform circle + { + // see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409 + double R = sqrt( + pow( + this->nodes().front().x() + - this->goals().front().x(), + 2 + ) + + pow( + this->nodes().front().y() + - this->goals().front().y(), + 2 + ) + ); + double a = atan2( + this->goals().front().y() - this->nodes().front().y(), + this->goals().front().x() - this->nodes().front().x() + ); + double cx = this->goals().front().x() - R/2 * cos(a); + double cy = this->goals().front().y() - R/2 * sin(a); + double r = R * sqrt(this->udx_(this->gen_)); + double theta = this->udy_(this->gen_) * 2 * M_PI; + x = cx + r * cos(theta); + y = cy + r * sin(theta); + h = this->udh_(this->gen_); + } + break; + case 3: { + if ( + this->steered1_.size() == 0 + && this->steered2_.size() == 0 + ) { + x = this->nodes().front().x(); + y = this->nodes().front().y(); + h = this->nodes().front().h(); + this->use_nn = &this->nodes().front(); + } else { + this->udi1_ = std::uniform_int_distribution( + 0, + this->steered1_.size() - 1 + ); + this->udi2_ = std::uniform_int_distribution( + 0, + this->steered2_.size() - 1 + ); + auto ind1 = this->udi1_(this->gen_); + auto ind2 = this->udi2_(this->gen_); + if ( + this->steered2_.size() == 0 + ) { + auto n1 = this->steered1_[ind1]; + x = n1->x(); + y = n1->y(); + h = n1->h(); + this->use_nn = this->steered1_[ind1]; + } else if ( + this->steered1_.size() == 0 + ) { + auto n2 = this->steered2_[ind2]; + x = n2->x(); + y = n2->y(); + h = n2->h(); + this->use_nn = this->steered2_[ind2]; + } else { + auto n1 = this->steered1_[ind1]; + auto n2 = this->steered2_[ind2]; + auto which = this->udx_(this->gen_); + if (which > 0.5) { + x = n1->x(); + y = n1->y(); + h = n1->h(); + this->use_nn = this->steered1_[ind1]; + } else { + x = n2->x(); + y = n2->y(); + h = n2->h(); + this->use_nn = this->steered2_[ind2]; + } + } + } + break; + } + default: // normal + x = this->ndx_(this->gen_); + y = this->ndy_(this->gen_); + h = this->ndh_(this->gen_); + } + this->samples().push_back(RRTNode()); + this->samples().back().x(x); + this->samples().back().y(y); + this->samples().back().h(h); } RRTNode *RRTS::nn(RRTNode &t) { - RRTNode *nn = &this->nodes().front(); - double cost = this->cost_search(*nn, t); - for (auto &f: this->nodes()) { - if (this->cost_search(f, t) < cost) { - nn = &f; - cost = this->cost_search(f, t); - } - } - return nn; + RRTNode *nn = &this->nodes().front(); + double cost = this->cost_search(*nn, t); + for (auto &f: this->nodes()) { + if (this->cost_search(f, t) < cost) { + nn = &f; + cost = this->cost_search(f, t); + } + } + return nn; } std::vector RRTS::nv(RRTNode &t) { - std::vector nv; - double cost = std::min(GAMMA(this->nodes().size()), ETA); - for (auto &f: this->nodes()) - if (this->cost_search(f, t) < cost) - nv.push_back(&f); - return nv; + std::vector nv; + double cost = std::min(GAMMA(this->nodes().size()), ETA); + for (auto &f: this->nodes()) + if (this->cost_search(f, t) < cost) + nv.push_back(&f); + return nv; } int cb_rs_steer(double q[4], void *user_data) { - std::vector *nodes = (std::vector *) user_data; - nodes->push_back(RRTNode()); - nodes->back().x(q[0]); - nodes->back().y(q[1]); - nodes->back().h(q[2]); - nodes->back().sp(q[3]); - if (nodes->back().sp() == 0) { - nodes->back().set_t(RRTNodeType::cusp); - } else if (nodes->size() >= 2) { - RRTNode* lln = nodes->back().p(); - RRTNode* ln = &nodes->back(); - if (lln != nullptr && ln != nullptr && sgn(lln->sp()) != sgn(ln->sp())) - ln->set_t(RRTNodeType::cusp); - } - return 0; + std::vector *nodes = (std::vector *) user_data; + nodes->push_back(RRTNode()); + nodes->back().x(q[0]); + nodes->back().y(q[1]); + nodes->back().h(q[2]); + nodes->back().sp(q[3]); + if (nodes->back().sp() == 0) { + nodes->back().set_t(RRTNodeType::cusp); + } else if (nodes->size() >= 2) { + RRTNode* lln = nodes->back().p(); + RRTNode* ln = &nodes->back(); + if (lln != nullptr && ln != nullptr && sgn(lln->sp()) != sgn(ln->sp())) + ln->set_t(RRTNodeType::cusp); + } + return 0; } void RRTS::steer(RRTNode &f, RRTNode &t) { - this->steered().clear(); - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - rsss.sample(q0, q1, 0.5, cb_rs_steer, &this->steered()); + this->steered().clear(); + double q0[] = {f.x(), f.y(), f.h()}; + double q1[] = {t.x(), t.y(), t.h()}; + ReedsSheppStateSpace rsss(this->bc.mtr()); + rsss.sample(q0, q1, 0.5, cb_rs_steer, &this->steered()); } void RRTS::tmp_steer(RRTNode &f, RRTNode &t) { @@ -378,109 +378,109 @@ void RRTS::steer2(RRTNode &f, RRTNode &t) void RRTS::join_steered(RRTNode *f) { - while (this->steered().size() > 0) { - this->store_node(this->steered().front()); - RRTNode *t = &this->nodes().back(); - t->p(f); - t->c(this->cost_build(*f, *t)); - this->steered().erase(this->steered().begin()); - f = t; - } + while (this->steered().size() > 0) { + this->store_node(this->steered().front()); + RRTNode *t = &this->nodes().back(); + t->p(f); + t->c(this->cost_build(*f, *t)); + this->steered().erase(this->steered().begin()); + f = t; + } } void RRTS::join_tmp_steered(RRTNode *f) { - while (this->tmp_steered_.size() > 0) { - this->store_node(this->tmp_steered_.front()); - RRTNode *t = &this->nodes().back(); - t->p(f); - t->c(this->cost_build(*f, *t)); - this->tmp_steered_.erase(this->tmp_steered_.begin()); - f = t; - } + while (this->tmp_steered_.size() > 0) { + this->store_node(this->tmp_steered_.front()); + RRTNode *t = &this->nodes().back(); + t->p(f); + t->c(this->cost_build(*f, *t)); + this->tmp_steered_.erase(this->tmp_steered_.begin()); + f = t; + } } bool RRTS::goal_found(RRTNode &f) { - auto &g = this->goals().front(); - double cost = this->cost_build(f, g); - double edist = sqrt( - pow(f.x() - g.x(), 2) - + pow(f.y() - g.y(), 2) - ); - double adist = std::abs(f.h() - g.h()); - if (edist < 0.05 && adist < M_PI / 32) { - if (g.p() == nullptr || f.cc + cost < g.cc) { - g.p(&f); - g.c(cost); - } - return true; - } - return false; + auto &g = this->goals().front(); + double cost = this->cost_build(f, g); + double edist = sqrt( + pow(f.x() - g.x(), 2) + + pow(f.y() - g.y(), 2) + ); + double adist = std::abs(f.h() - g.h()); + if (edist < 0.05 && adist < M_PI / 32) { + if (g.p() == nullptr || f.cc + cost < g.cc) { + g.p(&f); + g.c(cost); + } + return true; + } + return false; } // RRT* procedures bool RRTS::connect() { - RRTNode *t = &this->steered().front(); - RRTNode *f = this->nn(this->samples().back()); - double cost = f->cc + this->cost_build(*f, *t); - for (auto n: this->nv(*t)) { - if ( - !std::get<0>(this->collide_two_nodes(*n, *t)) - && n->cc + this->cost_build(*n, *t) < cost - ) { - f = n; - cost = n->cc + this->cost_build(*n, *t); - } - } - // steer from f->t and then continue with the steered. - this->tmp_steer(*f, *t); - if (this->tmp_steered_.size() > 0) { - auto col = this->collide_tmp_steered_from(*f); - if (std::get<0>(col)) - return false; - this->join_tmp_steered(f); - f = &this->nodes().back(); - } - auto fbc = BicycleCar(); - fbc.x(f->x()); - fbc.y(f->y()); - fbc.h(f->h()); - auto tbc = BicycleCar(); - tbc.x(t->x()); - tbc.y(t->y()); - tbc.h(t->h()); - if (!tbc.drivable(fbc)) - return false; - // cont. - this->store_node(this->steered().front()); - t = &this->nodes().back(); - t->p(f); - t->c(this->cost_build(*f, *t)); - t->set_t(RRTNodeType::connected); - return true; + RRTNode *t = &this->steered().front(); + RRTNode *f = this->nn(this->samples().back()); + double cost = f->cc + this->cost_build(*f, *t); + for (auto n: this->nv(*t)) { + if ( + !std::get<0>(this->collide_two_nodes(*n, *t)) + && n->cc + this->cost_build(*n, *t) < cost + ) { + f = n; + cost = n->cc + this->cost_build(*n, *t); + } + } + // steer from f->t and then continue with the steered. + this->tmp_steer(*f, *t); + if (this->tmp_steered_.size() > 0) { + auto col = this->collide_tmp_steered_from(*f); + if (std::get<0>(col)) + return false; + this->join_tmp_steered(f); + f = &this->nodes().back(); + } + auto fbc = BicycleCar(); + fbc.x(f->x()); + fbc.y(f->y()); + fbc.h(f->h()); + auto tbc = BicycleCar(); + tbc.x(t->x()); + tbc.y(t->y()); + tbc.h(t->h()); + if (!tbc.drivable(fbc)) + return false; + // cont. + this->store_node(this->steered().front()); + t = &this->nodes().back(); + t->p(f); + t->c(this->cost_build(*f, *t)); + t->set_t(RRTNodeType::connected); + return true; } void RRTS::rewire() { - RRTNode *f = &this->nodes().back(); - for (auto n: this->nv(*f)) { - if ( - !std::get<0>(this->collide_two_nodes(*f, *n)) - && f->cc + this->cost_build(*f, *n) < n->cc - ) { - this->tmp_steer(*f, *n); - if (this->tmp_steered_.size() > 0) { - auto col = this->collide_tmp_steered_from(*f); - if (std::get<0>(col)) - continue; - this->join_tmp_steered(f); - f = &this->nodes().back(); - } - n->p(f); - n->c(this->cost_build(*f, *n)); - } - } + RRTNode *f = &this->nodes().back(); + for (auto n: this->nv(*f)) { + if ( + !std::get<0>(this->collide_two_nodes(*f, *n)) + && f->cc + this->cost_build(*f, *n) < n->cc + ) { + this->tmp_steer(*f, *n); + if (this->tmp_steered_.size() > 0) { + auto col = this->collide_tmp_steered_from(*f); + if (std::get<0>(col)) + continue; + this->join_tmp_steered(f); + f = &this->nodes().back(); + } + n->p(f); + n->c(this->cost_build(*f, *n)); + } + } } // API @@ -490,399 +490,399 @@ void RRTS::init() void RRTS::reset() { - RRTNode init = RRTNode(); - init.x(this->nodes().front().x()); - init.y(this->nodes().front().y()); - init.h(this->nodes().front().h()); - this->nodes().clear(); - this->store_node(RRTNode()); - this->nodes().front().x(init.x()); - this->nodes().front().y(init.y()); - this->nodes().front().h(init.h()); - this->samples().clear(); - this->steered().clear(); - this->path().clear(); - this->gf(false); - for (auto& g: this->goals()) { - g.p(nullptr); - g.c_ = 0.0; - g.cc = 0.0; - } + RRTNode init = RRTNode(); + init.x(this->nodes().front().x()); + init.y(this->nodes().front().y()); + init.h(this->nodes().front().h()); + this->nodes().clear(); + this->store_node(RRTNode()); + this->nodes().front().x(init.x()); + this->nodes().front().y(init.y()); + this->nodes().front().h(init.h()); + this->samples().clear(); + this->steered().clear(); + this->path().clear(); + this->gf(false); + for (auto& g: this->goals()) { + g.p(nullptr); + g.c_ = 0.0; + g.cc = 0.0; + } } void RRTS::deinit() { - this->nodes().clear(); - this->samples().clear(); - this->steered().clear(); - this->store_node(RRTNode()); // root - this->icnt_ = 0; - this->scnt_ = 0; - this->pcnt_ = 0; - this->gf_ = false; + this->nodes().clear(); + this->samples().clear(); + this->steered().clear(); + this->store_node(RRTNode()); // root + this->icnt_ = 0; + this->scnt_ = 0; + this->pcnt_ = 0; + this->gf_ = false; } void RRTS::compute_path() { - if (this->goals().size() == 0) - return; - RRTNode *goal = &this->goals().front(); - if (goal->p() == nullptr) - return; - this->path_.clear(); - while (goal != nullptr) { - this->path_.push_back(goal); - goal = goal->p(); - } - std::reverse(this->path_.begin(), this->path_.end()); + if (this->goals().size() == 0) + return; + RRTNode *goal = &this->goals().front(); + if (goal->p() == nullptr) + return; + this->path_.clear(); + while (goal != nullptr) { + this->path_.push_back(goal); + goal = goal->p(); + } + std::reverse(this->path_.begin(), this->path_.end()); } bool RRTS::next() { - if (this->icnt_ == 0) - this->tstart_ = std::chrono::high_resolution_clock::now(); - bool next = true; - if (this->should_stop()) { - this->log_path_cost(); - return false; - } - if (this->samples().size() == 0) { - this->samples().push_back(RRTNode()); - this->samples().back().x(this->goals().front().x()); - this->samples().back().y(this->goals().front().y()); - this->samples().back().h(this->goals().front().h()); - } else { - this->sample(); - } - this->steer1( - *this->nn(this->samples().back()), - this->samples().back() - ); - if (this->steered().size() == 0) { - this->log_path_cost(); - return next; - } - auto col = this->collide_steered_from( - *this->nn(this->samples().back()) - ); - if (std::get<0>(col)) { - auto rcnt = this->steered().size() - std::get<1>(col); - while (rcnt-- > 0) { - this->steered().pop_back(); - } - } - if (!this->connect()) { - this->log_path_cost(); - return next; - } - this->rewire(); - unsigned scnt = this->steered().size(); - this->join_steered(&this->nodes().back()); - RRTNode *just_added = &this->nodes().back(); - while (scnt > 0) { - // store all the steered1 nodes - this->steered1_.push_back(just_added); - scnt--; - auto &g = this->goals().front(); - this->steer2(*just_added, g); - auto col = this->collide_steered_from(*just_added); - if (std::get<0>(col)) { - auto rcnt = this->steered().size() - std::get<1>(col); - while (rcnt-- > 0) { - this->steered().pop_back(); - } - } - this->join_steered(just_added); - // store all the steered2 nodes - RRTNode* jap = &this->nodes().back(); - while (jap != just_added) { - this->steered2_.push_back(jap); - jap = jap->p(); - } - auto gf = this->goal_found(this->nodes().back()); - this->gf(gf); - just_added = just_added->p(); - } - if ( - this->gf() - && ( - this->path().size() == 0 - || this->goals().front().cc < this->path().back()->cc - ) - ) { - this->compute_path(); - } - this->log_path_cost(); - return next; + if (this->icnt_ == 0) + this->tstart_ = std::chrono::high_resolution_clock::now(); + bool next = true; + if (this->should_stop()) { + this->log_path_cost(); + return false; + } + if (this->samples().size() == 0) { + this->samples().push_back(RRTNode()); + this->samples().back().x(this->goals().front().x()); + this->samples().back().y(this->goals().front().y()); + this->samples().back().h(this->goals().front().h()); + } else { + this->sample(); + } + this->steer1( + *this->nn(this->samples().back()), + this->samples().back() + ); + if (this->steered().size() == 0) { + this->log_path_cost(); + return next; + } + auto col = this->collide_steered_from( + *this->nn(this->samples().back()) + ); + if (std::get<0>(col)) { + auto rcnt = this->steered().size() - std::get<1>(col); + while (rcnt-- > 0) { + this->steered().pop_back(); + } + } + if (!this->connect()) { + this->log_path_cost(); + return next; + } + this->rewire(); + unsigned scnt = this->steered().size(); + this->join_steered(&this->nodes().back()); + RRTNode *just_added = &this->nodes().back(); + while (scnt > 0) { + // store all the steered1 nodes + this->steered1_.push_back(just_added); + scnt--; + auto &g = this->goals().front(); + this->steer2(*just_added, g); + auto col = this->collide_steered_from(*just_added); + if (std::get<0>(col)) { + auto rcnt = this->steered().size() - std::get<1>(col); + while (rcnt-- > 0) { + this->steered().pop_back(); + } + } + this->join_steered(just_added); + // store all the steered2 nodes + RRTNode* jap = &this->nodes().back(); + while (jap != just_added) { + this->steered2_.push_back(jap); + jap = jap->p(); + } + auto gf = this->goal_found(this->nodes().back()); + this->gf(gf); + just_added = just_added->p(); + } + if ( + this->gf() + && ( + this->path().size() == 0 + || this->goals().front().cc < this->path().back()->cc + ) + ) { + this->compute_path(); + } + this->log_path_cost(); + return next; } void RRTS::set_sample_normal( - double mx, double dx, - double my, double dy, - double mh, double dh + double mx, double dx, + double my, double dy, + double mh, double dh ) { - this->ndx_ = std::normal_distribution(mx, dx); - this->ndy_ = std::normal_distribution(my, dy); - this->ndh_ = std::normal_distribution(mh, dh); + this->ndx_ = std::normal_distribution(mx, dx); + this->ndy_ = std::normal_distribution(my, dy); + this->ndh_ = std::normal_distribution(mh, dh); } void RRTS::set_sample_uniform( - double xmin, double xmax, - double ymin, double ymax, - double hmin, double hmax + double xmin, double xmax, + double ymin, double ymax, + double hmin, double hmax ) { - this->udx_ = std::uniform_real_distribution(xmin,xmax); - this->udy_ = std::uniform_real_distribution(ymin,ymax); - this->udh_ = std::uniform_real_distribution(hmin,hmax); + this->udx_ = std::uniform_real_distribution(xmin,xmax); + this->udy_ = std::uniform_real_distribution(ymin,ymax); + this->udh_ = std::uniform_real_distribution(hmin,hmax); } void RRTS::set_sample_uniform_circle() { - this->udx_ = std::uniform_real_distribution(0, 1); - this->udy_ = std::uniform_real_distribution(0, 1); - this->udh_ = std::uniform_real_distribution(0, 2 * M_PI); + this->udx_ = std::uniform_real_distribution(0, 1); + this->udy_ = std::uniform_real_distribution(0, 1); + this->udh_ = std::uniform_real_distribution(0, 2 * M_PI); } void RRTS::set_sample( - double x1, double x2, - double y1, double y2, - double h1, double h2 + double x1, double x2, + double y1, double y2, + double h1, double h2 ) { - switch (this->sample_dist_type()) { - case 1: // uniform - x1 += this->nodes().front().x(); - x2 += this->nodes().front().x(); - y1 += this->nodes().front().y(); - y2 += this->nodes().front().y(); - this->set_sample_uniform(x1, x2, y1, y2, h1, h2); - break; - case 2: // uniform circle - this->set_sample_uniform_circle(); - break; - case 3: // uniform index of node in nodes - this->set_sample_uniform_circle(); - break; - default: // normal - this->set_sample_normal(x1, x2, y1, y2, h1, h2); - } + switch (this->sample_dist_type()) { + case 1: // uniform + x1 += this->nodes().front().x(); + x2 += this->nodes().front().x(); + y1 += this->nodes().front().y(); + y2 += this->nodes().front().y(); + this->set_sample_uniform(x1, x2, y1, y2, h1, h2); + break; + case 2: // uniform circle + this->set_sample_uniform_circle(); + break; + case 3: // uniform index of node in nodes + this->set_sample_uniform_circle(); + break; + default: // normal + this->set_sample_normal(x1, x2, y1, y2, h1, h2); + } } Json::Value RRTS::json() { - Json::Value jvo; - { - jvo["time"] = this->scnt(); - } - { - jvo["iterations"] = this->icnt(); - } - { - jvo["init"][0] = this->nodes().front().x(); - jvo["init"][1] = this->nodes().front().y(); - jvo["init"][2] = this->nodes().front().h(); - } - { - jvo["path_cost_before_opt"] = this->path_cost_before_opt_; - } - { - if (this->path().size() > 0) { - jvo["cost"] = this->path().back()->cc; - jvo["entry"][0] = this->goals().front().x(); - jvo["entry"][1] = this->goals().front().y(); - jvo["entry"][2] = this->goals().front().h(); - if (this->entry_set) { - jvo["entry"][2] = this->entry.b; - jvo["entry"][3] = this->entry.e; - } - if (this->entries_set) { - jvo["entries"][0][0] = this->entry1.x; - jvo["entries"][0][1] = this->entry1.y; - jvo["entries"][0][2] = this->entry1.h; - jvo["entries"][1][0] = this->entry2.x; - jvo["entries"][1][1] = this->entry2.y; - jvo["entries"][1][2] = this->entry2.h; - } - jvo["goal"][0] = this->goals().back().x(); - jvo["goal"][1] = this->goals().back().y(); - jvo["goal"][2] = this->goals().back().h(); - } - } - { - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - for (auto n: this->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; - } - { - unsigned int gcnt = 0; - for (auto g: this->goals()) { - jvo["goals"][gcnt][0] = g.x(); - jvo["goals"][gcnt][1] = g.y(); - jvo["goals"][gcnt][2] = g.h(); - gcnt++; - } - } - { - unsigned int ocnt = 0; - for (auto o: this->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++; - } - } - { - jvo["nodes"] = (unsigned int) this->nodes().size(); - } - { - unsigned int cnt = 0; - for (auto i: this->log_path_cost_) - jvo["log_path_cost"][cnt++] = i; - } - { - unsigned int cnt = 0; - for (auto i: this->log_opt_time_) - jvo["log_opt_time"][cnt++] = i; - } - //{ - // unsigned int ncnt = 0; - // for (auto n: this->nodes()) { - // jvo["nodes_x"][ncnt] = n.x(); - // jvo["nodes_y"][ncnt] = n.y(); - // //jvo["nodes_h"][ncnt] = n.h(); - // ncnt++; - // } - //} - //{ - // unsigned int ncnt = 0; - // for (auto n: this->steered1_) { - // jvo["steered1_x"][ncnt] = n->x(); - // jvo["steered1_y"][ncnt] = n->y(); - // //jvo["nodes_h"][ncnt] = n.h(); - // ncnt++; - // } - // ncnt = 0; - // for (auto n: this->steered2_) { - // jvo["steered2_x"][ncnt] = n->x(); - // jvo["steered2_y"][ncnt] = n->y(); - // //jvo["nodes_h"][ncnt] = n.h(); - // ncnt++; - // } - //} - return jvo; + Json::Value jvo; + { + jvo["time"] = this->scnt(); + } + { + jvo["iterations"] = this->icnt(); + } + { + jvo["init"][0] = this->nodes().front().x(); + jvo["init"][1] = this->nodes().front().y(); + jvo["init"][2] = this->nodes().front().h(); + } + { + jvo["path_cost_before_opt"] = this->path_cost_before_opt_; + } + { + if (this->path().size() > 0) { + jvo["cost"] = this->path().back()->cc; + jvo["entry"][0] = this->goals().front().x(); + jvo["entry"][1] = this->goals().front().y(); + jvo["entry"][2] = this->goals().front().h(); + if (this->entry_set) { + jvo["entry"][2] = this->entry.b; + jvo["entry"][3] = this->entry.e; + } + if (this->entries_set) { + jvo["entries"][0][0] = this->entry1.x; + jvo["entries"][0][1] = this->entry1.y; + jvo["entries"][0][2] = this->entry1.h; + jvo["entries"][1][0] = this->entry2.x; + jvo["entries"][1][1] = this->entry2.y; + jvo["entries"][1][2] = this->entry2.h; + } + jvo["goal"][0] = this->goals().back().x(); + jvo["goal"][1] = this->goals().back().y(); + jvo["goal"][2] = this->goals().back().h(); + } + } + { + unsigned int cu = 0; + unsigned int co = 0; + unsigned int pcnt = 0; + for (auto n: this->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; + } + { + unsigned int gcnt = 0; + for (auto g: this->goals()) { + jvo["goals"][gcnt][0] = g.x(); + jvo["goals"][gcnt][1] = g.y(); + jvo["goals"][gcnt][2] = g.h(); + gcnt++; + } + } + { + unsigned int ocnt = 0; + for (auto o: this->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++; + } + } + { + jvo["nodes"] = (unsigned int) this->nodes().size(); + } + { + unsigned int cnt = 0; + for (auto i: this->log_path_cost_) + jvo["log_path_cost"][cnt++] = i; + } + { + unsigned int cnt = 0; + for (auto i: this->log_opt_time_) + jvo["log_opt_time"][cnt++] = i; + } + //{ + // unsigned int ncnt = 0; + // for (auto n: this->nodes()) { + // jvo["nodes_x"][ncnt] = n.x(); + // jvo["nodes_y"][ncnt] = n.y(); + // //jvo["nodes_h"][ncnt] = n.h(); + // ncnt++; + // } + //} + //{ + // unsigned int ncnt = 0; + // for (auto n: this->steered1_) { + // jvo["steered1_x"][ncnt] = n->x(); + // jvo["steered1_y"][ncnt] = n->y(); + // //jvo["nodes_h"][ncnt] = n.h(); + // ncnt++; + // } + // ncnt = 0; + // for (auto n: this->steered2_) { + // jvo["steered2_x"][ncnt] = n->x(); + // jvo["steered2_y"][ncnt] = n->y(); + // //jvo["nodes_h"][ncnt] = n.h(); + // ncnt++; + // } + //} + return jvo; } void RRTS::json(Json::Value jvi) { - assert(jvi["init"] != Json::nullValue); - assert(jvi["goals"] != Json::nullValue); - assert(jvi["obst"] != Json::nullValue); - - this->nodes().front().x(jvi["init"][0].asDouble()); - this->nodes().front().y(jvi["init"][1].asDouble()); - this->nodes().front().h(jvi["init"][2].asDouble()); - { - RRTNode* gp = nullptr; - if (jvi["entry"] != Json::nullValue) { - this->entry_set = true; - this->entry.x = jvi["entry"][0].asDouble(); - this->entry.y = jvi["entry"][1].asDouble(); - this->entry.b = jvi["entry"][2].asDouble(); - this->entry.e = jvi["entry"][3].asDouble(); - RRTNode tmp_node; - tmp_node.x(this->entry.x); - tmp_node.y(this->entry.y); - tmp_node.h((this->entry.b + this->entry.e) / 2.0); - this->goals().push_back(tmp_node); - this->goals().back().p(gp); - gp = &this->goals().back(); - } - if (jvi["entries"] != Json::nullValue) { - this->entries_set = true; - this->entry1.x = jvi["entries"][0][0].asDouble(); - this->entry1.y = jvi["entries"][0][1].asDouble(); - this->entry1.h = jvi["entries"][0][2].asDouble(); - this->entry2.x = jvi["entries"][1][0].asDouble(); - this->entry2.y = jvi["entries"][1][1].asDouble(); - this->entry2.h = jvi["entries"][1][2].asDouble(); - } - for (auto g: jvi["goals"]) { - RRTNode tmp_node; - tmp_node.x(g[0].asDouble()); - tmp_node.y(g[1].asDouble()); - tmp_node.h(g[2].asDouble()); - this->goals().push_back(tmp_node); - this->goals().back().p(gp); - gp = &this->goals().back(); - } - this->goals().front().set_t(RRTNodeType::cusp); - this->goals().back().set_t(RRTNodeType::cusp); - } - { - 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); - } - this->obstacles().push_back(tmp_obstacle); - } - } - { - double edist_init_goal = sqrt( - pow( - this->nodes().front().x() - - this->goals().front().x(), - 2 - ) - + pow( - this->nodes().front().y() - - this->goals().front().y(), - 2 - ) - ); - this->set_sample( - this->nodes().front().x(), edist_init_goal, - this->nodes().front().y(), edist_init_goal, - 0, 2 * M_PI - ); - } + assert(jvi["init"] != Json::nullValue); + assert(jvi["goals"] != Json::nullValue); + assert(jvi["obst"] != Json::nullValue); + + this->nodes().front().x(jvi["init"][0].asDouble()); + this->nodes().front().y(jvi["init"][1].asDouble()); + this->nodes().front().h(jvi["init"][2].asDouble()); + { + RRTNode* gp = nullptr; + if (jvi["entry"] != Json::nullValue) { + this->entry_set = true; + this->entry.x = jvi["entry"][0].asDouble(); + this->entry.y = jvi["entry"][1].asDouble(); + this->entry.b = jvi["entry"][2].asDouble(); + this->entry.e = jvi["entry"][3].asDouble(); + RRTNode tmp_node; + tmp_node.x(this->entry.x); + tmp_node.y(this->entry.y); + tmp_node.h((this->entry.b + this->entry.e) / 2.0); + this->goals().push_back(tmp_node); + this->goals().back().p(gp); + gp = &this->goals().back(); + } + if (jvi["entries"] != Json::nullValue) { + this->entries_set = true; + this->entry1.x = jvi["entries"][0][0].asDouble(); + this->entry1.y = jvi["entries"][0][1].asDouble(); + this->entry1.h = jvi["entries"][0][2].asDouble(); + this->entry2.x = jvi["entries"][1][0].asDouble(); + this->entry2.y = jvi["entries"][1][1].asDouble(); + this->entry2.h = jvi["entries"][1][2].asDouble(); + } + for (auto g: jvi["goals"]) { + RRTNode tmp_node; + tmp_node.x(g[0].asDouble()); + tmp_node.y(g[1].asDouble()); + tmp_node.h(g[2].asDouble()); + this->goals().push_back(tmp_node); + this->goals().back().p(gp); + gp = &this->goals().back(); + } + this->goals().front().set_t(RRTNodeType::cusp); + this->goals().back().set_t(RRTNodeType::cusp); + } + { + 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); + } + this->obstacles().push_back(tmp_obstacle); + } + } + { + double edist_init_goal = sqrt( + pow( + this->nodes().front().x() + - this->goals().front().x(), + 2 + ) + + pow( + this->nodes().front().y() + - this->goals().front().y(), + 2 + ) + ); + this->set_sample( + this->nodes().front().x(), edist_init_goal, + this->nodes().front().y(), edist_init_goal, + 0, 2 * M_PI + ); + } } RRTS::RRTS() - : gen_(std::random_device{}()) + : gen_(std::random_device{}()) { - this->goals().reserve(100); - this->nodes().reserve(4000000); - this->samples().reserve(1000); - this->steered().reserve(20000); - this->store_node(RRTNode()); // root + this->goals().reserve(100); + this->nodes().reserve(4000000); + this->samples().reserve(1000); + this->steered().reserve(20000); + this->store_node(RRTNode()); // root } double cc(RRTNode &t) { - RRTNode *n = &t; - double cost = 0; - while (n != nullptr) { - cost += n->c(); - n = n->p(); - } - return cost; + RRTNode *n = &t; + double cost = 0; + while (n != nullptr) { + cost += n->c(); + n = n->p(); + } + return cost; }