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