src/compare_isp.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'' with In-slot planner.
   9  */
  10
  11 #include <cmath>
  12 #include <iostream>
  13 #include <vector>
  14 #include "pslot.hh"
  15
  16 #define CAR_CURB_TO_CURB 10.820
  17 #define CAR_WIDTH 1.625
  18 #define CAR_WHEELBASE 2.450
  19 #define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.105
  20 #define CAR_LENGTH 3.760
  21
  22 #define SLOT_MAX_WIDTH 2.2
  23 #define SLOT_STEP_LENGTH 0.01
  24 #define SLOT_STEP_WIDTH 0.01
  25
  26 #define PARKING_SPEED -0.001
  27 #define MAX_CUSP 10
  28 #define DELTA_ANGLE_TO_SLOT 0.0001
  29
  30 int main()
  31 {
  32         std::cout << std::fixed;
  33         std::cerr << std::fixed;
  34
  35         bcar::BicycleCar c;
  36         c.ctc(CAR_CURB_TO_CURB);
  37         c.w(CAR_WIDTH);
  38         c.wb(CAR_WHEELBASE);
  39         c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
  40         c.len(CAR_LENGTH);
  41
  42         bcar::Point zp(0.0, 0.0);
  43         double zh = 0.0;
  44         double len = c.len() + SLOT_STEP_LENGTH;
  45
  46         while (true) {
  47                 int cusp[2] = {-1, -1};
  48                 bcar::ParkingSlot s(zp, zh, SLOT_MAX_WIDTH, len);
  49                 // Several reversed trials
  50                 c.sp(PARKING_SPEED * -1.0);
  51                 c.set_max_steer();
  52                 s.set_max_cusp(MAX_CUSP);
  53                 double nx = s.lrx();
  54                 nx += (c.w() / 2.0 + 0.001) * cos(s.h() - M_PI / 2.0);
  55                 nx += (c.dr() + 0.001) * cos(s.h());
  56                 c.x(nx);
  57                 double ny = s.lry();
  58                 ny += (c.w() / 2.0 + 0.001) * sin(s.h() - M_PI / 2.0);
  59                 ny += (c.dr() + 0.001) * sin(s.h());
  60                 c.y(ny);
  61                 c.h(s.h());
  62                 auto ip = s.drive_of_slot(c);
  63                 if (ip.size() > 0) {
  64                         cusp[0] = s.get_max_cusp();
  65                 }
  66                 // In-slot planner
  67                 s.set_parking_speed(PARKING_SPEED);
  68                 s.set_max_cusp(MAX_CUSP);
  69                 s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
  70                 auto pr = s.fe(c);
  71                 if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
  72                                 && pr.e() == 0.0)) {
  73                         cusp[1] = s.get_max_cusp();
  74                 }
  75                 using namespace std;
  76                 cout << len << " " << cusp[0] << " " << cusp[1] << endl;
  77                 if (cusp[0] == 0 && cusp[1] == 0) {
  78                         break;
  79                 }
  80                 len += SLOT_STEP_LENGTH;
  81         }
  82         return 0;
  83 }