debugging group mode

social_mpc/goal.py

@@ -91,8 +91,8 @@ class GoalFinder():
                     print("Target human not in sight")
             if sh_goto_target_group_id[0]:
                 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
+                    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")
 
@@ -148,7 +148,7 @@ class GoalFinder():
             elif target[0] == 'group':
                 print('target', target)
                 center = [sh_groups[target[1], 0], sh_groups[target[1], 1]]
-                persons = humans[humans[:, -2] == target[1], :]
+                persons = humans[humans[:, -2] == target[2], :]
                 print('group : center {}, persons{}'.format(center, persons))
                 goal = ssn.calc_goal_pos(
                     f_dsz=f_dsz,