From cbdb0d03143ea22770a753b6e5aafc5b9824bd1c Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Tue, 11 Oct 2022 13:45:46 +0200
Subject: [PATCH] gcff removed from controller.py
---
social_mpc/controller.py | 12 +++++++++++-
1 file changed, 11 insertions(+), 1 deletion(-)
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index edc9c0e..ea08db7 100644
--- a/social_mpc/controller.py
+++ b/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))
--
GitLab