diff --git a/social_mpc/controller.py b/social_mpc/controller.py index 44a8848a9d9b1db5321cdc388e25d9987a25d51a..9bbea865b7eca2dad984f0cb4e5d1efd42f05049 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: