standardization dilation and x, y mesh for goal.py and path.py

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

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:

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