From 27bc59e8acf14eed9369e1f1dd807ef1c4a3d8ac Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Thu, 8 Jun 2023 19:06:37 +0200
Subject: [PATCH] create SocialSpaces once

---
 social_mpc/controller.py | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 44a8848..9bbea86 100644
--- a/social_mpc/controller.py
+++ b/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:
-- 
GitLab