src/compute_pslot_table.cc

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