bcar/src/compare_to_li.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 with time-optimal planning [1].
   9  *
  10  * Li, B., Wang, K., and Shao, Z. (2016). Time-Optimal Maneuver Planning in
  11  * Automatic Parallel Park- ing Using a Simultaneous Dynamic Optimization
  12  * Approach. IEEE Transactions on Intelligent Trans- portation Systems,
  13  * 17(11):3263–3274.
  14  */
  15
  16 #include <cmath>
  17 #include <iostream>
  18 #include <vector>
  19 #include "pslot.hh"
  20
  21 #define CAR_CURB_TO_CURB 11.031078891255458
  22 #define CAR_WIDTH 1.771
  23 #define CAR_WHEELBASE 2.588
  24 #define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.427
  25 #define CAR_LENGTH 4.084
  26
  27 #define SLOT_WIDTH 2.0
  28 #define SLOT_LENGTH 4.5
  29
  30 #define PARKING_SPEED -0.001
  31 #define MAX_CUSP 25
  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
  49         int cusp[2] = {-1, -1};
  50         bcar::ParkingSlot s(zp, zh, SLOT_WIDTH, SLOT_LENGTH);
  51         // In-slot planner
  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);
  55         auto pr = s.fe(c);
  56         if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
  57                         && pr.e() == 0.0)) {
  58                 cusp[1] = s.get_max_cusp();
  59         }
  60         using namespace std;
  61         cout << SLOT_WIDTH << " " << SLOT_LENGTH << " ";
  62         cout << cusp[1] << " " << pr << endl;
  63         return 0;
  64 }