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 {
39 unsigned int _cusp = 0;
40 int _segment_type = 0; // 0 ~ straight, 1 ~ left, -1 right
42 /*! Get cost to parent. */
45 /*! Set cost to parent. */
48 /*! Get cumulative cost from root. */
51 /*! Get parent node. */
54 /*! Set parent node. */
57 /*! Get number of backward-forward direction changes. */
58 unsigned int cusp() const;
60 /*! Set number of backward-forward direction changes. */
61 void cusp(RRTNode const& p);
63 /*! \brief Get Reeds & Shepp segment type.
67 * - 1 for turning left,
68 * - and -1 for turning right.
72 /*! Set Reeds & Shepp segment type. */
75 bool operator==(RRTNode const& n);
78 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
80 using PoseRange::PoseRange;
83 /*! RRT* algorithm basic class. */
88 unsigned int icnt_ = 0;
89 unsigned int _imax = 1000;
91 std::default_random_engine gen_;
92 std::vector<RRTNode> nodes_;
93 std::vector<RRTNode> steered_;
94 std::vector<RRTNode*> path_;
95 RRTNode* nn_ = nullptr;
96 std::vector<RRTNode*> nv_;
100 double last_goal_cc_ = 0.0;
101 std::vector<RRTNode> last_path_;
102 void recompute_cc(RRTNode* g);
103 void recompute_path_cc();
104 double min_gamma_eta() const;
105 bool should_continue() const;
106 void join_steered(RRTNode* f);
110 bool goal_drivable_from(RRTNode const& f);
111 virtual void store(RRTNode n);
112 virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
113 virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
114 virtual void find_nn(RRTNode const& t);
115 virtual void find_nv(RRTNode const& t);
116 virtual void compute_path();
117 virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
118 virtual bool collide_steered() = 0;
119 virtual RRTNode sample() = 0;
120 virtual bool should_finish() const = 0;
124 /*! Set internal bicycle car. */
127 /*! Set maximum number of iterations before reset. */
128 void set_imax_reset(unsigned int i);
131 void set_goal(double x, double y, double b, double e);
134 void set_start(double x, double y, double h);
137 std::vector<Pose> get_path() const;
139 /*! Get path cost. */
140 double get_path_cost() const;
142 /*! Get iterations counter. */
143 unsigned int icnt() const;
145 /*! Set iterations counter. */
146 void icnt(unsigned int i);
148 /*! Return elapsed time. */
154 /*! Generate JSON output. */
155 Json::Value json() const;
157 /*! Load JSON input. */
158 void json(Json::Value jvi);
160 /*! Run next RRT* iteration. */
163 /*! Reset the algorithm. */
164 virtual void reset();
168 #endif /* RRTS_RRTS_H */