From 853d5dc8b4344aedc6e6b5218314c34bd39fd827 Mon Sep 17 00:00:00 2001
From: AUTERNAUD Alex <alex.auternaud@inria.fr>
Date: Wed, 6 Dec 2023 19:26:15 +0100
Subject: [PATCH] waypoint_path updated
---
social_mpc/path.py | 10 ++++++----
social_mpc/utils.py | 4 ++--
2 files changed, 8 insertions(+), 6 deletions(-)
diff --git a/social_mpc/path.py b/social_mpc/path.py
index 70ad5f4..773a83b 100644
--- a/social_mpc/path.py
+++ b/social_mpc/path.py
@@ -67,12 +67,13 @@ class PathPlanner():
def get_next_waypoint(self, pos):
x, y = self.ssn.x_coordinates, self.ssn.y_coordinates
dx = 1/self.state_config.global_map_scale
- max_distance = np.linalg.norm(np.array(pos[:2]) - np.array(self.target[:2]))
+ target_dist = np.linalg.norm(np.array(pos[:2]) - np.array(self.target[:2]))
xl, yl, vxl, vyl, dl = utils.optimal_path_2d(
self.distance, pos[:2],
dx=self.max_d,
coords=(x, y),
- max_distance=max_distance-self.offset_max_distance)
+ max_distance=self.max_d,
+ goal=np.array(self.target[:2]))
# max_distance=self.max_d_orientation)
if len(xl) == 0 or len(yl) == 0 or len(vxl) == 0 or len(vyl) == 0:
@@ -84,9 +85,9 @@ class PathPlanner():
self.waypoints_path[i+2, :] = [self.target[0], self.target[1]]
d_angle = utils.constraint_angle(np.arctan2(yl[0] - pos[1], xl[0] - pos[0]) - pos[2])
- if dl[0] < self.max_d_orientation: # if within range, set waypoint = target
+ if target_dist < 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:
+ elif target_dist < self.max_d:
wpt_pos = [self.target[0],
self.target[1],
np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
@@ -118,6 +119,7 @@ class PathPlanner():
sh_robot_pose = np.array([*shared_robot_pose])
sh_global_map = np.array([*shared_global_map])
shared_path[:] = np.zeros_like(self.waypoints_path)
+ self.waypoints_path = np.zeros_like(self.waypoints_path)
if not sh_target[0]: # not ready
self.distance = None
with lock:
diff --git a/social_mpc/utils.py b/social_mpc/utils.py
index 509c7b8..667b37f 100644
--- a/social_mpc/utils.py
+++ b/social_mpc/utils.py
@@ -103,7 +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,
+def optimal_path_2d(travel_time, starting_point, dx, coords, goal,
N=100, max_distance=None):
"""
Find the optimal path from starting_point to the zero contour
@@ -165,7 +165,7 @@ def optimal_path_2d(travel_time, starting_point, dx, coords,
# xp, vp = sym(x, v)
d = get_distance(x)
if ((max_distance is not None) and
- (starting_distance - d > max_distance)):
+ (np.linalg.norm(goal - np.asarray(x)) < max_distance)):
if dl:
break
xl.append(x[0])
--
GitLab