2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
7 /*! \brief RRT* structure and default procedures.
16 #include <json/json.h>
24 /*! Compute elapsed time class. */
27 std::chrono::high_resolution_clock::time_point tstart_;
33 /*! Store RRT node. */
34 class RRTNode : public virtual Pose, public virtual CarMove {
38 RRTNode* p_ = nullptr;
39 unsigned int cusp_ = 0;
41 /*! Get cost to parent. */
44 /*! Set cost to parent. */
47 /*! Get cumulative cost from root. */
50 /*! Get parent node. */
53 /*! Set parent node. */
56 /*! Get number of backward-forward direction changes. */
57 unsigned int cusp() const;
59 /*! Set number of backward-forward direction changes. */
60 void cusp(RRTNode const& p);
62 bool operator==(RRTNode const& n);
65 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
67 using PoseRange::PoseRange;
70 /*! RRT* algorithm basic class. */
75 unsigned int icnt_ = 0;
76 unsigned int _imax = 1000;
78 std::default_random_engine gen_;
79 std::vector<RRTNode> nodes_;
80 std::vector<RRTNode> steered_;
81 std::vector<RRTNode*> path_;
82 RRTNode* nn_ = nullptr;
83 std::vector<RRTNode*> nv_;
87 double last_goal_cc_ = 0.0;
88 std::vector<RRTNode> last_path_;
89 void recompute_cc(RRTNode* g);
90 void recompute_path_cc();
91 double min_gamma_eta() const;
92 bool should_continue() const;
93 void join_steered(RRTNode* f);
97 bool goal_drivable_from(RRTNode const& f);
98 virtual void store(RRTNode n);
99 virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
100 virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
101 virtual void find_nn(RRTNode const& t);
102 virtual void find_nv(RRTNode const& t);
103 virtual void compute_path();
104 virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
105 virtual bool collide_steered() = 0;
106 virtual RRTNode sample() = 0;
107 virtual bool should_finish() const = 0;
111 /*! Set internal bicycle car. */
114 /*! Set maximum number of iterations before reset. */
115 void set_imax_reset(unsigned int i);
118 void set_goal(double x, double y, double b, double e);
121 void set_start(double x, double y, double h);
124 std::vector<Pose> get_path() const;
126 /*! Get path cost. */
127 double get_path_cost() const;
129 /*! Get iterations counter. */
130 unsigned int icnt() const;
132 /*! Set iterations counter. */
133 void icnt(unsigned int i);
135 /*! Return elapsed time. */
138 /*! Generate JSON output. */
139 Json::Value json() const;
141 /*! Load JSON input. */
142 void json(Json::Value jvi);
144 /*! Run next RRT* iteration. */
147 /*! Reset the algorithm. */
148 virtual void reset();
152 #endif /* RRTS_RRTS_H */