gcff removed from controller.py

social_mpc/controller.py

@@ -28,7 +28,7 @@ import os
 from social_mpc.mpc import MPC
 from social_mpc.path import PathPlanner
 from social_mpc.goal import GoalFinder
-from social_mpc.ssn_model.social_spaces import SocialSpaces
+from social_mpc.ssn_model.social_spaces import SocialSpaces, Group
 from social_mpc import utils
 from social_mpc.config.config import ControllerConfig
 
@@ -512,6 +512,16 @@ class RobotController():
                 local_position, local_angle, state.humans_velocity[i, 0], 0
         if n_human > 1:
             groups = gd.gcff.identify_groups(humans, stride=self.controller_config.group_detection_stride)
+            print('get_social_cost_map: groups from gcff {}'.format(groups))
+
+            groups = []
+            for idx, group_id in enumerate(state.groups_id):
+                if group_id != -1:
+                    groups.append(Group(center=state.groups_pose[idx], person_ids=np.where(state.humans_group_id == group_id)[0]))
+                else:
+                    break
+            print('get_social_cost_map: groups read from hri listener {}'.format(groups))
+
         f_dsz = ssn.calc_dsz(humans, groups=groups)
         f_dsz = np.flip(f_dsz, axis=0)
         logger.debug("Social cost map time : {0}".format(time.time() - tic))