6 bool PSPlanner::collide()
8 std::vector<std::tuple<double, double>> bc;
9 bc.push_back(std::make_tuple(this->cc().lfx(), this->cc().lfy()));
10 bc.push_back(std::make_tuple(this->cc().lrx(), this->cc().lry()));
11 bc.push_back(std::make_tuple(this->cc().rrx(), this->cc().rry()));
12 bc.push_back(std::make_tuple(this->cc().rfx(), this->cc().rfy()));
13 bc.push_back(std::make_tuple(this->cc().lfx(), this->cc().lfy()));
14 std::vector<std::tuple<double, double>> ps;
15 ps.push_back(std::make_tuple(this->ps().x1(), this->ps().y1()));
16 ps.push_back(std::make_tuple(this->ps().x2(), this->ps().y2()));
17 ps.push_back(std::make_tuple(this->ps().x3(), this->ps().y3()));
18 ps.push_back(std::make_tuple(this->ps().x4(), this->ps().y4()));
19 return std::get<0>(::collide(bc, ps));
22 bool PSPlanner::forward()
24 double heading = this->ps().heading();
25 while (heading < 0) heading += 2 * M_PI;
26 if (!this->ps().parallel())
28 double h = this->gc().h();
29 while (h < 0) h += 2 * M_PI;
30 if (-0.00001 < heading - h && heading - h < 0.00001)
36 void PSPlanner::guess_gc()
38 double x = this->ps().x1();
39 double y = this->ps().y1();
40 double h = this->ps().heading();
41 double dts = + M_PI / 2; // direction to slot
42 if (this->ps().right())
44 if (this->ps().parallel()) {
45 x += (this->gc().w() / 2 + 0.01) * cos(h + dts);
46 x += (this->gc().dr() + 0.01) * cos(h);
47 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
48 y += (this->gc().dr() + 0.01) * sin(h);
50 x += (this->ps().x4() - this->ps().x1()) / 2;
51 x += (this->gc().df() + 0.01) * cos(h + dts);
52 y += (this->ps().y4() - this->ps().y1()) / 2;
53 y += (this->gc().df() + 0.01) * sin(h + dts);
54 if (this->ps().right())
68 bool PSPlanner::left()
70 double lfx = this->cc().lfx();
71 double lfy = this->cc().lfy();
72 double lrx = this->cc().lrx();
73 double lry = this->cc().lry();
74 double rrx = this->cc().rrx();
75 double rry = this->cc().rry();
76 double rfx = this->cc().rfx();
77 double rfy = this->cc().rfy();
79 (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
80 - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
83 (lrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
84 - (lry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
87 (rrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
88 - (rry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
91 (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
92 - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
94 if (this->ps().parallel())
95 return lfs == rfs && (lfs != lrs || lfs != rrs);
96 else if (!this->forward())
97 return lfs == rfs && (lfs != lrs || lfs != rrs);
99 return lrs == rrs && (lrs != lfs || lrs != rfs);
102 bool PSPlanner::parked()
104 std::vector<std::tuple<double, double>> slot;
105 slot.push_back(std::make_tuple(this->ps().x1(), this->ps().y1()));
106 slot.push_back(std::make_tuple(this->ps().x2(), this->ps().y2()));
107 slot.push_back(std::make_tuple(this->ps().x3(), this->ps().y3()));
108 slot.push_back(std::make_tuple(this->ps().x4(), this->ps().y4()));
109 return inside(this->gc().lfx(), this->gc().lfy(), slot)
110 && inside(this->gc().lrx(), this->gc().lry(), slot)
111 && inside(this->gc().rrx(), this->gc().rry(), slot)
112 && inside(this->gc().rfx(), this->gc().rfy(), slot);
115 std::vector<BicycleCar> PSPlanner::possible_inits(
120 std::vector<BicycleCar> pi;
121 this->cc().sp(this->cc().sp() * -dist);
122 this->cc().st(this->cc().st() * -1);
123 BicycleCar orig_cc(this->cc());
124 for (unsigned int i = 0; i < cnt; i++) {
126 pi.push_back(BicycleCar(this->cc()));
128 this->cc() = BicycleCar(orig_cc);
135 if (this->ps().parallel())
136 return this->fe_parallel();
138 return this->fe_perpendicular();
141 void PSPlanner::fe_parallel()
143 // angle for distance from "entry" corner
144 double dist_angl = this->ps().heading() + M_PI;
145 dist_angl += (this->ps().right()) ? - M_PI / 4 : + M_PI / 4;
146 // set bicycle car `bci` basic dimensions and heading
147 BicycleCar bci = BicycleCar(this->gc());
148 BicycleCar bco = BicycleCar(this->gc());
149 bci.h(this->ps().heading());
150 // move 0.01 from the "entry" corner
151 bci.x(this->ps().x4() + 0.01 * cos(dist_angl));
152 bci.y(this->ps().y4() + 0.01 * sin(dist_angl));
153 // align with parking "top" of slot (move backward)
154 dist_angl = bci.h() + M_PI;
155 bci.x(bci.x() + bci.df() * cos(dist_angl));
156 bci.y(bci.y() + bci.df() * sin(dist_angl));
157 // align with "entry" to pakring slot (move outside)
158 dist_angl = this->ps().heading();
159 dist_angl += (this->ps().right()) ? + M_PI / 2 : - M_PI / 2;
160 bci.x(bci.x() + bci.w() / 2 * cos(dist_angl));
161 bci.y(bci.y() + bci.w() / 2 * sin(dist_angl));
162 // set default speed, steer
163 bci.st(bci.wb() / bci.mtr());
164 if (!this->ps().right())
165 bci.st(bci.st() * -1);
167 // BFS - init all starts
168 // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
169 double dist_diag = sqrt(pow(bci.w() / 2, 2) + pow(bci.df(), 2));
170 if (this->ps().right())
171 dist_angl = atan2(bci.y() - bci.rfy(), bci.x() - bci.rfx());
173 dist_angl = atan2(bci.y() - bci.lfy(), bci.x() - bci.lfx());
174 double DIST_ANGL = dist_angl;
175 std::queue<BicycleCar, std::list<BicycleCar>> q;
179 && dist_angl < DIST_ANGL + 3 * M_PI / 4
183 && dist_angl > DIST_ANGL - 3 * M_PI / 4
186 this->cc() = BicycleCar(bci);
187 if (this->ps().right()) {
188 this->cc().x(bci.rfx() + dist_diag * cos(dist_angl));
189 this->cc().y(bci.rfy() + dist_diag * sin(dist_angl));
191 this->cc().x(bci.lfx() + dist_diag * cos(dist_angl));
192 this->cc().y(bci.lfy() + dist_diag * sin(dist_angl));
194 this->cc().h(this->ps().heading() + dist_angl - DIST_ANGL);
195 if (!this->collide()) {
196 q.push(BicycleCar(this->cc()));
198 dist_angl += (this->ps().right()) ? + 0.01 : - 0.01;
200 // BFS - find entry current car `cc` and corresponding goal car `gc`
201 unsigned int iter_cntr = 0;
202 while (!q.empty() && iter_cntr < 9) {
203 this->cc() = BicycleCar(q.front());
208 this->cc().h() - this->ps().heading()
212 this->cc().sp(this->cc().sp() * -1);
214 this->gc() = BicycleCar(this->cc());
217 this->cc().st(this->cc().st() * -1);
218 q.push(BicycleCar(this->cc()));
219 if (sgn(this->cc().st()) == sgn(q.front().st()))
223 this->gc() = BicycleCar(bco);
225 return this->fer_parallel();
228 void PSPlanner::fe_perpendicular()
230 // TODO Try multiple angles when going from parking slot.
232 // Do not use just the maximum steer angle. Test angles
233 // until the whole current car `cc` is out of the parking
236 // Another approach could be testing angles from the
237 // beginning of the escape parkig slot maneuver.
238 return fer_perpendicular();
241 void PSPlanner::fer()
243 if (this->ps().parallel())
244 return this->fer_parallel();
246 return this->fer_perpendicular();
249 void PSPlanner::fer_parallel()
251 this->cc().st(this->cc().wb() / this->cc().mtr());
252 if (!this->ps().right())
253 this->cc().st(this->cc().st() * -1);
255 while (!this->left()) {
256 while (!this->collide() && !this->left())
258 if (this->left() && !this->collide()) {
261 this->cc().sp(this->cc().sp() * -1);
263 this->cc().st(this->cc().st() * -1);
268 void PSPlanner::fer_perpendicular()
270 bool delta_use[] = {true, true, true};
271 double cc_h = this->cc().h();
274 // check inner radius
275 if (this->forward()) {
284 if (this->ps().right()) {
285 x1 = this->cc().ccr().x();
286 y1 = this->cc().ccr().y();
288 x1 = this->cc().ccl().x();
289 y1 = this->cc().ccl().y();
291 double IR = this->cc().iradi();
295 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
297 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
298 double c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
299 double D = pow(b, 2) - 4 * a * c;
301 delta = -b - sqrt(D);
303 double delta_1 = delta;
305 delta_use[0] = false;
306 // check outer radius
307 if (this->forward()) {
314 IR = this->cc().ofradi();
317 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
319 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
320 c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
321 D = pow(b, 2) - 4 * a * c;
322 if (this->forward()) {
323 delta = -b + sqrt(D);
326 double delta_2 = delta;
328 delta_use[1] = false;
329 delta = -b - sqrt(D);
331 double delta_3 = delta;
333 delta_use[2] = false;
334 if (delta_use[0] && delta_use[1] && delta_use[22])
335 delta = std::max(delta_1, std::max(delta_2, delta_3));
336 else if (delta_use[0] && delta_use[1])
337 delta = std::max(delta_1, delta_2);
338 else if (delta_use[0] && delta_use[2])
339 delta = std::max(delta_1, delta_3);
340 else if (delta_use[1] && delta_use[2])
341 delta = std::max(delta_2, delta_3);
342 else if (delta_use[0])
344 else if (delta_use[1])
346 else if (delta_use[2])
350 // current car `cc` can get out of slot with max steer
351 this->cc().x(this->cc().x() + delta * cos(cc_h));
352 this->cc().y(this->cc().y() + delta * sin(cc_h));
354 // get current car `cc` out of slot
359 this->cc().st(this->cc().wb() / this->cc().mtr());
360 if (this->ps().right())
361 this->cc().st(this->cc().st() * -1);
362 while (!this->left()) {
363 while (!this->collide() && !this->left())
365 if (this->left() && !this->collide()) {
368 this->cc().sp(this->cc().sp() * -1);
370 this->cc().st(this->cc().st() * -1);
375 PSPlanner::PSPlanner()