From 43e7129061a3ba643c0b9273a447a46b7314da9d Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Wed, 9 Feb 2022 15:05:06 +0100
Subject: [PATCH] if can not find a path issue
---
social_mpc/path.py | 16 ++++++++++------
1 file changed, 10 insertions(+), 6 deletions(-)
diff --git a/social_mpc/path.py b/social_mpc/path.py
index 97a6f48..29300ca 100644
--- a/social_mpc/path.py
+++ b/social_mpc/path.py
@@ -69,6 +69,9 @@ class PathPlanner():
dx=dx,
coords=(x, y),
max_distance=self.max_d)
+
+ if len(xl) == 0 or len(yl) == 0 or len(vxl) == 0 or len(vyl) == 0:
+ return []
d_angle = np.arctan2(vxl[0], -vyl[0]) - pos[2]
if dl[0] < self.max_d_orientation: # if within range, set waypoint = target
wpt_pos = [self.target[0], self.target[1], self.target[2]]
@@ -142,9 +145,10 @@ class PathPlanner():
continue
wpt = self.get_next_waypoint(robot_pose)
-
- with lock:
- shared_waypoint[0] = True
- shared_waypoint[1] = float(wpt[0])
- shared_waypoint[2] = float(wpt[1])
- shared_waypoint[3] = float(wpt[2])
+
+ if wpt:
+ with lock:
+ shared_waypoint[0] = True
+ shared_waypoint[1] = float(wpt[0])
+ shared_waypoint[2] = float(wpt[1])
+ shared_waypoint[3] = float(wpt[2])
\ No newline at end of file
--
GitLab