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/>.
22 #include "reeds_shepp.h"
31 #define ST3TURNINGRADIUS 10.82
35 #define ST4SPEED 0.2 // aka STEP
40 #define ST5TURNINGRADIUS 10.82
43 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
49 std::vector<RRTNode *> nodes;
51 for (i = 1; i < ST1COUNT + 1; i++) {
52 angl = atan2(goal->y() - init->y(), goal->x() - init->x());
53 new_x = init->x() + i * ST1STEP * cos(angl);
54 new_y = init->y() + i * ST1STEP * sin(angl);
56 nodes.push_back(new RRTNode(new_x, new_y, new_h));
61 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
63 std::vector<RRTNode *> nodes;
64 BicycleCar bc(init->x(), init->y(), init->h());
72 for (i = 0; i < ST2COUNT; i++) {
74 angl = atan2(goal->y() - bc.y(), goal->x() - bc.x());
75 steer = angl - bc.h();
76 co = cos(angl - bc.h());
77 si = sin(angl - bc.h());
90 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
95 int cb_st3(double q[4], void *user_data)
97 std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
98 nodes->push_back(new RRTNode(q[0], q[1], q[2]));
99 nodes->back()->s(q[3]);
103 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
105 return st3(init, goal, ST3STEP);
108 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal, float step)
110 std::vector<RRTNode *> nodes;
111 double q0[] = {init->x(), init->y(), init->h()};
112 double q1[] = {goal->x(), goal->y(), goal->h()};
113 ReedsSheppStateSpace rsss(ST3TURNINGRADIUS);
114 rsss.sample(q0, q1, step, cb_st3, &nodes);
118 std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
120 std::vector<RRTNode *> nodes;
121 BicycleCar bc(init->x(), init->y(), init->h());
123 float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
124 float co = cos(angl - init->h());
125 //float si = sin(angl - init->h());
127 float speed = ST4SPEED; // desired
128 float gx = goal->x();
129 float gy = goal->y();
134 float rx = 0; // recomputed goal x
135 float ry = 0; // recomputed goal y
138 for (i = 0; i < ST4COUNT; i++) {
140 cerr = speed - bc.speed();
141 bc.speed(ST4KP * cerr + ST4KI * lerr);
144 rx = (gx - bc.x()) * cos(-bc.h()) -
145 (gy - bc.y()) * sin(-bc.h());
146 ry = (gx - bc.x()) * sin(-bc.h()) -
147 (gy - bc.y()) * cos(-bc.h());
149 r = pow(rx, 2) + pow(ry, 2) / (2 * ry);
154 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
159 int cb_st5(double q[3], double t, void *user_data)
161 std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
162 nodes->push_back(new RRTNode(q[0], q[1], q[2]));
166 std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal)
168 return st5(init, goal, ST5STEP, false);
171 std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, bool bw)
173 return st5(init, goal, ST5STEP, bw);
176 std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, float step, bool bw)
178 std::vector<RRTNode *> nodes;
179 double q0[] = {init->x(), init->y(), init->h()};
180 double q1[] = {goal->x(), goal->y(), goal->h()};
183 dubins_shortest_path(&path, q1, q0, ST5TURNINGRADIUS);
185 dubins_shortest_path(&path, q0, q1, ST5TURNINGRADIUS);
186 dubins_path_sample_many(&path, step, cb_st5, &nodes);
188 std::reverse(nodes.begin(), nodes.end());