Find cusps in path

diff --git a/path-to-traj.py b/path-to-traj.py
index f4f64f6e3407585c56c3981025e9c49743aa21ea..1e9331a3b3571e4fe658f635b08d9f02640f8c91 100755 (executable)
--- a/path-to-traj.py
+++ b/path-to-traj.py
@@ -143,12 +143,12 @@ def find_segments_of(path):
     return segments
 
 
-def find_cusps_in(segments):
-    """Cusp pose is the first pose where the direction changed."""
+def find_cusps_in(path):
+    """Cusp pose is the pose where the direction changed."""
     cusps = []
-    for i in range(1, len(segments)):
-        if segments[i-1].direction() != segments[i].direction():
-            cusps.append(segments[i].init_index)
+    for i in range(len(path)):
+        if path[i][5]:
+            cusps.append(i)
     return cusps
 
 
@@ -399,19 +399,6 @@ class BicycleCar:
                 self.wa = 0
 
 
-def find_cusps(path):
-    assert len(path) > 1
-    cusps_i = []
-    for i in range(1, len(path) - 1):
-        if path[i][3] == 0:
-            if sgn(path[i-1][3]) != sgn(path[i+1][3]):
-                cusps_i.append(i)
-        else:
-            if sgn(path[i][3]) != sgn(path[i+1][3]) and path[i+1][3] != 0:
-                cusps_i.append(i)
-    return cusps_i
-
-
 if __name__ == "__main__":
     plan = None
     if len(sys.argv) == 2:
@@ -421,13 +408,12 @@ if __name__ == "__main__":
     plan["nnpath"] = list(plan["path"])
     plan["path"] = normalize_path(plan["path"])
     path = list(plan["path"])
-    cusps = find_cusps(path)
+    cusps = find_cusps_in(path)
     path[0][3] = path[1][3]  # initial point is part of the first segment
     path[0][4] = path[1][4]
     path[-1][3] = path[-2][3]  # goal point is part of the last segment
     path[-1][4] = path[-2][4]
     path_segments = find_segments_of(path)
-    cusps = find_cusps_in(path_segments)
     print(f"path segments ({len(path_segments)}):")
     for s in path_segments:
         print(f"- {s}")
@@ -450,8 +436,7 @@ if __name__ == "__main__":
     # Interpolate over segments.
     for i in range(len(path_segments)):
         s = path_segments[i]
-        s_is_cusp = False if i + 1 >= len(path_segments) else (
-            sgn(s.direction()) != sgn(path_segments[i+1].direction()))
+        s_is_cusp = i in cusps
         # Comment the following three lines to have the single
         # BicycleCar model over all the segments without reseting the
         # model at the beginning of each segment.