import cv2 import numpy as np import socket import threading from ultralytics import YOLO from pythonosc.dispatcher import Dispatcher from pythonosc.osc_server import ThreadingOSCUDPServer from pythonosc.udp_client import SimpleUDPClient from gesture_runtime import GestureRecognizer MODEL_CANDIDATES = [ "yolo11n-pose.pt", # current Ultralytics small pose model "yolov8n-pose.pt", # older fallback ] CAMERA_INDEX = 0 VIDEO = r"C:\Users\enjaf\Documents\Michi & Konzi\Uni\6.Sem\Interaktion\AI_Interaktion_Python_Code_002\Interaktion\vid2.mp4" OSC_TARGET_IP = "100.83.253.33" #VIDEO_INPUT = VIDEO VIDEO_INPUT = CAMERA_INDEX CONF_THRESHOLD = 0.35 IOU_THRESHOLD = 0.50 KEYPOINT_CONF_THRESHOLD = 0.35 PERSON_PADDING_RATIO = 0.15 GESTURE_DATA_PICKLE = "gesture_samples.pkl" GESTURE_ANALYSIS_PICKLE = "gesture_feature_analysis.pkl" GESTURE_FEATURE_FAMILY = "ANGLES" GESTURE_CLASSIFIER_TYPE = "knn" GESTURE_RANKING_METHOD = "fisher" GESTURE_NUM_FEATURES = 10 GESTURE_UNKNOWN_THRESHOLD = 0.7 GESTURE_MODEL_PATH = "hand_landmarker.task" OSC_RECEIVER_IP = "0.0.0.0" OSC_RECEIVER_PORT = 9000 OSC_TARGET_PORT = 9000 # COCO 17-keypoint skeleton used by YOLO human pose models POSE_CONNECTIONS = [ (0, 1), (0, 2), (1, 3), (2, 4), # face (5, 6), # shoulders (5, 7), (7, 9), # left arm (6, 8), (8, 10), # right arm (5, 11), (6, 12), (11, 12), # torso (11, 13), (13, 15), # left leg (12, 14), (14, 16), # right leg ] persons_of_interest = set() # Set, um die Indizes der interessanten Personen zu speichern persons_of_interest_lock = threading.Lock() target_persons = set() # Set mit Personen, auf die gezeigt werden kann target_persons_lock = threading.Lock() LEFT_SHOULDER = 5 RIGHT_SHOULDER = 6 LEFT_HIP = 11 RIGHT_HIP = 12 ARUCO_ASSIGN_DISTANCE_THRESHOLD = 840.0 ARUCO_SLEEP_MISSING_FRAMES = 3 POINTING_STABLE_FRAMES = 6 aruco_person_assignments = {} aruco_person_missing_counts = {} aruco_person_sleeping = {} aruco_state_lock = threading.Lock() # When True, do not auto-reassign ArUco markers during normal frame updates. # Calling `/getAllIds` will perform a fresh assignment and set this to True. aruco_mapping_locked = False # Latest per-frame snapshots used by the `/getAllIds` handler to create a mapping # from currently visible persons to markers when the game starts. latest_marker_centers = {} latest_person_centers = {} pointing_target_tracker = {} pointing_target_lock = threading.Lock() osc_client_global = None osc_status_enabled = True # Confirmation wait state: set by OSC `on_confirmation` to a set of player track_ids to listen to. # When all players show matching gestures (B or C), we send the result. confirmation_poi_ids = set() confirmation_gestures = {} # Maps track_id -> gesture_label (e.g., "A", "B", "C") confirmation_stable_counter = 0 confirmation_last_consensus = None confirmation_lock = threading.Lock() def get_local_ip() -> str: s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) try: s.connect(("8.8.8.8", 80)) ip = s.getsockname()[0] except OSError: ip = "127.0.0.1" finally: s.close() return ip def toggle_person_of_interest(index): with persons_of_interest_lock: if index in persons_of_interest: persons_of_interest.remove(index) return False persons_of_interest.add(index) return True def clear_persons_of_interest(): with persons_of_interest_lock: persons_of_interest.clear() def get_persons_of_interest_snapshot(): with persons_of_interest_lock: return sorted(persons_of_interest) def toggle_target_person(index): with target_persons_lock: if index in target_persons: target_persons.remove(index) return False target_persons.add(index) return True def clear_target_persons(): with target_persons_lock: target_persons.clear() def add_all_target_persons(ids): """Initialize target_persons with the provided list of track IDs.""" with target_persons_lock: target_persons.clear() if ids is not None: for tid in ids: target_persons.add(int(tid)) def get_target_persons_snapshot(): with target_persons_lock: return sorted(target_persons) def parse_osc_id_list(value): if isinstance(value, str): raw_items = [item.strip() for item in value.split(",") if item.strip()] elif isinstance(value, (list, tuple, set)): raw_items = list(value) elif hasattr(value, "__iter__") and not isinstance(value, (bytes, bytearray)): raw_items = list(value) else: raw_items = [value] if len(raw_items) == 1 and isinstance(raw_items[0], (list, tuple, set)): raw_items = list(raw_items[0]) parsed_items = [] for item in raw_items: try: parsed_items.append(int(item)) except (TypeError, ValueError): print(f"Ignoring non-numeric OSC id: {item!r}") return parsed_items def get_aruco_id_for_track_id(track_id): with aruco_state_lock: return aruco_person_assignments.get(track_id) def get_track_id_for_aruco_id(aruco_id): with aruco_state_lock: for track_id, assigned_aruco_id in aruco_person_assignments.items(): if assigned_aruco_id == aruco_id: return track_id return None def translate_aruco_ids_to_track_ids(aruco_ids): track_ids = [] for aruco_id in aruco_ids: track_id = get_track_id_for_aruco_id(aruco_id) if track_id is None: print(f"No internal track found for ArUco id: {aruco_id}") continue track_ids.append(track_id) return track_ids def translate_track_ids_to_aruco_ids(track_ids): aruco_ids = [] for track_id in track_ids: aruco_id = get_aruco_id_for_track_id(track_id) if aruco_id is None: continue aruco_ids.append(int(aruco_id)) return sorted(set(aruco_ids)) def get_all_assigned_aruco_ids(): with aruco_state_lock: return sorted({int(aruco_id) for aruco_id in aruco_person_assignments.values() if aruco_id is not None}) def get_box_center(box): x1, y1, x2, y2 = [float(v) for v in box] return (0.5 * (x1 + x2), 0.5 * (y1 + y2)) def distance_xy(point_a, point_b): return ((point_a[0] - point_b[0]) ** 2 + (point_a[1] - point_b[1]) ** 2) ** 0.5 def scale_aruco_corners(marker_corners, scale_x, scale_y): scaled_corners = [] for x, y in marker_corners: scaled_corners.append((float(x) * scale_x, float(y) * scale_y)) return scaled_corners def get_marker_center(marker_corners): center_x = sum(point[0] for point in marker_corners) / len(marker_corners) center_y = sum(point[1] for point in marker_corners) / len(marker_corners) return center_x, center_y def cleanup_missing_person_aruco_state(active_person_keys): with aruco_state_lock: for person_key in list(aruco_person_assignments.keys()): if person_key not in active_person_keys: aruco_person_assignments.pop(person_key, None) aruco_person_missing_counts.pop(person_key, None) aruco_person_sleeping.pop(person_key, None) def update_person_aruco_state(person_key, person_center, current_marker_centers, claimed_marker_ids): with aruco_state_lock: assigned_marker_id = aruco_person_assignments.get(person_key) missing_count = aruco_person_missing_counts.get(person_key, 0) sleeping = aruco_person_sleeping.get(person_key, False) # If mapping is locked (game started via /getAllIds), do not create new assignments # here — only update presence/missing counters for the existing assignment. if aruco_mapping_locked: if assigned_marker_id is not None and assigned_marker_id in current_marker_centers: claimed_marker_ids.add(assigned_marker_id) missing_count = 0 sleeping = False else: missing_count += 1 sleeping = missing_count >= ARUCO_SLEEP_MISSING_FRAMES else: if assigned_marker_id is not None and assigned_marker_id in current_marker_centers: claimed_marker_ids.add(assigned_marker_id) missing_count = 0 sleeping = False elif assigned_marker_id is None and person_center is not None and current_marker_centers: best_marker_id = None best_marker_distance = None for marker_id, marker_center in current_marker_centers.items(): if marker_id in claimed_marker_ids: continue marker_distance = distance_xy(person_center, marker_center) if best_marker_distance is None or marker_distance < best_marker_distance: best_marker_distance = marker_distance best_marker_id = marker_id if best_marker_id is not None and best_marker_distance is not None and best_marker_distance <= ARUCO_ASSIGN_DISTANCE_THRESHOLD: assigned_marker_id = best_marker_id claimed_marker_ids.add(best_marker_id) missing_count = 0 sleeping = False else: missing_count += 1 sleeping = missing_count >= ARUCO_SLEEP_MISSING_FRAMES else: missing_count += 1 sleeping = missing_count >= ARUCO_SLEEP_MISSING_FRAMES aruco_person_assignments[person_key] = assigned_marker_id aruco_person_missing_counts[person_key] = missing_count aruco_person_sleeping[person_key] = sleeping return assigned_marker_id, sleeping, missing_count def set_osc_status_enabled(enabled): global osc_status_enabled osc_status_enabled = bool(enabled) print(f"OSC status sending {'enabled' if osc_status_enabled else 'disabled'}") def print_osc_message(prefix, address, *args): print(f"{prefix} address={address} args={args}") def on_werwolf_poi_toggle(address, *args): print_osc_message("OSC poi/toggle", address, *args) for track_id in translate_aruco_ids_to_track_ids(parse_osc_id_list(args)): toggle_person_of_interest(track_id) def on_getAllIds(address, *args): print_osc_message("OSC poi/getAllIds", address, *args) # Start the game: perform a one-time assignment of currently visible ArUco markers # to detected person track IDs. If called again, reassign based on the latest # frame snapshot. After assignment we lock automatic reassignment until # `/getAllIds` is called again (which will reassign). global aruco_mapping_locked try: with aruco_state_lock: # Build fresh assignments using the latest per-frame snapshots if not latest_person_centers: print("on_getAllIds: no person snapshot available to assign") new_assignments = {} claimed = set() for person_key in sorted(latest_person_centers.keys()): best_marker_id = None best_marker_distance = None person_center = latest_person_centers.get(person_key) for marker_id, marker_center in latest_marker_centers.items(): if marker_id in claimed: continue if person_center is None: continue d = distance_xy(person_center, marker_center) if best_marker_distance is None or d < best_marker_distance: best_marker_distance = d best_marker_id = marker_id if best_marker_id is not None and best_marker_distance is not None and best_marker_distance <= ARUCO_ASSIGN_DISTANCE_THRESHOLD: new_assignments[person_key] = int(best_marker_id) claimed.add(best_marker_id) print("Assigned person to ArUco ") else: new_assignments[person_key] = None print("No suitable ArUco marker found for person (closest distance if any)") # Replace current assignments with the new mapping aruco_person_assignments.clear() aruco_person_assignments.update(new_assignments) # Reset missing counts and sleeping flags for assigned persons for pk in list(aruco_person_assignments.keys()): aruco_person_missing_counts[pk] = 0 aruco_person_sleeping[pk] = False aruco_mapping_locked = True # Reply with the assigned ArUco IDs if 'osc_client_global' in globals() and osc_client_global is not None: osc_client_global.send_message('/getAllIds', get_all_assigned_aruco_ids()) print("Sent getAllIds reply with assigned ArUco IDs:", get_all_assigned_aruco_ids()) else: print("No OSC client available to send getAllIds reply") except Exception as e: print(f"Failed to assign/send getAllIds reply: {e}") def on_everyoneAsleep(address, *args): print_osc_message("OSC everyoneAsleep", address, *args) # Return True if all players are asleep, otherwise return array of ArUco IDs of awake players with aruco_state_lock: awake_aruco_ids = [] for pid in aruco_person_assignments.keys(): if not aruco_person_sleeping.get(pid, False): aruco_id = aruco_person_assignments.get(pid) if aruco_id is not None: awake_aruco_ids.append(int(aruco_id)) # If no awake players, everyone is asleep if not awake_aruco_ids: result = True else: result = sorted(awake_aruco_ids) print(f"everyoneAsleep -> {result}") # Send result back via OSC try: if 'osc_client_global' in globals() and osc_client_global is not None: osc_client_global.send_message('/everyoneAsleep', result) else: print("No OSC client available to send everyoneAsleep reply") except Exception as e: print(f"Failed to send everyoneAsleep reply: {e}") def on_confirmation(address, *args): print_osc_message("OSC confirmation", address, *args) aruco_ids = parse_osc_id_list(args) if not aruco_ids: print("on_confirmation: no aruco ids") return # Translate all ArUco IDs to internal track IDs track_ids = translate_aruco_ids_to_track_ids(aruco_ids) if not track_ids: print(f"on_confirmation: no internal track found for any aruco ids {aruco_ids}") return # Add all players to POIs so gesture detection runs for them clear_persons_of_interest() for tid in track_ids: toggle_person_of_interest(tid) # Set up confirmation wait state with confirmation_lock: confirmation_poi_ids.clear() confirmation_poi_ids.update(track_ids) confirmation_gestures.clear() confirmation_stable_counter = 0 confirmation_last_consensus = None print(f"Waiting for confirmation gestures from POI players: {track_ids} (aruco {aruco_ids})") def on_getPlayerID(address, *args): print_osc_message("OSC getPlayerID", address, *args) if len(args) < 4: print(f"ERROR: getPlayerID expects at least 4 args, got {len(args)}") return try: num_pois = int(args[0]) poi_aruco_ids = args[1 : 1 + num_pois] idx_num_targets = 1 + num_pois num_targets = int(args[idx_num_targets]) target_aruco_ids = args[idx_num_targets + 1 : idx_num_targets + 1 + num_targets] poi_list = translate_aruco_ids_to_track_ids(poi_aruco_ids) target_list = translate_aruco_ids_to_track_ids(target_aruco_ids) clear_persons_of_interest() for poi_id in poi_list: toggle_person_of_interest(poi_id) clear_target_persons() with target_persons_lock: for target_id in target_list: target_persons.add(target_id) with pointing_target_lock: pointing_target_tracker.clear() print(f"Updated POI tracks: {poi_list}, Targets tracks: {target_list}") except Exception as e: print(f"ERROR in on_getPlayerID: {e}") def on_osc_unknown(address, *args): print_osc_message("OSC unknown", address, *args) def start_osc_bridge(): global osc_client_global local_ip = get_local_ip() print(f"Local IP address: {local_ip}") print(f"OSC receiver listening on {OSC_RECEIVER_IP}:{OSC_RECEIVER_PORT}") print(f"OSC sender target: {OSC_TARGET_IP}:{OSC_TARGET_PORT}") dispatcher = Dispatcher() dispatcher.map("/getPlayerID", on_getPlayerID) dispatcher.map("/confirmation", on_confirmation) dispatcher.map("/everyoneAsleep", on_everyoneAsleep) dispatcher.map("/getAllIds", on_getAllIds) dispatcher.set_default_handler(on_osc_unknown) server = ThreadingOSCUDPServer((OSC_RECEIVER_IP, OSC_RECEIVER_PORT), dispatcher) thread = threading.Thread(target=server.serve_forever, daemon=True) thread.start() client = SimpleUDPClient(OSC_TARGET_IP, OSC_TARGET_PORT) osc_client_global = client return server, client, local_ip def find_closest_target_person(ray_x, ray_y, ray_dx, ray_dy, person_positions, target_ids): """ Findet die Zielperson mit der geringsten Winkelabweichung zum Zeigestrahl. """ from math import acos, degrees, sqrt if not target_ids or not person_positions: return None, float('inf') min_angle = float('inf') closest_target_id = None ray_len = sqrt(ray_dx**2 + ray_dy**2) if ray_len < 1e-6: return None, float('inf') for target_id in target_ids: if target_id not in person_positions: continue person_x, person_y = person_positions[target_id] to_person_x = person_x - ray_x to_person_y = person_y - ray_y dist_to_person = sqrt(to_person_x**2 + to_person_y**2) if dist_to_person < 1e-6: continue dot = (ray_dx * to_person_x + ray_dy * to_person_y) cos_theta = max(-1.0, min(1.0, dot / (ray_len * dist_to_person))) angle = degrees(acos(cos_theta)) # Nur Personen "vor" der Hand berücksichtigen if dot > 0 and angle < min_angle: min_angle = angle closest_target_id = target_id return closest_target_id, min_angle def send_osc_status(client, person_count, selected_poi_ids, selected_detection): if client is None or not osc_status_enabled: return client.send_message("/werwolf/status/person_count", int(person_count)) client.send_message("/werwolf/status/poi_ids", ",".join(str(person_id) for person_id in translate_track_ids_to_aruco_ids(selected_poi_ids))) target_ids = get_target_persons_snapshot() client.send_message("/werwolf/status/target_ids", ",".join(str(person_id) for person_id in translate_track_ids_to_aruco_ids(target_ids))) if selected_detection is None: return track_id = selected_detection.get("track_id") aruco_id = selected_detection.get("aruco_id") if aruco_id is None and track_id is not None: aruco_id = get_aruco_id_for_track_id(track_id) label = selected_detection.get("label", "unknown") confidence = float(selected_detection.get("confidence", 0.0)) handedness = selected_detection.get("handedness", "") client.send_message( "/werwolf/status/gesture", [int(aruco_id) if aruco_id is not None else -1, label, confidence, handedness], ) def compute_pointing_ray_from_hand(hand_landmarks, crop_x, crop_y, crop_w, crop_h): """Extract start point and direction vector from hand landmarks (index finger). Returns (start_x, start_y, direction_x, direction_y) in full-frame coordinates.""" from gesture_runtime import INDEX_TIP, INDEX_MCP, landmark_to_pixel index_tip_x, index_tip_y = landmark_to_pixel(hand_landmarks[INDEX_TIP], crop_w, crop_h) index_mcp_x, index_mcp_y = landmark_to_pixel(hand_landmarks[INDEX_MCP], crop_w, crop_h) start_x = index_tip_x + crop_x start_y = index_tip_y + crop_y direction_x = index_tip_x - index_mcp_x direction_y = index_tip_y - index_mcp_y return start_x, start_y, direction_x, direction_y def distance_point_to_ray(point_x, point_y, ray_start_x, ray_start_y, ray_dir_x, ray_dir_y): """ Compute distance from point to an infinite line defined by ray_start and ray_dir. Returns (distance, t) where t is the projection parameter along the ray direction. """ dx = point_x - ray_start_x dy = point_y - ray_start_y ray_len_sq = ray_dir_x * ray_dir_x + ray_dir_y * ray_dir_y if ray_len_sq < 1e-6: return float('inf'), 0.0 t = (dx * ray_dir_x + dy * ray_dir_y) / ray_len_sq closest_x = ray_start_x + t * ray_dir_x closest_y = ray_start_y + t * ray_dir_y dist = ((point_x - closest_x) ** 2 + (point_y - closest_y) ** 2) ** 0.5 return dist, t def expand_box(box, frame_width, frame_height, padding_ratio): x1, y1, x2, y2 = [float(v) for v in box] cx = (x1 + x2) / 2 cy = (y1 + y2) / 2 w = x2 - x1 h = y2 - y1 # Erstelle ein Quadrat (Square ROI) basierend auf der längeren Seite side = max(w, h) * (1.0 + padding_ratio) x1 = max(0, int(cx - side / 2)) y1 = max(0, int(cy - side / 2)) x2 = min(frame_width, int(cx + side / 2)) y2 = min(frame_height, int(cy + side / 2)) return x1, y1, x2, y2 def load_model(): last_error = None for model_path in MODEL_CANDIDATES: try: model = YOLO(model_path) return model, model_path except Exception as exc: last_error = exc raise RuntimeError(f"Could not load any pose model: {last_error}") def draw_person(frame, box, track_id, det_conf): x1, y1, x2, y2 = [int(v) for v in box] # Bounding box cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 180, 255), 2) # Skeleton lines #for a, b in POSE_CONNECTIONS: # if kp_conf is not None: # if kp_conf[a] < KEYPOINT_CONF_THRESHOLD or kp_conf[b] < KEYPOINT_CONF_THRESHOLD: # continue # xa, ya = int(xy[a][0]), int(xy[a][1]) # xb, yb = int(xy[b][0]), int(xy[b][1]) # cv2.line(frame, (xa, ya), (xb, yb), (0, 255, 0), 2) # Keypoints #for i, (x, y) in enumerate(xy): # if kp_conf is not None and kp_conf[i] < KEYPOINT_CONF_THRESHOLD: # continue # cv2.circle(frame, (int(x), int(y)), 4, (0, 0, 255), -1) # Center point from bounding box cx = int((x1 + x2) / 2) cy = int((y1 + y2) / 2) cv2.circle(frame, (cx, cy), 4, (255, 255, 0), -1) # Display ID in large text at the top if track_id is not None: id_label = f"ID: {track_id}" cv2.putText( frame, id_label, (x1, max(20, y1 - 30)), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 3, cv2.LINE_AA, ) # Display confidence below conf_label = f"conf: {det_conf:.2f}" cv2.putText( frame, conf_label, (x1, max(20, y1 - 10)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2, cv2.LINE_AA, ) def get_point_xy(keypoints_xy, keypoints_conf, index): if keypoints_xy is None or index >= len(keypoints_xy): return None if keypoints_conf is not None and index < len(keypoints_conf) and keypoints_conf[index] < KEYPOINT_CONF_THRESHOLD: return None point = keypoints_xy[index] return float(point[0]), float(point[1]) def get_body_center(keypoints_xy, keypoints_conf): points = [] for index in (LEFT_SHOULDER, RIGHT_SHOULDER, LEFT_HIP, RIGHT_HIP): point = get_point_xy(keypoints_xy, keypoints_conf, index) if point is not None: points.append(point) if points: xs = [point[0] for point in points] ys = [point[1] for point in points] return (sum(xs) / len(xs), sum(ys) / len(ys)) center_point = get_point_xy(keypoints_xy, keypoints_conf, 0) return center_point def select_farthest_hand(detections, crop_x, crop_y, body_center): if not detections: return None if body_center is None: return detections[0] best_detection = None best_distance = -1.0 for detection in detections: hand_landmarks = detection["hand_landmarks"] wrist = hand_landmarks[0] wrist_x = crop_x + float(wrist.x * detection["crop_width"]) wrist_y = crop_y + float(wrist.y * detection["crop_height"]) distance = ((wrist_x - body_center[0]) ** 2 + (wrist_y - body_center[1]) ** 2) ** 0.5 if distance > best_distance: best_distance = distance best_detection = detection return best_detection def main(): model, model_name = load_model() cap = cv2.VideoCapture(VIDEO_INPUT) if not cap.isOpened(): raise RuntimeError("Could not open webcam") print(f"Using model: {model_name}") print("Press q to quit.") gesture = None osc_server = None osc_client = None try: osc_server, osc_client, _local_ip = start_osc_bridge() gesture = GestureRecognizer( data_pickle=GESTURE_DATA_PICKLE, analysis_pickle=GESTURE_ANALYSIS_PICKLE, feature_family=GESTURE_FEATURE_FAMILY, classifier_type=GESTURE_CLASSIFIER_TYPE, ranking_method=GESTURE_RANKING_METHOD, num_features=GESTURE_NUM_FEATURES, unknown_threshold=GESTURE_UNKNOWN_THRESHOLD, hand_model_path=GESTURE_MODEL_PATH, ) frame_count = 0 # ArUco marker detection setup (OpenCV 4.7+) arucoDict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) arucoParams = cv2.aruco.DetectorParameters() detector = cv2.aruco.ArucoDetector(arucoDict, arucoParams) while True: ok, frame_orig = cap.read() if not ok: print("Failed to read frame") break orig_height, orig_width = frame_orig.shape[:2] frame = cv2.resize(frame_orig, (980, 540)) #frame = frame_orig #frame_count += 1 # Entfernt: Das Drosseln auf jeden 6. Frame macht die Erkennung zu instabil # if frame_count % 6 != 0: # continue frame_height, frame_width = frame.shape[:2] scale_x = frame_width / float(orig_width) scale_y = frame_height / float(orig_height) corners, ids, rejected = detector.detectMarkers(frame_orig) # Track mode helps keep person IDs stable over time. results = model.track( frame, conf=CONF_THRESHOLD, iou=IOU_THRESHOLD, classes=[0], # person persist=True, verbose=False, ) display = frame.copy() person_count = 0 selected_detection = None person_positions = {} selected_pointing_target_id = None current_frame_pointing_targets = set() person_centers = {} # Visualize detected ArUco markers current_marker_centers = {} if ids is not None and len(ids) > 0: for i, marker_id in enumerate(ids): marker_corners = corners[i][0] marker_id_value = int(marker_id[0]) marker_corners_scaled = scale_aruco_corners(marker_corners, scale_x, scale_y) center_x, center_y = get_marker_center(marker_corners_scaled) current_marker_centers[marker_id_value] = (center_x, center_y) # Draw marker rectangle pts = np.array(marker_corners_scaled, dtype=int) cv2.polylines(display, [pts], True, (255, 0, 255), 2) # Draw center point cv2.circle(display, (int(center_x), int(center_y)), 6, (0, 255, 255), -1) # Draw marker ID marker_text = f"M:{marker_id_value}" cv2.putText( display, marker_text, (int(center_x) - 30, int(center_y) - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 200, 0), 2, cv2.LINE_AA, ) claimed_marker_ids = set() if results: result = results[0].cpu().numpy() if result.boxes is not None and result.keypoints is not None: # Reset per-frame gesture observations for confirmation logic with confirmation_lock: confirmation_gestures.clear() boxes_xyxy = result.boxes.xyxy track_ids = result.boxes.id if result.boxes.id is not None else None keypoints_xy = result.keypoints.xy keypoints_conf = result.keypoints.conf person_count = len(boxes_xyxy) # Entfernt: Überschreibt sonst die via OSC gesetzten Targets # if len(get_target_persons_snapshot()) == 0: # add_all_target_persons(track_ids) persons_of_interest_current = set(get_persons_of_interest_snapshot()) for i in range(person_count): track_id = None if track_ids is not None: track_id = int(track_ids[i]) kp_conf = None if keypoints_conf is not None: kp_conf = keypoints_conf[i] # Draw person with ID det_conf = float(result.boxes.conf[i]) if result.boxes.conf is not None else 0.0 draw_person( display, boxes_xyxy[i], track_id, det_conf, ) x1, y1, x2, y2 = expand_box( boxes_xyxy[i], frame_width, frame_height, PERSON_PADDING_RATIO, ) crop = frame[y1:y2, x1:x2] if crop.size == 0: continue body_center = get_body_center(keypoints_xy[i], kp_conf) if body_center is not None and track_id is not None: person_positions[track_id] = body_center person_centers[track_id] = body_center if track_id is not None: assigned_marker_id, sleeping, missing_count = update_person_aruco_state( track_id, body_center, current_marker_centers, claimed_marker_ids, ) else: assigned_marker_id = None sleeping = False missing_count = 0 if sleeping: cx, cy = get_box_center(boxes_xyxy[i]) radius = 10 cv2.circle(display, (int(cx), int(cy)), radius, (255, 0, 0), -1) cv2.putText( display, f"Sleep | A:{assigned_marker_id if assigned_marker_id is not None else '-'}", (int(cx) - radius, int(cy) - radius - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2, cv2.LINE_AA, ) # Einrückung korrigiert: Diese Anzeige muss pro Person in der Schleife erfolgen if assigned_marker_id is not None and body_center is not None: cv2.putText( display, f"Aruco: {assigned_marker_id}", (x1, max(20, y2 + 20)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2, cv2.LINE_AA, ) if track_id is not None and (int(track_id) in persons_of_interest_current or int(track_id) in confirmation_poi_ids): detections = gesture.detect_gestures(crop) for detection in detections: detection["crop_width"] = crop.shape[1] detection["crop_height"] = crop.shape[0] detection["track_id"] = track_id detection["aruco_id"] = assigned_marker_id selected_detection = select_farthest_hand( detections, x1, y1, body_center, ) if selected_detection is not None: label = selected_detection.get("label", "unknown") if label.upper() == "A" and body_center is not None: ray_x, ray_y, ray_dx, ray_dy = compute_pointing_ray_from_hand( selected_detection["hand_landmarks"], x1, y1, selected_detection["crop_width"], selected_detection["crop_height"], ) target_ids = get_target_persons_snapshot() target_id, target_dist = find_closest_target_person( ray_x, ray_y, ray_dx, ray_dy, person_positions, target_ids, ) if target_id is not None and target_dist < 25: current_frame_pointing_targets.add(target_id) selected_pointing_target_id = target_id with pointing_target_lock: if target_id not in pointing_target_tracker: pointing_target_tracker[target_id] = 0 pointing_target_tracker[target_id] += 1 gesture.draw_gesture( display, selected_detection, x1, y1, crop.shape[1], crop.shape[0], ) # Collect gestures for confirmation logic (Lock wird bereits außen gehalten) if confirmation_poi_ids and track_id in confirmation_poi_ids: lab = selected_detection.get("label", "").upper() if lab in ("B", "C"): confirmation_gestures[track_id] = lab # Confirmation logic: evaluate gathered gestures after processing all persons in the frame with confirmation_lock: # Geändert: Prüft, ob mindestens alle POIs ihre Geste zeigen (weniger strikt als exakte Gleichheit) if confirmation_poi_ids and confirmation_poi_ids.issubset(confirmation_gestures.keys()): # Nur Gesten der relevanten Personen prüfen relevant_gestures = [confirmation_gestures[tid] for tid in confirmation_poi_ids] if len(set(relevant_gestures)) == 1: current_consensus = relevant_gestures[0] if current_consensus == confirmation_last_consensus: confirmation_stable_counter += 1 else: confirmation_last_consensus = current_consensus confirmation_stable_counter = 1 # Da wir jetzt mehr Frames verarbeiten, erhöhen wir auf ca. 10 Frames (~0.3 Sek) für Stabilität if confirmation_stable_counter >= 10: result_conf = True if confirmation_last_consensus == "C" else False if osc_client is not None: osc_client.send_message("/confirmation", result_conf) print(f"Sent confirmation {result_conf} after 3 stable frames (Gestures: {confirmation_last_consensus})") # Reset confirmation state confirmation_poi_ids.clear() confirmation_stable_counter = 0 confirmation_last_consensus = None clear_persons_of_interest() else: # Gestures are visible but don't match each other (e.g. one B, one C) confirmation_stable_counter = 0 confirmation_last_consensus = None else: # Not all players of interest are showing a valid gesture in this frame confirmation_stable_counter = 0 confirmation_last_consensus = None cleanup_missing_person_aruco_state(set(person_positions.keys())) # Publish latest per-frame snapshots for `/getAllIds` to use when assigning markers. try: with aruco_state_lock: latest_marker_centers.clear() latest_marker_centers.update(current_marker_centers) latest_person_centers.clear() latest_person_centers.update(person_positions) except Exception: pass with pointing_target_lock: for target_id, frame_count in list(pointing_target_tracker.items()): # Prüfen, ob dieses Ziel in diesem Frame aktiv anvisiert wurde if target_id in current_frame_pointing_targets: if frame_count >= 4: if osc_client is not None: osc_client.send_message("/getPlayerID", int(get_aruco_id_for_track_id(target_id))) print(f"Sent stable pointing target: {get_aruco_id_for_track_id(target_id)} after {frame_count} frames") clear_persons_of_interest() pointing_target_tracker.clear() break else: # Langsam abbauen, wenn in diesem Frame nicht darauf gezeigt wurde pointing_target_tracker[target_id] = max(0, frame_count - 1) #send_osc_status(osc_client, person_count, get_persons_of_interest_snapshot(), selected_detection) #if osc_client is not None and osc_status_enabled and selected_pointing_target_id is not None: #osc_client.send_message("/werwolf/status/pointing_target", int(get_aruco_id_for_track_id(selected_pointing_target_id)) if get_aruco_id_for_track_id(selected_pointing_target_id) is not None else -1) cv2.putText( display, f"People detected: {person_count}", (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2, cv2.LINE_AA, ) cv2.putText( display, "q = quit", (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2, cv2.LINE_AA, ) cv2.putText( display, f"Gesture: {GESTURE_FEATURE_FAMILY} | {GESTURE_CLASSIFIER_TYPE} | top {GESTURE_NUM_FEATURES} | POI IDs: {get_persons_of_interest_snapshot()} | OSC: {'on' if osc_status_enabled else 'off'}", (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 200, 0), 2, cv2.LINE_AA, ) target_str = f"Target IDs: {get_target_persons_snapshot()}" if selected_pointing_target_id is not None: target_str += f" | Pointing at: {selected_pointing_target_id}" cv2.putText( display, target_str, (20, 160), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2, cv2.LINE_AA, ) cv2.imshow("YOLO Multi-Person Pose", display) key = cv2.waitKey(1) & 0xFF if key == ord("q"): break elif key == ord("a"): osc_client.send_message("/demo/number", 42) print("Sent: /demo/number 42") elif key == ord("s"): osc_client.send_message("/speak", "Impertinent") print("Sent: /demo/xy [0.25, 0.75]") elif ord("0") <= key <= ord("9"): toggle_person_of_interest(key - ord("0")) finally: cap.release() cv2.destroyAllWindows() if gesture is not None: gesture.close() if osc_server is not None: osc_server.shutdown() osc_server.server_close() if __name__ == "__main__": main()