diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 3a9124aed08c8aca5f8b29891c58b35a2ea1e167..edc9c0e22fd00adc6785bc8b28ee2c4b01beb9cd 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -44,6 +44,7 @@ class RobotController():
self.shared_global_map = None
self.shared_humans = None
+ self.shared_groups = None
self.goto_target_flag = False
self.pan_target_id = None
@@ -194,13 +195,19 @@ class RobotController():
dtype=gmap.dtype,
buffer=self.shm_global_map.buf)
- # 6 = pos + angle + velocity + id
if (state.humans_pose.shape[0] > 0):
humans = np.zeros((state.humans_pose.shape[0], 7)) # x, y, orientation, lin vel, ang vel, group id, id
self.shm_humans = self.smm.SharedMemory(size=humans.nbytes)
self.shared_humans = np.ndarray(shape=humans.shape,
dtype=humans.dtype,
buffer=self.shm_humans.buf)
+
+ if (state.groups_pose.shape[0] > 0):
+ groups = np.zeros((state.groups_pose.shape[0], 3)) # x, y, id
+ self.shm_groups = self.smm.SharedMemory(size=groups.nbytes)
+ self.shared_groups = np.ndarray(shape=groups.shape,
+ dtype=groups.dtype,
+ buffer=self.shm_groups.buf)
else:
list_type = list
@@ -279,13 +286,18 @@ class RobotController():
self.shared_target[0] = True
self.shared_humans[:, -1] = -1
-
idx = np.where(state.humans_visible == 0)[0]
n = len(idx)
self.shared_humans[:n, -1] = state.humans_id[idx]
self.shared_humans[:n, -2] = state.humans_group_id[idx]
self.shared_humans[:n, :3] = state.humans_pose[idx, :]
self.shared_humans[:n, 3:5] = state.humans_velocity[idx, :]
+
+ self.shared_groups[:, -1] = -1
+ idx = np.where(state.groups_pose[:, -1] >= 0)[0]
+ n = len(idx)
+ self.shared_groups[:n, :2] = state.groups_pose[idx, :]
+ self.shared_groups[:n, -1] = state.groups_id[idx]
# if self.shared_goto_target_human_id[0]:
# if self.shared_goto_target_human_id[1] not in self.shared_humans[:, -1]:
# logger.warning("Target human not in FOV, back to ground truth")
@@ -322,7 +334,7 @@ class RobotController():
if target >= 0:
self.goto_target_flag = True
self.set_target_human_id(target)
- elif len(state.humans_group_id) > 0 and target in state.humans_group_id:
+ elif len(state.groups_id) > 0 and target in state.groups_id:
if target >= 0:
self.goto_target_flag = True
self.set_target_group_id(target)
@@ -382,7 +394,7 @@ class RobotController():
if target >= 0:
self.pan_target_id = target
self.pan_target_flag = True
- elif len(state.humans_group_id) > 0 and target in state.humans_group_id:
+ elif len(state.groups_id) > 0 and target in state.groups_id:
if target >= 0:
pass # to integrate look at group
# self.pan_target_id = target
@@ -441,6 +453,7 @@ class RobotController():
self.goal_finder = GoalFinder(state.config, self.controller_config)
self.goal_finder_process = Process(target=self.goal_finder.run,
args=(self.shared_humans,
+ self.shared_groups,
self.shared_target,
self.shared_robot_pose,
self.shared_goto_target_human_id,
diff --git a/social_mpc/goal.py b/social_mpc/goal.py
index 73c6f259e1fd325ccd8a420be2c7401200a542a9..936a0146e50e3c100140cba424cf5b1ca530f3a4 100644
--- a/social_mpc/goal.py
+++ b/social_mpc/goal.py
@@ -8,7 +8,7 @@
# alex.auternaud@inria.fr
# timothee.wintz@inria.fr
-from social_mpc.ssn_model.social_spaces import SocialSpaces
+from social_mpc.ssn_model.social_spaces import SocialSpaces, Group
import social_mpc.ssn_model.group_detection as gd
from social_mpc import utils
@@ -35,6 +35,7 @@ class GoalFinder():
def run(self,
shared_humans,
+ shared_groups,
shared_target,
shared_robot_pose,
shared_goto_target_human_id,
@@ -43,6 +44,7 @@ class GoalFinder():
flags,
lock):
ssn = self.ssn
+ stride = self.controller_config.group_detection_stride
last_time = time.time()
while flags[0]:
with lock:
@@ -59,7 +61,9 @@ class GoalFinder():
sh_goto_target_group_id = np.array([*shared_goto_target_group_id])
if shared_humans is not None:
sh_humans = np.array([*shared_humans])
- else:
+ if shared_groups is not None:
+ sh_groups = np.array([*shared_groups])
+ if shared_groups == None and shared_humans == None:
continue
if not sh_goto_target_human_id[0] and not sh_goto_target_group_id[0]:
logger.debug("No target set")
@@ -67,43 +71,83 @@ class GoalFinder():
n_human = np.sum(sh_humans[:, -1] >= 0)
if n_human < 1:
continue
- humans = np.zeros((n_human, 5))
+ humans = np.zeros((n_human, sh_humans.shape[1]))
for j in range(n_human):
- humans[j, :] = sh_humans[j, :-2]
+ humans[j, :] = sh_humans[j, :]
humans[j, 2] = utils.robot_frame_to_ssn_frame_ang(humans[j, 2])
human_ids = np.array(
sh_humans[sh_humans[:, -1] >= 0, -1],
dtype=int).tolist()
+ group_ids = np.array(sh_groups[sh_groups[:, -1] >= 0, -1], dtype=int).tolist()
logger.debug("human_ids : {}, {}, {}".format(human_ids, humans, n_human))
- target_id = None
+ target = None
if sh_goto_target_human_id[0]:
- target_id = sh_goto_target_human_id[1]
- if target_id not in human_ids:
+ if sh_goto_target_human_id[1] in human_ids:
+ idx = human_ids.index(sh_goto_target_human_id[1])
+ if sh_humans[idx, -2] != -1:
+ target = ('human_in_group', idx, sh_humans[idx, -2]) # type, human id, group id
+ else:
+ target = ('isolated_human', idx) # type, human id
+ else:
logger.warning("Target human not in sight")
- continue
if sh_goto_target_group_id[0]:
- pass
+ if sh_goto_target_group_id[1] in group_ids:
+ idx = group_ids.index(sh_goto_target_group_id[1])
+ target = ('group', idx) # type, group id
+ else:
+ logger.warning("Target group not in sight")
- groups = gd.gcff.identify_groups(humans, stride=self.controller_config.group_detection_stride)
- if target_id:
- idx = human_ids.index(target_id)
+ groups = gd.gcff.identify_groups(humans, stride=stride)
+ if target:
+ idx = human_ids.index(target)
for group in groups:
if idx in group.person_ids:
group = group
break
humans_in_group = humans[group.person_ids, :]
+ print('groups from gcff {}'.format(groups))
+ groups = []
+ for group_id in group_ids:
+ groups.append(Group(center=sh_groups[group_id, :2], person_ids=np.where(humans[:, -2] == group_id)[0]))
+
+ print('groups read from hri listener {}'.format(groups))
f_dsz = ssn.calc_dsz(humans, groups=groups)
- # TODO group_id, group center, group members to integrate
- if target_id:
- goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=group.center, robot=robot_pose)
+
+ if target[0] == 'human_in_group':
+ center = [sh_groups[target[2], 0], sh_groups[target[2], 1]]
+ persons = humans[humans[:, -2] == target[2], :]
+ goal = ssn.calc_goal_pos(
+ f_dsz=f_dsz,
+ map=global_map,
+ persons=persons,
+ group_center=center,
+ robot=robot_pose)
goal = list(goal)
- goal[2] = np.arctan2(goal[1] - humans[idx][1], goal[0] - humans[idx][0])
+ goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[2]][0])
+ elif target[0] == 'isolated_human':
+ person = humans[target[1], :]
+ ts_x = person[0] + np.cos(person[2]) * stride
+ ts_y = person[1] + np.sin(person[2]) * stride
+ center = [ts_x, ts_y]
+ goal = ssn.calc_goal_pos(
+ f_dsz=f_dsz,
+ map=global_map,
+ persons=person,
+ group_center=center,
+ robot=robot_pose)
+ elif target[0] == 'group':
+ center = [sh_groups[target[1], 0], sh_groups[target[1], 1]]
+ persons = humans[humans[:, -2] == target[1], :]
+ goal = ssn.calc_goal_pos(
+ f_dsz=f_dsz,
+ map=global_map,
+ persons=persons,
+ group_center=center,
+ robot=robot_pose)
else:
- center = [0., 0.] # TODO
- humans_in_group = humans[:, :] # TODO
- goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=center, robot=robot_pose)
+ continue
logger.debug("goal : {}".format(goal))
with lock:
diff --git a/social_mpc/ssn_model/social_spaces.py b/social_mpc/ssn_model/social_spaces.py
index a88fa192346b8bd558133d823b07c2a5ab569edb..b2eeb3b1d4f47f83776cb330c6c4ef811183a250 100644
--- a/social_mpc/ssn_model/social_spaces.py
+++ b/social_mpc/ssn_model/social_spaces.py
@@ -10,17 +10,23 @@
# timothee.wintz@inria.fr
import numpy as np
-from social_mpc.ssn_model.group_detection import Group
from scipy import interpolate
+
+class Group:
+ def __init__(self, center, person_ids):
+ self.center = center
+ self.person_ids = person_ids
+
class DynamicGroup(Group):
def __init__(self, theta, v, group):
- super().__init__(group.group_center, group.person_ids)
+ super().__init__(group.center, group.person_ids)
self.theta = theta
self.v = v
class SocialSpacesConfig():
- def __init__(self, default_hand_distance,
+ def __init__(self,
+ default_hand_distance,
A_p,
sigma_p_x,
sigma_p_y,