optimal_path_2d stopping criteria

social_mpc/utils.py

@@ -103,8 +103,7 @@ def constraint_angle(angle, min_value=-np.pi, max_value=np.pi):
     return new_angle
 
 
-def optimal_path_2d(travel_time, starting_point, dx, coords, goal,
-                    N=100, max_distance=None):
+def optimal_path_2d(travel_time, starting_point, dx, coords, goal=None, max_d_orientation=None, N=100):
     """
     Find the optimal path from starting_point to the zero contour
     of travel_time. dx is the grid spacing
@@ -163,9 +162,13 @@ def optimal_path_2d(travel_time, starting_point, dx, coords, goal,
 
     for i in range(N):
         # xp, vp = sym(x, v)
-        d = get_distance(x)
-        if ((max_distance is not None) and
-                (np.linalg.norm(goal - np.asarray(x)) < max_distance)):
+        d = get_distance(x)[0][0]
+        # if negative value d < 0, waypoint in an obstacle
+        if d < 0:
+            return False
+        # goal is implicit defined in the travel_time map but we need to test it explicitly because we can not really trust d
+        if ((goal is not None and max_d_orientation is not None) and
+                (np.linalg.norm(goal - np.asarray(x)) < max_d_orientation)):
             if dl:
                 break
         xl.append(x[0])