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/>.
29 #include "slotplanner.h"
31 #define NOFNODES 20000
34 #define IXSTEP (1.0 * ((VMAX) - (VMIN)) / IXSIZE)
35 #define IXI(x) ({ __typeof__ (x) _x = (x); \
36 (int) floor(_x / IXSTEP); })
38 #define IYSTEP (1.0 * ((HMAX) - (HMIN)) / IYSIZE)
39 #define IYI(y) ({ __typeof__ (y) _y = (y); \
40 (int) floor(_y / IYSTEP); })
44 std::vector<RRTNode *> nodes_;
46 bool changed_ = false;
52 std::vector<RRTNode *> nodes();
55 void add_node(RRTNode *n);
60 RRTNode *root_ = nullptr;
61 RRTNode *goal_ = nullptr;
63 std::vector<RRTNode *> nodes_;
64 std::vector<RRTNode *> dnodes_;
65 PolygonObstacle frame_;
66 std::vector<RRTNode *> samples_;
67 std::vector<CircleObstacle> *cobstacles_;
68 std::vector<SegmentObstacle> *sobstacles_;
70 bool goal_found_ = false;
71 std::chrono::high_resolution_clock::time_point tstart_;
72 std::chrono::high_resolution_clock::time_point tend_;
74 std::vector<float> clog_; // costs of trajectories
75 std::vector<float> nlog_; // #nodes of RRT
76 std::vector<std::vector<RRTEdge *>> rlog_; // log tree
77 std::vector<float> slog_; // seconds of trajectories
78 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
79 std::vector<RRTNode *> slot_cusp_; // cusp nodes in slot
81 std::default_random_engine gen_;
83 const float GOAL_FOUND_DISTANCE = 0.2;
84 const float GOAL_FOUND_ANGLE = M_PI / 32;
88 RRTBase(RRTNode *init, RRTNode *goal);
93 std::vector<RRTNode *> &nodes();
94 std::vector<RRTNode *> &dnodes();
95 PolygonObstacle &frame();
96 std::vector<RRTNode *> &samples();
97 std::vector<RRTNode *> iy_[IYSIZE];
98 Cell ixy_[IXSIZE][IYSIZE];
99 std::vector<CircleObstacle> *co();
100 std::vector<SegmentObstacle> *so();
101 std::vector<float> &clog();
102 std::vector<float> &nlog();
103 std::vector<std::vector<RRTEdge *>> &rlog();
104 std::vector<float> &slog();
105 std::vector<std::vector<RRTNode *>> &tlog();
106 std::vector<RRTNode *> &slot_cusp();
111 void root(RRTNode *node);
112 void goal(RRTNode *node);
113 bool logr(RRTNode *root);
114 float ocost(RRTNode *n);
115 bool tlog(std::vector<RRTNode *> t);
119 std::vector<CircleObstacle> *cobstacles,
120 std::vector<SegmentObstacle> *sobstacles);
121 bool add_iy(RRTNode *n);
122 bool add_ixy(RRTNode *n);
123 bool goal_found(bool f);
124 void slot_cusp(std::vector<RRTNode *> sc);
130 float (*cost)(RRTNode *, RRTNode *));
131 bool collide(RRTNode *init, RRTNode *goal);
133 std::vector<RRTNode *> &cusps,
134 std::vector<int> &npi);
136 std::vector<RRTNode *> &cusps,
137 std::vector<int> &npi);
139 std::vector<RRTNode *> &cusps,
140 std::vector<int> &npi);
142 bool rebase(RRTNode *nr);
143 std::vector<RRTNode *> findt();
144 std::vector<RRTNode *> findt(RRTNode *n);
147 SamplingInfo samplingInfo_;
149 float cost(RRTNode *init, RRTNode *goal);
150 RRTNode *nn(RRTNode *rs);
151 std::vector<RRTNode *> nv(RRTNode *node, float dist);
152 std::vector<RRTNode *> steer(RRTNode *init, RRTNode *goal);
153 std::vector<RRTNode *> steer(
158 // virtuals - implemented by child classes
159 virtual bool next() = 0;