Add parking slot table computation binary

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9d6a77bf590b158ca1ea2312ad5b6c3381b38104..c5a0d6aacb39af18838c7d0eb9799ef65d480d53 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,9 @@ add_library(pslot STATIC src/pslot.cc)
 target_include_directories(pslot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/incl)
 target_link_libraries(pslot bcar)
 
+add_executable(compute_pslot_table src/compute_pslot_table.cc)
+target_link_libraries(compute_pslot_table pslot)
+
 if (SKIP_UT)
        return()
 endif()

diff --git a/src/compute_pslot_table.cc b/src/compute_pslot_table.cc
new file mode 100644 (file)
index 0000000..8b060ed
--- /dev/null
+++ b/src/compute_pslot_table.cc
@@ -0,0 +1,88 @@
+#include <iostream>
+#include <vector>
+#include "pslot.hh"
+
+#define CAR "fiat_punto"
+#define CAR_CURB_TO_CURB 10.820
+#define CAR_WIDTH 1.625
+#define CAR_WHEELBASE 2.450
+#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.105
+#define CAR_LENGTH 3.760
+
+#define SLOT_MAX_LENGTH 6.5
+#define SLOT_MAX_WIDTH 2.2
+#define SLOT_STEP_LENGTH 0.01
+#define SLOT_STEP_WIDTH 0.01
+
+#define PARKING_SPEED -0.001
+#define MAX_CUSP 10
+#define DELTA_ANGLE_TO_SLOT 0.0001
+
+struct entry {
+       double len = 0.0;
+       double w = 0.0;
+       bcar::PoseRange pr;
+};
+
+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;
+       double len = SLOT_MAX_LENGTH;
+       double w = SLOT_MAX_WIDTH;
+
+       std::vector<struct entry> entries;
+       while (len > c.len()) {
+               if (len > c.perfect_parking_slot_len() + SLOT_STEP_LENGTH) {
+                       len -= SLOT_STEP_LENGTH;
+                       continue;
+               }
+               w = SLOT_MAX_WIDTH;
+               while (w > c.w()) {
+                       bcar::ParkingSlot s(zp, zh, w, len);
+                       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)) {
+                               struct entry e{len, w, pr};
+                               entries.push_back(e);
+                       }
+                       w -= SLOT_STEP_WIDTH;
+               }
+               std::cerr << "Slot length " << len << " done." << std::endl;
+               len -= SLOT_STEP_LENGTH;
+       }
+
+       using namespace std;
+       cout << "/* Generated by `./compute_pslot_table` */" << endl;
+       cout << "#ifndef PSLOT_" << CAR << "_TABLE" << endl;
+       cout << "#define PSLOT_" << CAR << "_TABLE" << endl;
+       cout << "#include \"bcar.hh\"" << endl;
+       cout << "bcar::PoseRange " << "get_" << CAR << "_entry(double len,";
+                       cout << "double w)" << endl;
+       cout << "{" << endl;
+       for (auto e: entries) {
+               cout << "\tif (len >= " << e.len;
+                               cout << " && w >= " << e.w << ") {" << endl;
+                       cout << "\t\treturn bcar::PoseRange(";
+                       cout << e.pr.x() << "," << e.pr.y() << ",";
+                       cout << e.pr.b() << "," << e.pr.e() << ");" << endl;
+               cout << "\t}" << endl;
+       }
+       cout << "\treturn bcar::PoseRange(0.0,0.0,0.0,0.0);" << endl;
+       cout << "}" << endl;
+       cout << "#endif /* PSLOT_" << CAR << "_TABLE */" << endl;
+       return 0;
+}