2 This file is part of I am car.
4 I am car is free software: you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation, either version 3 of the License, or
7 (at your option) any later version.
9 I am car is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
14 You should have received a copy of the GNU General Public License
15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
31 #include "slotplanner.h"
33 #define NOFNODES 20000
36 #define IXSTEP (1.0 * ((HMAX) - (HMIN)) / IXSIZE)
37 #define IXI(x) ({ __typeof__ (x) _x = (x); \
38 std::abs((int) floor(_x / IXSTEP)); })
40 #define IYSTEP (1.0 * ((VMAX) - (VMIN)) / IYSIZE)
42 __typeof__ (y) _y = (y); \
43 (int) floor((_y - VMIN) / IYSTEP); \
46 #define GLVERTEX(n) ((n)->x() * glplwscale), ((n)->y() * glplhscale)
50 std::vector<RRTNode *> nodes_;
52 bool changed_ = false;
58 std::vector<RRTNode *> nodes();
61 void add_node(RRTNode *n);
66 RRTNode *root_ = nullptr;
67 RRTNode *goal_ = nullptr;
68 std::vector<RRTNode *> goals_;
70 std::vector<RRTNode *> nodes_;
71 std::vector<RRTNode *> dnodes_;
72 std::queue<RRTNode *> firsts_;
73 PolygonObstacle frame_;
74 std::vector<RRTNode *> samples_;
75 std::vector<CircleObstacle> *cobstacles_;
76 std::vector<SegmentObstacle> *sobstacles_;
78 bool goal_found_ = false;
79 std::chrono::high_resolution_clock::time_point tstart_;
80 std::chrono::high_resolution_clock::time_point tend_;
82 std::vector<float> clog_; // costs of trajectories
83 std::vector<float> nlog_; // #nodes of RRT
84 std::vector<std::vector<RRTEdge *>> rlog_; // log tree
85 std::vector<float> slog_; // seconds of trajectories
86 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
87 std::vector<RRTNode *> slot_cusp_; // cusp nodes in slot
89 std::default_random_engine gen_;
90 std::normal_distribution<float> ndx_;
91 std::normal_distribution<float> ndy_;
92 std::normal_distribution<float> ndh_;
94 const float GOAL_FOUND_DISTANCE = 0.2;
95 const float GOAL_FOUND_ANGLE = M_PI / 32;
103 RRTBase(RRTNode *init, RRTNode *goal);
108 std::vector<RRTNode *> &goals();
109 std::vector<RRTNode *> &nodes();
110 std::vector<RRTNode *> &dnodes();
111 std::queue<RRTNode *> &firsts();
112 PolygonObstacle &frame();
113 std::vector<RRTNode *> &samples();
114 std::array<std::vector<RRTNode *>, IYSIZE> iy_;
115 Cell ixy_[IXSIZE][IYSIZE];
116 std::vector<CircleObstacle> *co();
117 std::vector<SegmentObstacle> *so();
118 std::vector<float> &clog();
119 std::vector<float> &nlog();
120 std::vector<std::vector<RRTEdge *>> &rlog();
121 std::vector<float> &slog();
122 std::vector<std::vector<RRTNode *>> &tlog();
123 std::vector<RRTNode *> &slot_cusp();
126 std::vector<RRTNode *> traj_cusp();
129 void root(RRTNode *node);
130 void goal(RRTNode *node);
131 void goals(std::vector<RRTNode *> g);
132 bool logr(RRTNode *root);
133 float ocost(RRTNode *n);
134 bool tlog(std::vector<RRTNode *> t);
138 std::vector<CircleObstacle> *cobstacles,
139 std::vector<SegmentObstacle> *sobstacles);
140 bool add_iy(RRTNode *n);
141 bool add_ixy(RRTNode *n);
142 bool goal_found(bool f);
143 void slot_cusp(std::vector<RRTNode *> sc);
149 float (*cost)(RRTNode *, RRTNode *));
154 bool collide(RRTNode *init, RRTNode *goal);
156 std::vector<RRTNode *> &cusps,
157 std::vector<int> &npi);
159 std::vector<RRTNode *> &cusps,
160 std::vector<int> &npi);
162 std::vector<RRTNode *> &cusps,
163 std::vector<int> &npi);
165 bool rebase(RRTNode *nr);
166 std::vector<RRTNode *> findt();
167 std::vector<RRTNode *> findt(RRTNode *n);
172 void setSamplingInfo(SamplingInfo si);
174 float cost(RRTNode *init, RRTNode *goal);
175 RRTNode *nn(RRTNode *rs);
176 std::vector<RRTNode *> nv(RRTNode *node, float dist);
177 std::vector<RRTNode *> steer(RRTNode *init, RRTNode *goal);
178 std::vector<RRTNode *> steer(
183 // virtuals - implemented by child classes
184 virtual bool next() = 0;