groups_visible feature added

social_mpc/controller.py

@@ -293,7 +293,7 @@ class RobotController():
                 self.shared_humans[:n, 3:5] = state.humans_velocity[idx, :]
 
                 self.shared_groups[:, -1] = -1
-                idx = np.where(state.groups_pose[:, -1] >= 0)[0]
+                idx = np.where(state.groups_visible == 0)[0]
                 n = len(idx)
                 self.shared_groups[:n, :2] = state.groups_pose[idx, :]
                 self.shared_groups[:n, -1] = state.groups_id[idx]
@@ -661,9 +661,10 @@ class RobotController():
                 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, 0, 0, 1]
+                    if state.groups_visible[idx] == 0.:
+                        global_pos = state.groups_pose[idx]
+                        pos = utils.global_to_local(state.robot_pose, global_pos)
+                        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]