From 1ad5c89c2f90f90bd1937adbbd4df13a8dab2470 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Tue, 7 Jun 2022 10:51:13 +0200
Subject: [PATCH] angle issue
---
social_mpc/path.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/social_mpc/path.py b/social_mpc/path.py
index 8113dd6..731274f 100644
--- a/social_mpc/path.py
+++ b/social_mpc/path.py
@@ -76,7 +76,7 @@ class PathPlanner():
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]
+ d_angle = utils.constraint_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]]
elif dl[0] < self.max_d:
--
GitLab