diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index ea08db70633821a879db13efca64a92d06c3c60a..be887f9a4edd7e3912a14911ded4097a18a908bc 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -326,21 +326,20 @@ class RobotController():
self.shared_goto_target_group_id[0] = True
self.shared_goto_target_group_id[1] = group_id
- def set_goto_target(self, state, target):
+ def set_goto_target(self, state, target, group=False):
self.goto_target_flag = False
self.is_failure = False
if isinstance(target, int):
- if len(state.humans_id) > 0 and target in state.humans_id:
- if target >= 0:
+ if target >= 0:
+ if group and len(state.groups_id) > 0 and target in state.groups_id:
self.goto_target_flag = True
- self.set_target_human_id(target)
- elif len(state.groups_id) > 0 and target in state.groups_id:
- if target >= 0:
+ self.set_target_group_id(target)
+ elif len(state.humans_id) > 0 and target in state.humans_id:
self.goto_target_flag = True
- self.set_target_group_id(target)
+ self.set_target_human_id(target)
+ else:
+ logger.warning('Target id does not exist for goto target')
else:
- logger.warning('Target id does not exist for goto target')
- self.is_failure = True
self.controller_config.goto_target_id = -1
with self.lock:
self.shared_goto_target_human_id[0] = False
@@ -384,24 +383,22 @@ class RobotController():
self.is_failure = True
self.controller_config.human_target_id = -1
- def set_pan_target(self, state, target):
+ def set_pan_target(self, state, target, group=False):
self.pan_target_id = None
self.pan_target_pose = None
self.pan_target_flag = False
self.is_failure = False
if isinstance(target, int):
- if len(state.humans_id) > 0 and target in state.humans_id:
- if target >= 0:
- self.pan_target_id = target
+ if target >= 0:
+ if group and len(state.groups_id) > 0 and target in state.groups_id:
+ self.pan_target_id = ('group', target)
self.pan_target_flag = True
- 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
- # self.pan_target_flag = True
+ elif len(state.humans_id) > 0 and target in state.humans_id:
+ self.pan_target_id = ('human', target)
+ self.pan_target_flag = True
+ else:
+ logger.warning('Target id does not exist for pan target')
else:
- logger.warning('Target id does not exist for pan target')
- self.is_failure = True
self.controller_config.pan_target_id = -1
elif len(target) == self.controller_config.target_pose_look_dim and (
isinstance(target, np.ndarray) or all(
@@ -652,16 +649,22 @@ class RobotController():
if self.mode == 3 or self.pan_target_flag:
self.pan_goal = np.zeros(self.controller_config.pan_target_dim)
- humans_id = state.humans_id.tolist()
- if self.pan_target_id in humans_id:
- idx = humans_id.index(self.pan_target_id)
- if state.humans_visible[idx] == 0.:
- global_pos = state.humans_pose[idx, :2]
- vel = state.humans_velocity[idx, :]
+ if self.pan_target_id[0] == 'human':
+ humans_id = state.humans_id.tolist()
+ if self.pan_target_id[1] in humans_id:
+ idx = humans_id.index(self.pan_target_id[1])
+ if state.humans_visible[idx] == 0.:
+ global_pos = state.humans_pose[idx, :2]
+ vel = state.humans_velocity[idx, :]
+ pos = utils.global_to_local(state.robot_pose, global_pos)
+ self.pan_goal[:] = [*pos, *vel, 1]
+ if self.pan_target_id[0] == 'group':
+ groups_id = state.groups_id.tolist()
+ if self.pan_target_id[1] in groups_id:
+ idx = groups_id.index(self.pan_target_id[1])
+ global_pos = state.groups_pose[idx]
pos = utils.global_to_local(state.robot_pose, global_pos)
- self.pan_goal[:] = [
- *pos, *vel, 0]
- self.pan_goal[-1] = 1
+ self.pan_goal[:] = [*pos, 0, 0, 1]
if self.pan_target_pose is not None:
self.pan_goal[:] = [*self.pan_target_pose, 0, 0, 1]
diff --git a/social_mpc/goal.py b/social_mpc/goal.py
index 936a0146e50e3c100140cba424cf5b1ca530f3a4..408931ba5c745df5eea6530e7a505be7be7fadd9 100644
--- a/social_mpc/goal.py
+++ b/social_mpc/goal.py
@@ -75,9 +75,9 @@ class GoalFinder():
for j in range(n_human):
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()
+ human_ids = np.array(sh_humans[sh_humans[:, -1] >= 0, -1], dtype=int).tolist()
+ n_group = np.sum(sh_groups[:, -1] >= 0)
+ sh_groups = sh_groups[n_group, :]
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 = None
@@ -85,8 +85,9 @@ class GoalFinder():
if sh_goto_target_human_id[0]:
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
+ if humans[idx, -2] != -1:
+ gr_idx = group_ids.index(humans[idx, -2])
+ target = ('human_in_group', idx, gr_idx) # type, human id, group id
else:
target = ('isolated_human', idx) # type, human id
else:
@@ -125,7 +126,7 @@ class GoalFinder():
group_center=center,
robot=robot_pose)
goal = list(goal)
- goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[2]][0])
+ goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0])
elif target[0] == 'isolated_human':
person = humans[target[1], :]
ts_x = person[0] + np.cos(person[2]) * stride