create SocialSpaces once

social_mpc/controller.py

@@ -491,12 +491,6 @@ class RobotController():
     def get_social_cost_map(self, state):
         tic = time.time()
         groups = None
-        world_size = state.config.local_map_size
-        ssn = SocialSpaces(
-            bbox=world_size,
-            map_shape=(state.config.local_map_height,
-                       state.config.local_map_width))
-
         ids = np.where(state.humans_visible == 0)[0]
         # ids = ids[ids != state.humans_id.tolist().index(self.human_target_id)]
         n_human = len(ids)
@@ -520,7 +514,7 @@ class RobotController():
                 else:
                     break
 
-        f_dsz = ssn.calc_dsz(humans, groups=groups)
+        f_dsz = self.ssn.calc_dsz(humans, groups=groups)
         rospy.logdebug("Social cost map time : {0}".format(time.time() - tic))
         return f_dsz
 
@@ -720,6 +714,11 @@ class RobotController():
             self.set_targets(state=state)
             self.set_mode()
             self.set_weights()
+            world_size = state.config.local_map_size
+            self.ssn = SocialSpaces(
+                bbox=world_size,
+                map_shape=(state.config.local_map_height,
+                           state.config.local_map_width))
         self.compute_cost_map(state)
 
         if self.update_goals_enabled or self.path_planner_enabled: