Fix pe2

diff --git a/path-to-traj.py b/path-to-traj.py
index 7c9dcdaa762d79764d2302cd75acaa7ff3409ec5..dee417830c82dd610bf82b28aae54a518566446c 100644 (file)
--- 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