Add compare to li

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9d5d0025622b1643a560ea365ec4bc0a0114d745..6625d7ace5e3254fc1384bfa1f78b820d9ed3e3c 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,6 +29,9 @@ target_link_libraries(compare_to_vorobieva pslot)
 add_executable(compare_to_zips src/compare_to_zips.cc)
 target_link_libraries(compare_to_zips pslot)
 
+add_executable(compare_to_li src/compare_to_li.cc)
+target_link_libraries(compare_to_li pslot)
+
 if (SKIP_UT)
        return()
 endif()

diff --git a/src/compare_to_li.cc b/src/compare_to_li.cc
new file mode 100644 (file)
index 0000000..773ee1f
--- /dev/null
+++ b/src/compare_to_li.cc
@@ -0,0 +1,64 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+/*! \file
+ * \brief Compare with time-optimal planning [1].
+ *
+ * Li, B., Wang, K., and Shao, Z. (2016). Time-Optimal Maneuver Planning in
+ * Automatic Parallel Park- ing Using a Simultaneous Dynamic Optimization
+ * Approach. IEEE Transactions on Intelligent Trans- portation Systems,
+ * 17(11):3263–3274.
+ */
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+#include "pslot.hh"
+
+#define CAR_CURB_TO_CURB 11.031078891255458
+#define CAR_WIDTH 1.771
+#define CAR_WHEELBASE 2.588
+#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.427
+#define CAR_LENGTH 4.084
+
+#define SLOT_WIDTH 2.0
+#define SLOT_LENGTH 4.5
+
+#define PARKING_SPEED -0.001
+#define MAX_CUSP 25
+#define DELTA_ANGLE_TO_SLOT 0.0001
+
+int main()
+{
+       std::cout << std::fixed;
+       std::cerr << std::fixed;
+
+       bcar::BicycleCar c;
+       c.ctc(CAR_CURB_TO_CURB);
+       c.w(CAR_WIDTH);
+       c.wb(CAR_WHEELBASE);
+       c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
+       c.len(CAR_LENGTH);
+
+       bcar::Point zp(0.0, 0.0);
+       double zh = 0.0;
+
+       int cusp[2] = {-1, -1};
+       bcar::ParkingSlot s(zp, zh, SLOT_WIDTH, SLOT_LENGTH);
+       // In-slot planner
+       s.set_parking_speed(PARKING_SPEED);
+       s.set_max_cusp(MAX_CUSP);
+       s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
+       auto pr = s.fe(c);
+       if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
+                       && pr.e() == 0.0)) {
+               cusp[1] = s.get_max_cusp();
+       }
+       using namespace std;
+       cout << SLOT_WIDTH << " " << SLOT_LENGTH << " ";
+       cout << cusp[1] << " " << pr << endl;
+       return 0;
+}