From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Mon, 27 Feb 2023 23:31:25 +0000 (+0100)
Subject: Fix pe2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/path-to-traj.git/commitdiff_plain/87686527fb8b8a46ef605d4c34c7428d66826552

Fix pe2
---

diff --git a/path-to-traj.py b/path-to-traj.py
index 7c9dcda..dee4178 100644
--- a/path-to-traj.py
+++ b/path-to-traj.py
@@ -205,6 +205,8 @@ if __name__ == "__main__":
         path = path[:30] + path[32:]
     if PLAN_FILE == "pe1":
         path = path[:34]
+    if PLAN_FILE == "pe2":
+        path = path[:24] + path[27:45]
     cusps = find_cusps(path)
     c = BicycleCar(*path[0][:3])
     del path[0]
@@ -248,19 +250,6 @@ if __name__ == "__main__":
             traj.append(c.pose())
             del cusps[0]
         del path[0]
-        if PLAN_FILE == "pe2":
-            if i == 24:  # needed for pe2
-                del path[0]
-                del path[0]
-                del path[0]
-                del path[0]
-                del path[0]
-            if i == 29:  # needed for pe2
-                del path[0]
-            if i == 32:  # needed for pe2
-                break
-    if PLAN_FILE == "pe2":
-        traj = traj[:-30]  # needed for pe2
     print(f"left {len(path)} waypoints")
     plan["nnpath"] = nnpath
     plan["path"] = npath