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::left()
24 double lfx = this->cc().lfx();
25 double lfy = this->cc().lfy();
26 double lrx = this->cc().lrx();
27 double lry = this->cc().lry();
28 double rrx = this->cc().rrx();
29 double rry = this->cc().rry();
30 double rfx = this->cc().rfx();
31 double rfy = this->cc().rfy();
33 (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
34 - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
37 (lrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
38 - (lry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
41 (rrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
42 - (rry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
45 (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
46 - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
48 if (this->ps().parallel())
49 return lfs == rfs && (lfs != lrs || lfs != rrs);
50 else if (!this->forward())
51 return lfs == rfs && (lfs != lrs || lfs != rrs);
53 return lrs == rrs && (lrs != lfs || lrs != rfs);
56 bool PSPlanner::parked()
58 std::vector<std::tuple<double, double>> slot;
59 slot.push_back(std::make_tuple(this->ps().x1(), this->ps().y1()));
60 slot.push_back(std::make_tuple(this->ps().x2(), this->ps().y2()));
61 slot.push_back(std::make_tuple(this->ps().x3(), this->ps().y3()));
62 slot.push_back(std::make_tuple(this->ps().x4(), this->ps().y4()));
63 return inside(this->gc().lfx(), this->gc().lfy(), slot)
64 && inside(this->gc().lrx(), this->gc().lry(), slot)
65 && inside(this->gc().rrx(), this->gc().rry(), slot)
66 && inside(this->gc().rfx(), this->gc().rfy(), slot);
72 if (this->ps().parallel())
73 return this->fe_parallel();
75 return this->fe_perpendicular();
78 void PSPlanner::fe_parallel()
80 // angle for distance from "entry" corner
81 double dist_angl = this->ps().heading() + M_PI;
82 dist_angl += (this->ps().right()) ? - M_PI / 4 : + M_PI / 4;
83 // set bicycle car `bci` basic dimensions and heading
84 BicycleCar bci = BicycleCar(this->gc());
85 BicycleCar bco = BicycleCar(this->gc());
86 bci.h(this->ps().heading());
87 // move 0.01 from the "entry" corner
88 bci.x(this->ps().x4() + 0.01 * cos(dist_angl));
89 bci.y(this->ps().y4() + 0.01 * sin(dist_angl));
90 // align with parking "top" of slot (move backward)
91 dist_angl = bci.h() + M_PI;
92 bci.x(bci.x() + bci.df() * cos(dist_angl));
93 bci.y(bci.y() + bci.df() * sin(dist_angl));
94 // align with "entry" to pakring slot (move outside)
95 dist_angl = this->ps().heading();
96 dist_angl += (this->ps().right()) ? + M_PI / 2 : - M_PI / 2;
97 bci.x(bci.x() + bci.w() / 2 * cos(dist_angl));
98 bci.y(bci.y() + bci.w() / 2 * sin(dist_angl));
99 // BFS - init all starts
100 // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
101 double dist_diag = sqrt(pow(bci.w() / 2, 2) + pow(bci.df(), 2));
102 if (this->ps().right())
103 dist_angl = atan2(bci.y() - bci.rfy(), bci.x() - bci.rfx());
105 dist_angl = atan2(bci.y() - bci.lfy(), bci.x() - bci.lfx());
106 double DIST_ANGL = dist_angl;
107 std::queue<BicycleCar, std::list<BicycleCar>> q;
111 && dist_angl < DIST_ANGL + 3 * M_PI / 4
115 && dist_angl > DIST_ANGL - 3 * M_PI / 4
118 this->cc() = BicycleCar(bci);
119 if (this->ps().right()) {
120 this->cc().x(bci.rfx() + dist_diag * cos(dist_angl));
121 this->cc().y(bci.rfy() + dist_diag * sin(dist_angl));
123 this->cc().x(bci.lfx() + dist_diag * cos(dist_angl));
124 this->cc().y(bci.lfy() + dist_diag * sin(dist_angl));
126 this->cc().h(this->ps().heading() + dist_angl - DIST_ANGL);
127 if (!this->collide()) {
128 this->cc().st(this->cc().wb() / this->cc().mtr());
129 if (!this->ps().right())
130 this->cc().st(this->cc().st() * -1);
131 this->cc().sp(-0.01);
132 q.push(BicycleCar(this->cc()));
134 dist_angl += (this->ps().right()) ? + 0.01 : - 0.01;
136 // BFS - find entry current car `cc` and corresponding goal car `gc`
137 unsigned int iter_cntr;
138 while (!q.empty() && iter_cntr < 9) {
139 this->cc() = BicycleCar(q.front());
144 this->cc().h() - this->ps().heading()
148 this->cc().sp(this->cc().sp() * -1);
150 this->gc() = BicycleCar(this->cc());
153 this->cc().st(this->cc().st() * -1);
154 q.push(BicycleCar(this->cc()));
155 if (sgn(this->cc().st()) == sgn(q.front().st()))
159 this->gc() = BicycleCar(bco);
161 return this->fer_parallel();
164 void PSPlanner::fe_perpendicular()
166 // TODO Try multiple angles when going from parking slot.
168 // Do not use just the maximum steer angle. Test angles
169 // until the whole current car `cc` is out of the parking
172 // Another approach could be testing angles from the
173 // beginning of the escape parkig slot maneuver.
174 return fer_perpendicular();
177 void PSPlanner::fer()
179 if (this->ps().parallel())
180 return this->fer_parallel();
182 return this->fer_perpendicular();
185 void PSPlanner::fer_parallel()
187 this->cc().st(this->cc().wb() / this->cc().mtr());
188 if (!this->ps().right())
189 this->cc().st(this->cc().st() * -1);
191 while (!this->left()) {
192 while (!this->collide() && !this->left())
194 if (this->left() && !this->collide()) {
197 this->cc().sp(this->cc().sp() * -1);
199 this->cc().st(this->cc().st() * -1);
204 void PSPlanner::fer_perpendicular()
206 double cc_h = this->cc().h();
209 // check inner radius
210 if (this->forward()) {
219 if (this->ps().right()) {
220 x1 = this->cc().ccr().x();
221 y1 = this->cc().ccr().y();
223 x1 = this->cc().ccl().x();
224 y1 = this->cc().ccl().y();
226 double IR = this->cc().iradi();
230 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
232 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
233 double c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
234 double D = D = pow(b, 2) - 4 * a * c;
236 delta = -b - sqrt(D);
238 double delta_1 = delta;
239 // check outer radius
240 if (this->forward()) {
247 IR = this->cc().ofradi();
250 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
252 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
253 c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
254 D = pow(b, 2) - 4 * a * c;
255 if (this->forward()) {
256 delta = -b + sqrt(D);
259 double delta_2 = delta;
260 delta = -b - sqrt(D);
262 double delta_3 = delta;
263 delta = std::max(delta_1, std::max(delta_2, delta_3));
264 // current car `cc` can get out of slot with max steer
265 this->cc().x(this->cc().x() + delta * cos(cc_h));
266 this->cc().y(this->cc().y() + delta * sin(cc_h));
268 // get current car `cc` out of slot
273 this->cc().st(this->cc().wb() / this->cc().mtr());
274 if (this->ps().right())
275 this->cc().st(this->cc().st() * -1);
276 while (!this->left()) {
277 while (!this->collide() && !this->left())
279 if (this->left() && !this->collide()) {
282 this->cc().sp(this->cc().sp() * -1);
284 this->cc().st(this->cc().st() * -1);
289 bool PSPlanner::forward()
291 double heading = this->ps().heading();
292 while (heading < 0) heading += 2 * M_PI;
293 if (!this->ps().parallel())
295 double h = this->gc().h();
296 while (h < 0) h += 2 * M_PI;
297 if (-0.00001 < heading - h && heading - h < 0.00001)
303 PSPlanner::PSPlanner()