From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Mon, 1 Nov 2021 13:00:52 +0000 (+0100)
Subject: Add compare isp binary
X-Git-Tag: v0.7.0^2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/bcar.git/commitdiff_plain/ecde88e34291c7fc800c58087ba5f2024fad09c3

Add compare isp binary
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c67da4b..a27f99b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,6 +23,9 @@ target_link_libraries(compute_pslot_table pslot)
 add_executable(prove_parallel_slot src/prove_parallel_slot.cc)
 target_link_libraries(prove_parallel_slot pslot)
 
+add_executable(compare_isp src/compare_isp.cc)
+target_link_libraries(compare_isp pslot)
+
 if (SKIP_UT)
 	return()
 endif()
diff --git a/src/compare_isp.cc b/src/compare_isp.cc
new file mode 100644
index 0000000..bad7360
--- /dev/null
+++ b/src/compare_isp.cc
@@ -0,0 +1,83 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+/*! \file
+ * \brief Compare ``Several reversed trials'' with In-slot planner.
+ */
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+#include "pslot.hh"
+
+#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_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
+
+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 = c.len() + SLOT_STEP_LENGTH;
+
+	while (true) {
+		int cusp[2] = {-1, -1};
+		bcar::ParkingSlot s(zp, zh, SLOT_MAX_WIDTH, len);
+		// Several reversed trials
+		c.sp(PARKING_SPEED * -1.0);
+		c.set_max_steer();
+		s.set_max_cusp(MAX_CUSP);
+		double nx = s.lrx();
+		nx += (c.w() / 2.0 + 0.001) * cos(s.h() - M_PI / 2.0);
+		nx += (c.dr() + 0.001) * cos(s.h());
+		c.x(nx);
+		double ny = s.lry();
+		ny += (c.w() / 2.0 + 0.001) * sin(s.h() - M_PI / 2.0);
+		ny += (c.dr() + 0.001) * sin(s.h());
+		c.y(ny);
+		c.h(s.h());
+		auto ip = s.drive_of_slot(c);
+		if (ip.size() > 0) {
+			cusp[0] = s.get_max_cusp();
+		}
+		// 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 << len << " " << cusp[0] << " " << cusp[1] << endl;
+		if (cusp[0] == 0 && cusp[1] == 0) {
+			break;
+		}
+		len += SLOT_STEP_LENGTH;
+	}
+	return 0;
+}