From 6745994c62788640f960cf8d21fb687f23b489d7 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Tue, 4 Apr 2023 11:23:57 +0200
Subject: [PATCH] if target not in sight condition
---
social_mpc/goal.py | 91 ++++++++++++++++++++--------------------------
1 file changed, 40 insertions(+), 51 deletions(-)
diff --git a/social_mpc/goal.py b/social_mpc/goal.py
index c0ab032..ea94c39 100644
--- a/social_mpc/goal.py
+++ b/social_mpc/goal.py
@@ -61,7 +61,6 @@ class GoalFinder():
if shared_groups is None and shared_humans is None:
continue
if not sh_goto_target_human_id[0] and not sh_goto_target_group_id[0]:
- # print("No target set")
continue
n_human = np.sum(sh_humans[:, -1] >= 0)
if n_human < 1:
@@ -75,8 +74,6 @@ class GoalFinder():
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()
- print("human_ids : {}, {}, {}".format(human_ids, humans, n_human))
- print("group_ids : {}, {}, {}".format(group_ids, sh_groups, n_group))
target = None
if sh_goto_target_human_id[0]:
@@ -89,12 +86,14 @@ class GoalFinder():
target = ('isolated_human', idx) # type, human id
else:
print("Target human not in sight")
+ continue
if sh_goto_target_group_id[0]:
if sh_goto_target_group_id[1] in group_ids:
gr_idx = group_ids.index(sh_goto_target_group_id[1])
target = ('group', gr_idx, sh_goto_target_group_id[1]) # type, group index, group id
else:
print("Target group not in sight")
+ continue
groups = gd.gcff.identify_groups(humans, stride=stride)
for group in groups:
@@ -102,7 +101,6 @@ class GoalFinder():
groups = []
for idx, group_id in enumerate(group_ids):
- print('np.where(humans[:, -2] == group_id)[0]', np.where(humans[:, -2] == group_id)[0], group_id)
groups.append(Group(center=sh_groups[idx, :2], person_ids=np.where(humans[:, -2] == group_id)[0]))
for group in groups:
print('groups from hri listener GOAL: center :{} person_ids : {}'.format(group.center, group.person_ids))
@@ -111,53 +109,44 @@ class GoalFinder():
humans_fdz[:, :3] = humans[:, :3]
f_dsz = ssn.calc_dsz(humans_fdz, groups=groups)
- if target[0] == 'human_in_group':
- print('target', target)
- center = [sh_groups[target[2], 0], sh_groups[target[2], 1]]
- persons = humans[humans[:, -2] == target[3], :]
- print('human_in_group : center {}, persons{}'.format(center, persons))
- goal = ssn.calc_goal_pos(
- f_dsz=f_dsz,
- map=global_map,
- persons=persons,
- group_center=center,
- robot=robot_pose)
- goal = list(goal)
- print('goal human_in_group before {}'.format(goal))
- goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0]) # to face the human
- print('goal human_in_group after {}'.format(goal))
- elif target[0] == 'isolated_human':
- print('target', target)
- 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]
- person = person[np.newaxis, :]
- print('isolated_human : center {}, person{}'.format(center, person))
- print(person.shape)
- goal = ssn.calc_goal_pos(
- f_dsz=f_dsz,
- map=global_map,
- persons=person,
- group_center=center,
- robot=robot_pose)
- goal = list(goal)
- print('goal isolated_human before {}'.format(goal))
- goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0]) # to face the human
- print('goal isolated_human after {}'.format(goal))
- elif target[0] == 'group':
- print('target', target)
- center = [sh_groups[target[1], 0], sh_groups[target[1], 1]]
- persons = humans[humans[:, -2] == target[2], :]
- print('group : center {}, persons{}'.format(center, persons))
- goal = ssn.calc_goal_pos(
- f_dsz=f_dsz,
- map=global_map,
- persons=persons,
- group_center=center,
- robot=robot_pose)
- goal = list(goal)
- print('goal group {}'.format(goal))
+ if target:
+ if target[0] == 'human_in_group':
+ center = [sh_groups[target[2], 0], sh_groups[target[2], 1]]
+ persons = humans[humans[:, -2] == target[3], :]
+ 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[target[1]][1], goal[0] - humans[target[1]][0]) # to face the human
+ 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]
+ person = person[np.newaxis, :]
+ goal = ssn.calc_goal_pos(
+ f_dsz=f_dsz,
+ map=global_map,
+ persons=person,
+ group_center=center,
+ robot=robot_pose)
+ goal = list(goal)
+ goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0]) # to face the human
+ elif target[0] == 'group':
+ center = [sh_groups[target[1], 0], sh_groups[target[1], 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)
+ else:
+ continue
else:
continue
print("goal : {}".format(goal))
--
GitLab