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/>.
30 #include "slotplanner.h"
32 #define NOFNODES 20000
37 #define GLVERTEX(n) ((n)->x() * glplwscale), ((n)->y() * glplhscale)
40 struct mcnn { // min-cost nearest neighbour
44 #pragma omp declare reduction \
45 (minn: struct mcnn: omp_out = \
46 omp_in.mc < omp_out.mc ? omp_in : omp_out) \
51 #define GAMMA_RRTSTAR(card_V) ({ \
52 __typeof__ (card_V) _card_V = (card_V); \
53 pow(log(_card_V) / _card_V, 1.0/3.0); \
56 # pragma omp declare reduction \
57 (merge: std::vector<RRTNode *>: \
58 omp_out.insert(omp_out.end(), omp_in.begin(), omp_in.end()))
62 std::vector<RRTNode *> nodes_;
64 bool changed_ = false;
70 std::vector<RRTNode *> nodes();
73 void add_node(RRTNode *n);
78 RRTNode *root_ = nullptr;
79 RRTNode *goal_ = nullptr;
80 std::vector<RRTNode *> goals_;
82 std::vector<RRTNode *> nodes_;
83 std::vector<RRTNode *> dnodes_;
84 std::queue<RRTNode *> firsts_;
85 PolygonObstacle frame_;
86 std::vector<RRTNode *> samples_;
87 std::vector<CircleObstacle> *cobstacles_;
88 std::vector<SegmentObstacle> *sobstacles_;
90 bool goal_found_ = false;
91 std::chrono::high_resolution_clock::time_point tstart_;
92 std::chrono::high_resolution_clock::time_point tend_;
94 std::vector<float> clog_; // costs of trajectories
95 std::vector<float> nlog_; // #nodes of RRT
96 std::vector<std::vector<RRTEdge *>> rlog_; // log tree
97 std::vector<float> slog_; // seconds of trajectories
98 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
99 std::vector<RRTNode *> slot_cusp_; // cusp nodes in slot
101 std::default_random_engine gen_;
102 std::normal_distribution<float> ndx_;
103 std::normal_distribution<float> ndy_;
104 std::normal_distribution<float> ndh_;
106 const float GOAL_FOUND_DISTANCE = 0.2;
107 const float GOAL_FOUND_ANGLE = M_PI / 32;
115 RRTBase(RRTNode *init, RRTNode *goal);
120 std::vector<RRTNode *> &goals();
121 std::vector<RRTNode *> &nodes();
122 std::vector<RRTNode *> &dnodes();
123 std::queue<RRTNode *> &firsts();
124 PolygonObstacle &frame();
125 std::vector<RRTNode *> &samples();
126 std::array<std::vector<RRTNode *>, IYSIZE> iy_;
127 Cell ixy_[IXSIZE][IYSIZE];
128 std::vector<CircleObstacle> *co();
129 std::vector<SegmentObstacle> *so();
130 std::vector<float> &clog();
131 std::vector<float> &nlog();
132 std::vector<std::vector<RRTEdge *>> &rlog();
133 std::vector<float> &slog();
134 std::vector<std::vector<RRTNode *>> &tlog();
135 std::vector<RRTNode *> &slot_cusp();
138 std::vector<RRTNode *> traj_cusp();
141 void root(RRTNode *node);
142 void goal(RRTNode *node);
143 void goals(std::vector<RRTNode *> g);
144 bool logr(RRTNode *root);
145 float ocost(RRTNode *n);
146 bool tlog(std::vector<RRTNode *> t);
150 std::vector<CircleObstacle> *cobstacles,
151 std::vector<SegmentObstacle> *sobstacles);
152 bool add_iy(RRTNode *n);
153 bool add_ixy(RRTNode *n);
154 bool goal_found(bool f);
155 void slot_cusp(std::vector<RRTNode *> sc);
161 float (*cost)(RRTNode *, RRTNode *));
166 bool collide(RRTNode *init, RRTNode *goal);
168 std::vector<RRTNode *> &cusps,
169 std::vector<int> &npi);
171 std::vector<RRTNode *> &cusps,
172 std::vector<int> &npi);
174 std::vector<RRTNode *> &cusps,
175 std::vector<int> &npi);
177 bool rebase(RRTNode *nr);
178 std::vector<RRTNode *> findt();
179 std::vector<RRTNode *> findt(RRTNode *n);
184 void defaultSamplingInfo();
185 void setSamplingInfo(SamplingInfo si);
187 float cost(RRTNode *init, RRTNode *goal);
188 RRTNode *nn(RRTNode *rs);
189 std::vector<RRTNode *> nv(RRTNode *node, float dist);
190 std::vector<RRTNode *> steer(RRTNode *init, RRTNode *goal);
191 std::vector<RRTNode *> steer(
196 // virtuals - implemented by child classes
197 virtual bool next() = 0;