if target not in sight condition

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))