From abcbfccee8910df2e44c585d8d74eb82b6446b75 Mon Sep 17 00:00:00 2001
From: AUTERNAUD Alex <alex.auternaud@inria.fr>
Date: Wed, 28 Feb 2024 10:56:40 +0100
Subject: [PATCH] optimal_path_2d stopping criteria
---
social_mpc/utils.py | 13 ++++++++-----
1 file changed, 8 insertions(+), 5 deletions(-)
diff --git a/social_mpc/utils.py b/social_mpc/utils.py
index 667b37f..bffdf80 100644
--- a/social_mpc/utils.py
+++ b/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])
--
GitLab