diff --git a/social_mpc/goal.py b/social_mpc/goal.py
index 9540b64b38baf6803495d230b46b65b848e4f244..303e47d0cbded1d295c4ef8db34a8d22a8b180af 100644
--- a/social_mpc/goal.py
+++ b/social_mpc/goal.py
@@ -15,10 +15,7 @@ from social_mpc import utils
import numpy as np
import time
import logging
-import multiprocessing
-import cv2
-# logger = multiprocessing.log_to_stderr()
logger = logging.getLogger("controller.goal")
@@ -92,15 +89,11 @@ class GoalFinder():
humans_in_group = humans
f_dsz = ssn.calc_dsz(humans, groups=groups)
- radius = int(
- np.max(np.abs(self.controller_config.wall_avoidance_points)) * self.state_config.global_map_scale) + 1
- kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
- (radius, radius))
- global_map = cv2.dilate(global_map, kernel)
goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=group.center, robot=robot_pose)
- logger.debug("goal : {}".format(goal))
+ logger.debug("goal : {}".format(goal))
+
with lock:
shared_target[1] = True
shared_target[2] = float(goal[0])
shared_target[3] = float(goal[1])
- shared_target[4] = float(goal[2]) + np.pi/2
+ shared_target[4] = float(goal[2]) + np.pi/2
\ No newline at end of file
diff --git a/social_mpc/path.py b/social_mpc/path.py
index 29300cae823e01655f5dd324c1fe169a2152a0f6..8113dd6d4fafbe11e02b850c4b5b5d4c8431de78 100644
--- a/social_mpc/path.py
+++ b/social_mpc/path.py
@@ -9,15 +9,13 @@
# timothee.wintz@inria.fr
import skfmm
-import cv2
import numpy.ma as ma
import numpy as np
import logging
import time
from social_mpc import utils
-import multiprocessing
+from social_mpc.ssn_model.social_spaces import SocialSpaces
-# logger = multiprocessing.log_to_stderr()
logger = logging.getLogger("controller.path")
@@ -30,7 +28,11 @@ class PathPlanner():
self.max_d_orientation = max_d_orientation
self.distance = None
self.path_loop_time = path_loop_time
-
+ world_size = state_config.global_map_size
+ self.ssn = SocialSpaces(
+ bbox=world_size,
+ map_shape=(self.state_config.global_map_height,
+ self.state_config.global_map_width))
logger.setLevel(level=logging_mode)
def position_to_px_coord(self, pos):
@@ -62,14 +64,16 @@ class PathPlanner():
self.distance = skfmm.distance(phi, dx=dx)
def get_next_waypoint(self, pos):
- x, y = self.px_coord_grid()
+ #x, y = self.px_coord_grid()
+ x, y = self.ssn.x_coordinates, self.ssn.y_coordinates
dx = 1/self.state_config.global_map_scale
xl, yl, vxl, vyl, dl = utils.optimal_path_2d(
np.flip(self.distance, axis=0), pos[:2],
dx=dx,
coords=(x, y),
- max_distance=self.max_d)
-
+ #max_distance=self.max_d)
+ max_distance=self.max_d_orientation)
+
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]
@@ -81,7 +85,7 @@ class PathPlanner():
np.arctan2(vxl[0], -vyl[0])]
elif np.abs(d_angle) > self.max_target_angle:
wpt_pos = [xl[0], yl[0], np.arctan2(vxl[0], -vyl[0])]
- else: # else, point on shortest path with orientation towards target
+ else: # else, point on shortest path with orientation towards target
wpt_pos = [xl[-1], yl[-1], np.arctan2(vxl[-1], -vyl[-1])]
return wpt_pos
@@ -112,12 +116,6 @@ class PathPlanner():
if sh_target[1]:
update_distance = True
global_map = np.squeeze(sh_global_map)
- # scale = self.state_config.global_map_scale
- # radius = int(
- # self.robot_config.base_footprint_radius * scale * 2)
- # kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
- # (radius, radius))
- # global_map = cv2.dilate(sh_global_map, kernel)
self.target = [sh_target[2],
sh_target[3], sh_target[4]]
else:
diff --git a/social_mpc/ssn_model/social_spaces.py b/social_mpc/ssn_model/social_spaces.py
index 88eb3eabb305fafa4a41fb5aeae28b081c8dac42..a88fa192346b8bd558133d823b07c2a5ab569edb 100644
--- a/social_mpc/ssn_model/social_spaces.py
+++ b/social_mpc/ssn_model/social_spaces.py
@@ -10,10 +10,8 @@
# timothee.wintz@inria.fr
import numpy as np
-from numpy.lib.shape_base import row_stack
from social_mpc.ssn_model.group_detection import Group
from scipy import interpolate
-from matplotlib import pyplot as plt
class DynamicGroup(Group):
def __init__(self, theta, v, group):
@@ -298,14 +296,7 @@ class SocialSpaces():
if circle_pts.shape[0] > 0:
break
- r *= r_step
-
- extent = [self.bbox[0][0], self.bbox[0][1], self.bbox[1][0], self.bbox[1][1]]
- x = np.stack((map, f_dsz, f_dsz), axis=2)
- # plt.imshow(x, origin='lower', extent=extent)
- # plt.plot(circle_pts[:,0], circle_pts[:,1])
- # plt.show()
-
+ r *= r_step
if robot is not None:
idx = np.argmin((circle_pts[:, 0] - robot[0])**2 + (circle_pts[:, 1] - robot[1])**2)
else:
@@ -313,4 +304,4 @@ class SocialSpaces():
idx = np.argmin(circle_dsz)
x, y = circle_pts[idx, 0], circle_pts[idx, 1]
theta_q = np.arctan2(y - group_center[1], x - group_center[0])
- return x, y, theta_q
+ return x, y, theta_q
\ No newline at end of file