2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
8 * \brief Compare ``Several reversed trials'' [1] with In-slot planner.
10 * [1]: Vorobieva, H., Glaser, S., Minoiu-Enache, N., and Mammar, S. (2015).
11 * Automatic Parallel Parking in Tiny Spots: Path Planning and Control. IEEE,
20 #define CAR_CURB_TO_CURB 11.031078891255458
21 #define CAR_WIDTH 1.771
22 #define CAR_WHEELBASE 2.588
23 #define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.427
24 #define CAR_LENGTH 4.084
26 #define SLOT_WIDTH 2.0
27 #define SLOT_MAX_LENGTH 5.8
28 #define SLOT_STEP_LENGTH 0.01
30 #define PARKING_SPEED -0.001
32 #define DELTA_ANGLE_TO_SLOT 0.0001
36 std::cout << std::fixed;
37 std::cerr << std::fixed;
40 c.ctc(CAR_CURB_TO_CURB);
43 c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
46 bcar::Point zp(0.0, 0.0);
48 double len = c.len() + SLOT_STEP_LENGTH;
50 while (len < SLOT_MAX_LENGTH) {
51 int cusp[2] = {-1, -1};
52 bcar::ParkingSlot s(zp, zh, SLOT_WIDTH, len);
53 // Several reversed trials
54 c.sp(PARKING_SPEED * -1.0);
56 s.set_max_cusp(MAX_CUSP);
58 nx += (c.w() / 2.0 + 0.001) * cos(s.h() - M_PI / 2.0);
59 nx += (c.dr() + 0.001) * cos(s.h());
62 ny += (c.w() / 2.0 + 0.001) * sin(s.h() - M_PI / 2.0);
63 ny += (c.dr() + 0.001) * sin(s.h());
66 auto ip = s.drive_of_slot(c);
68 cusp[0] = s.get_max_cusp();
71 s.set_parking_speed(PARKING_SPEED);
72 s.set_max_cusp(MAX_CUSP);
73 s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
75 if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
77 cusp[1] = s.get_max_cusp();
80 cout << SLOT_WIDTH << " " << len << " ";
81 cout << cusp[0] << " " << cusp[1] << " ";
83 len += SLOT_STEP_LENGTH;