bcar/src/compare_to_vorobieva.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 /*! \file
   8  * \brief Compare ``Several reversed trials'' [1] with In-slot planner.
   9  *
  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,
  12  * 16(1):396–410.
  13  */
  14
  15 #include <cmath>
  16 #include <iostream>
  17 #include <vector>
  18 #include "pslot.hh"
  19
  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
  25
  26 #define SLOT_WIDTH 2.0
  27 #define SLOT_MAX_LENGTH 5.8
  28 #define SLOT_STEP_LENGTH 0.01
  29
  30 #define PARKING_SPEED -0.001
  31 #define MAX_CUSP 10
  32 #define DELTA_ANGLE_TO_SLOT 0.0001
  33
  34 int main()
  35 {
  36         std::cout << std::fixed;
  37         std::cerr << std::fixed;
  38
  39         bcar::BicycleCar c;
  40         c.ctc(CAR_CURB_TO_CURB);
  41         c.w(CAR_WIDTH);
  42         c.wb(CAR_WHEELBASE);
  43         c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
  44         c.len(CAR_LENGTH);
  45
  46         bcar::Point zp(0.0, 0.0);
  47         double zh = 0.0;
  48         double len = c.len() + SLOT_STEP_LENGTH;
  49
  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);
  55                 c.set_max_steer();
  56                 s.set_max_cusp(MAX_CUSP);
  57                 double nx = s.lrx();
  58                 nx += (c.w() / 2.0 + 0.001) * cos(s.h() - M_PI / 2.0);
  59                 nx += (c.dr() + 0.001) * cos(s.h());
  60                 c.x(nx);
  61                 double ny = s.lry();
  62                 ny += (c.w() / 2.0 + 0.001) * sin(s.h() - M_PI / 2.0);
  63                 ny += (c.dr() + 0.001) * sin(s.h());
  64                 c.y(ny);
  65                 c.h(s.h());
  66                 auto ip = s.drive_of_slot(c);
  67                 if (ip.size() > 0) {
  68                         cusp[0] = s.get_max_cusp();
  69                 }
  70                 // In-slot planner
  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);
  74                 auto pr = s.fe(c);
  75                 if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
  76                                 && pr.e() == 0.0)) {
  77                         cusp[1] = s.get_max_cusp();
  78                 }
  79                 using namespace std;
  80                 cout << SLOT_WIDTH << " " << len << " ";
  81                 cout << cusp[0] << " " << cusp[1] << " ";
  82                 cout << pr << endl;
  83                 len += SLOT_STEP_LENGTH;
  84         }
  85         return 0;
  86 }