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 #define IYSTEP (1.0 * ((HMAX) - (HMIN)) / IYSIZE)
30 #define IYI(y) ({ __typeof__ (y) _y = (y); \
31 (int) floor(_y / IYSTEP); })
35 RRTNode *root_ = nullptr;
36 RRTNode *goal_ = nullptr;
38 std::vector<RRTNode *> nodes_;
39 std::vector<RRTNode *> dnodes_;
40 std::vector<RRTNode *> samples_;
41 std::vector<CircleObstacle> *cobstacles_;
42 std::vector<SegmentObstacle> *sobstacles_;
44 bool goal_found_ = false;
45 std::chrono::high_resolution_clock::time_point tstart_;
46 std::chrono::high_resolution_clock::time_point tend_;
48 std::vector<float> clog_; // costs of trajectories
49 std::vector<float> nlog_; // #nodes of RRT
50 std::vector<std::vector<RRTEdge *>> rlog_; // log tree
51 std::vector<float> slog_; // seconds of trajectories
52 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
54 const float GOAL_FOUND_DISTANCE = 0.2;
55 const float GOAL_FOUND_ANGLE = M_PI / 32;
59 RRTBase(RRTNode *init, RRTNode *goal);
64 std::vector<RRTNode *> &nodes();
65 std::vector<RRTNode *> &dnodes();
66 std::vector<RRTNode *> &samples();
67 std::vector<RRTNode *> iy_[IYSIZE];
68 std::vector<CircleObstacle> *cos();
69 std::vector<SegmentObstacle> *sos();
70 std::vector<float> &clog();
71 std::vector<float> &nlog();
72 std::vector<std::vector<RRTEdge *>> &rlog();
73 std::vector<float> &slog();
74 std::vector<std::vector<RRTNode *>> &tlog();
79 void root(RRTNode *node);
80 void goal(RRTNode *node);
81 bool logr(RRTNode *root);
82 float ocost(RRTNode *n);
83 bool tlog(std::vector<RRTNode *> t);
87 std::vector<CircleObstacle> *cobstacles,
88 std::vector<SegmentObstacle> *sobstacles);
89 bool add_iy(RRTNode *n);
90 bool goal_found(bool f);
96 float (*cost)(RRTNode *, RRTNode *));
97 bool collide(RRTNode *init, RRTNode *goal);
99 bool rebase(RRTNode *nr);
100 std::vector<RRTNode *> findt();
101 std::vector<RRTNode *> findt(RRTNode *n);
105 float cost(RRTNode *init, RRTNode *goal);
106 RRTNode *nn(RRTNode *rs);
107 std::vector<RRTNode *> nv(RRTNode *node, float dist);
108 std::vector<RRTNode *> steer(RRTNode *init, RRTNode *goal);
110 // virtuals - implemented by child classes
111 virtual bool next() = 0;