2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
8 * \brief Compare with two-phased static optimization [1].
10 * [1]: Zips, P., Bock, M., and Kugi, A. (2013). A fast mo- tion planning
11 * algorithm for car parking based on static optimization. In 2013 IEEE/RSJ
12 * Interna- tional Conference on Intelligent Robots and Sys- tems, pages
13 * 2392–2397. ISSN: 2153-0866.
21 #define CAR_CURB_TO_CURB 9.0
23 #define CAR_WHEELBASE 2.7
24 #define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.7
25 #define CAR_LENGTH 4.7
27 #define SLOT_WIDTH 2.2
28 #define SLOT_LENGTH 5.1
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);
49 int cusp[2] = {-1, -1};
50 bcar::ParkingSlot s(zp, zh, SLOT_WIDTH, SLOT_LENGTH);
52 s.set_parking_speed(PARKING_SPEED);
53 s.set_max_cusp(MAX_CUSP);
54 s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
56 if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
58 cusp[1] = s.get_max_cusp();
61 cout << SLOT_WIDTH << " " << SLOT_LENGTH << " ";
62 cout << cusp[1] << " " << pr << endl;