bcar/src/compute_pslot_table.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 #include <iostream>
   8 #include <vector>
   9 #include "pslot.hh"
  10
  11 #define CAR "fiat_punto"
  12 #define CAR_CURB_TO_CURB 10.820
  13 #define CAR_WIDTH 1.625
  14 #define CAR_WHEELBASE 2.450
  15 #define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.105
  16 #define CAR_LENGTH 3.760
  17
  18 #define SLOT_MAX_LENGTH 6.5
  19 #define SLOT_MAX_WIDTH 2.2
  20 #define SLOT_STEP_LENGTH 0.01
  21 #define SLOT_STEP_WIDTH 0.01
  22
  23 #define PARKING_SPEED -0.001
  24 #define MAX_CUSP 10
  25 #define DELTA_ANGLE_TO_SLOT 0.0001
  26
  27 struct entry {
  28         double len = 0.0;
  29         double w = 0.0;
  30         bcar::PoseRange pr;
  31 };
  32
  33 int main()
  34 {
  35         std::cout << std::fixed;
  36         std::cerr << std::fixed;
  37
  38         bcar::BicycleCar c;
  39         c.ctc(CAR_CURB_TO_CURB);
  40         c.w(CAR_WIDTH);
  41         c.wb(CAR_WHEELBASE);
  42         c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
  43         c.len(CAR_LENGTH);
  44
  45         bcar::Point zp(0.0, 0.0);
  46         double zh = 0.0;
  47         double len = SLOT_MAX_LENGTH;
  48         double w = SLOT_MAX_WIDTH;
  49
  50         std::vector<struct entry> entries;
  51         while (len > c.len()) {
  52                 if (len > c.perfect_parking_slot_len() + SLOT_STEP_LENGTH) {
  53                         len -= SLOT_STEP_LENGTH;
  54                         continue;
  55                 }
  56                 w = SLOT_MAX_WIDTH;
  57                 while (w > c.w()) {
  58                         bcar::ParkingSlot s(zp, zh, w, len);
  59                         s.set_parking_speed(PARKING_SPEED);
  60                         s.set_max_cusp(MAX_CUSP);
  61                         s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
  62                         auto pr = s.fe(c);
  63                         if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
  64                                         && pr.e() == 0.0)) {
  65                                 struct entry e{len, w, pr};
  66                                 entries.push_back(e);
  67                         }
  68                         w -= SLOT_STEP_WIDTH;
  69                 }
  70                 std::cerr << "Slot length " << len << " done." << std::endl;
  71                 len -= SLOT_STEP_LENGTH;
  72         }
  73
  74         using namespace std;
  75         cout << "/* Generated by `./compute_pslot_table` */" << endl;
  76         cout << "#ifndef PSLOT_" << CAR << "_TABLE" << endl;
  77         cout << "#define PSLOT_" << CAR << "_TABLE" << endl;
  78         cout << "#include \"bcar.hh\"" << endl;
  79         cout << "bcar::PoseRange " << "get_" << CAR << "_entry(double len,";
  80                         cout << "double w)" << endl;
  81         cout << "{" << endl;
  82         for (auto e: entries) {
  83                 cout << "\tif (len >= " << e.len;
  84                                 cout << " && w >= " << e.w << ") {" << endl;
  85                         cout << "\t\treturn bcar::PoseRange(";
  86                         cout << e.pr.x() << "," << e.pr.y() << ",";
  87                         cout << e.pr.b() << "," << e.pr.e() << ");" << endl;
  88                 cout << "\t}" << endl;
  89         }
  90         cout << "\treturn bcar::PoseRange(0.0,0.0,0.0,0.0);" << endl;
  91         cout << "}" << endl;
  92         cout << "#endif /* PSLOT_" << CAR << "_TABLE */" << endl;
  93         return 0;
  94 }