""" ESP32-CAM 자율주행 자동차 메인 프로그램 영상 수신 → 차선 인식 → 제어 계산 → UDP 전송 """ import cv2 import json import os import time import sys import numpy as np from video_stream import VideoStream from lane_detector import LaneDetector from controller import ProportionalController from udp_sender import UDPSender from object_detector import ObjectDetector, ULTRALYTICS_AVAILABLE def load_config(config_path='../config/config.json'): """설정 파일 로드""" if not os.path.exists(config_path): print(f"설정 파일이 없습니다: {config_path}") print("기본 설정을 사용합니다.") return None with open(config_path, 'r', encoding='utf-8') as f: config = json.load(f) return config def print_help(): """도움말 출력""" print("\n=== 조작 키 ===") print("q: 종료") print("s: 시작/정지 토글") print("t: HSV 튜닝 모드 토글 (단순화된 5개 파라미터)") print("c: HSV 설정 저장 (config.json)") print("+/-: Kp 증가/감소") print("w/x: 속도 증가/감소") print("a/d: k_angle 증가/감소 (방향 전환 반응 속도)") print("z/v: k_position 증가/감소 (위치 오차 반응)") print("f/g: 프레임 스킵 증가/감소 (지연 감소)") print("[ / ]: 감지(초록) 영역 위로 확장/축소 (ROI 길이)") print(", / .: 흰색 밝기 최소값 낮춤/높임 (어두운 흰색 감지 → 초록 길게)") print("; / ': 흰색 채도 최대값 높임/낮춤 (회색빛 흰색 포함)") print("1/2/3: 차선 색 선택 (1=흰색, 2=검정, 3=주황)") print("y/u: YOLO 실행 주기 증가/감소 (성능 vs 정확도)") print("m: 수동/자동 모드 전환") print("i/k (또는 방향키 ↑/↓): 수동 속도 증가/감소") print("j/l (또는 방향키 ←/→): 수동 조향 좌/우") print("n: 조향 중립으로 리셋 (수동 모드)") print("e/r: 조향 중립 트림 ←/→ (캘리브레이션, config.json 자동 저장)") print("p: 키 코드 디버그 모드 (방향키 코드 확인)") print("SPACE: 긴급 정지") print("h: 도움말") print("\n=== HSV 튜닝 가이드 ===") print("White Brightness Min: 낮을수록 어두운 흰색 인식 (기본: 220, 권장: 200-240)") print("White Saturation Max: 높을수록 회색빛 인식 (권장: 30-80)") print("Yellow Hue Min/Max: 노란색 범위 (권장: 15-35)") print("Yellow Brightness Min: 낮을수록 어두운 노란색 인식 (권장: 80-150)") print() def nothing(x): """트랙바 콜백""" pass def save_hsv_config(config_path, white_lower, white_upper, yellow_lower, yellow_upper): """HSV 설정을 config.json에 저장""" try: with open(config_path, 'r', encoding='utf-8') as f: config = json.load(f) config['lane_detection']['white_hsv_lower'] = white_lower config['lane_detection']['white_hsv_upper'] = white_upper config['lane_detection']['yellow_hsv_lower'] = yellow_lower config['lane_detection']['yellow_hsv_upper'] = yellow_upper with open(config_path, 'w', encoding='utf-8') as f: json.dump(config, f, indent=2) print(f"\nHSV 설정 저장 완료!") print(f"흰색: {white_lower} ~ {white_upper}") print(f"노란색: {yellow_lower} ~ {yellow_upper}") return True except Exception as e: print(f"\n저장 실패: {e}") return False def save_steering_neutral(config_path, neutral): """조향 중립값을 config.json 의 control.steering_neutral 에 저장 (캘리브레이션 유지)""" try: with open(config_path, 'r', encoding='utf-8') as f: config = json.load(f) config.setdefault('control', {})['steering_neutral'] = int(neutral) with open(config_path, 'w', encoding='utf-8') as f: json.dump(config, f, indent=2) return True except Exception as e: print(f"\n중립값 저장 실패: {e}") return False def save_lane_color(config_path, color): """선택한 차선 색을 config.json 의 lane_detection.lane_color 에 저장""" try: with open(config_path, 'r', encoding='utf-8') as f: config = json.load(f) config.setdefault('lane_detection', {})['lane_color'] = color with open(config_path, 'w', encoding='utf-8') as f: json.dump(config, f, indent=2) return True except Exception as e: print(f"\n차선색 저장 실패: {e}") return False def main(): # 설정 로드 config_path = os.path.join(os.path.dirname(__file__), '..', '..', 'config', 'config.json') config = load_config(config_path) if config is None: # 기본 설정 esp32_ip_base = "192.168.1." esp32_ip_default_last = "100" stream_port = 80 udp_port = 4210 lane_config = {} control_config = { 'kp': 0.3, 'k_position': 2.0, 'k_angle': 2.5, 'base_speed': 200, 'steering_min': 45, 'steering_max': 135, 'steering_neutral': 90 } else: # 설정 파일에서 읽기 esp32_ip_from_config = config['esp32']['ip'] # IP 주소를 앞부분과 끝자리로 분리 ip_parts = esp32_ip_from_config.rsplit('.', 1) esp32_ip_base = ip_parts[0] + '.' # 예: "192.168.137." esp32_ip_default_last = ip_parts[1] # 예: "74" stream_port = config['esp32']['stream_port'] udp_port = config['esp32']['udp_port'] lane_config = config.get('lane_detection', {}) control_config = config.get('control', {}) # IP 끝자리 입력받기 print("\n=== ESP32-CAM IP 설정 ===") print(f"기본 IP 주소: {esp32_ip_base}{esp32_ip_default_last}") ip_last_input = input(f"ESP32-CAM IP 끝자리를 입력하세요 (기본값: {esp32_ip_default_last}, 엔터=기본값 사용): ").strip() if ip_last_input == "": # 엔터만 누르면 기본값 사용 esp32_ip_last = esp32_ip_default_last else: # 입력값 검증 try: ip_num = int(ip_last_input) if 1 <= ip_num <= 254: esp32_ip_last = ip_last_input else: print(f"잘못된 입력입니다. 기본값 {esp32_ip_default_last}를 사용합니다.") esp32_ip_last = esp32_ip_default_last except ValueError: print(f"잘못된 입력입니다. 기본값 {esp32_ip_default_last}를 사용합니다.") esp32_ip_last = esp32_ip_default_last # 최종 IP 주소 구성 esp32_ip = esp32_ip_base + esp32_ip_last # 스트리밍 URL stream_url = f"http://{esp32_ip}:{stream_port}/stream" print("=== ESP32-CAM 자율주행 자동차 ===") print(f"ESP32-CAM IP: {esp32_ip}") print(f"스트리밍 URL: {stream_url}") print(f"UDP 포트: {udp_port}") print_help() # 모듈 초기화 print("모듈 초기화 중...") video_stream = VideoStream(stream_url) lane_detector = LaneDetector(lane_config) controller = ProportionalController( kp=control_config.get('kp', 0.3), k_position=control_config.get('k_position', 1.0), k_angle=control_config.get('k_angle', 2.5), base_speed=control_config.get('base_speed', 200), steering_min=control_config.get('steering_min', 45), steering_max=control_config.get('steering_max', 135), steering_neutral=control_config.get('steering_neutral', 90) ) udp_sender = UDPSender(esp32_ip, udp_port) # YOLO 객체 감지기 초기화 (선택적 실행으로 성능 최적화) model_path = os.path.join(os.path.dirname(__file__), "yolov8n.pt") traffic_light_model_path = os.path.join(os.path.dirname(__file__), "traffic_light.pt") if os.path.exists(model_path) and ULTRALYTICS_AVAILABLE: print("YOLO 객체 감지기 초기화 중...") # 신호등 특화 모델 확인 if os.path.exists(traffic_light_model_path): print(f"신호등 특화 모델 발견: {traffic_light_model_path}") object_detector = ObjectDetector(model_path, confidence=0.5, traffic_light_model_path=traffic_light_model_path) else: print("신호등 특화 모델 없음 - 기본 HSV 색상 감지 사용") print("(신호등 특화 모델을 사용하려면 traffic_light.pt 파일을 python 폴더에 넣으세요)") object_detector = ObjectDetector(model_path, confidence=0.5) object_detection_enabled = True print("객체 감지 활성화 (3-5프레임마다 실행):") print(" - 신호등 (빨간불)") print(" - 정지 표지판") print(" - 보행자 (즉시 정지)") print(" - 자전거/오토바이 (즉시 정지)") print(" - 차량 (거리 기반 속도 조절)") print(" - 키보드: y/u 키로 YOLO 실행 주기 조절") else: if not ULTRALYTICS_AVAILABLE: print("ultralytics 미설치 → 객체 감지(YOLO) 비활성화") print("단계9 객체 감지를 사용하려면: pip install ultralytics") else: print(f"YOLO 모델을 찾을 수 없습니다: {model_path}") print("객체 감지 기능 비활성화 (차선주행 단계6~8은 정상 동작)") object_detector = None object_detection_enabled = False # 연결 print("\n연결 중...") if not video_stream.connect(): print("영상 스트림 연결 실패!") return if not udp_sender.connect(): print("UDP 소켓 생성 실패!") return print("연결 완료!") print("\n=== 단순화된 HSV 튜닝 모드 ===") print("- 't' 키를 눌러 튜닝 모드 활성화") print("- 5개의 핵심 파라미터만 조정 (이전 12개에서 단순화)") print("- 자세한 도움말은 'h' 키를 누르세요\n") # 자율주행 상태 is_running = False manual_mode = False # False: 자동, True: 수동 key_debug_mode = False # 키 코드 디버그 모드 frame_count = 0 start_time = time.time() tuning_mode = False trackbars_created = False # 수동 조종 상태 manual_speed = 0 manual_steering = 90 # 중립 # 성능 모니터링 processing_times = [] frame_skip = 0 # 0: 모든 프레임, 1: 1프레임씩 건너뛰기, 2: 2프레임씩 건너뛰기 # YOLO 실행 주기 제어 (성능 최적화) yolo_interval = 3 # 3프레임마다 한 번 YOLO 실행 (y/u 키로 조절 가능) yolo_frame_counter = 0 last_detection_result = None # 마지막 객체 감지 결과 캐싱 # HSV 초기값 white_h_min = lane_config.get('white_hsv_lower', [0, 0, 220])[0] white_s_min = lane_config.get('white_hsv_lower', [0, 0, 220])[1] white_v_min = lane_config.get('white_hsv_lower', [0, 0, 220])[2] white_h_max = lane_config.get('white_hsv_upper', [180, 50, 255])[0] white_s_max = lane_config.get('white_hsv_upper', [180, 50, 255])[1] white_v_max = lane_config.get('white_hsv_upper', [180, 50, 255])[2] yellow_h_min = lane_config.get('yellow_hsv_lower', [20, 100, 100])[0] yellow_s_min = lane_config.get('yellow_hsv_lower', [20, 100, 100])[1] yellow_v_min = lane_config.get('yellow_hsv_lower', [20, 100, 100])[2] yellow_h_max = lane_config.get('yellow_hsv_upper', [30, 255, 255])[0] yellow_s_max = lane_config.get('yellow_hsv_upper', [30, 255, 255])[1] yellow_v_max = lane_config.get('yellow_hsv_upper', [30, 255, 255])[2] try: while True: # 처리 시작 시간 loop_start_time = time.time() # 프레임 가져오기 frame = video_stream.get_frame() if frame is None: print("프레임을 가져올 수 없음") time.sleep(0.1) continue # 프레임 스킵 (성능 향상) frame_count += 1 if frame_skip > 0 and frame_count % (frame_skip + 1) != 0: continue # 프레임 회전 없음 (0도) # frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE) # 좌우 반전 (미러링) frame = cv2.flip(frame, 1) height, width = frame.shape[:2] frame_center_x = width // 2 # 튜닝 모드: HSV 값 업데이트 if tuning_mode and trackbars_created: # 창이 닫혔는지 확인 try: window_visible = cv2.getWindowProperty('HSV Tuning', cv2.WND_PROP_VISIBLE) if window_visible < 1: # 창이 닫혔으면 튜닝 모드 종료 print("\n튜닝 창이 닫혔습니다. 튜닝 모드를 종료합니다.") tuning_mode = False trackbars_created = False else: # 단순화된 트랙바에서 값 읽기 white_v_min = cv2.getTrackbarPos('White Brightness Min', 'HSV Tuning') white_s_max = cv2.getTrackbarPos('White Saturation Max', 'HSV Tuning') yellow_h_min = cv2.getTrackbarPos('Yellow Hue Min', 'HSV Tuning') yellow_h_max = cv2.getTrackbarPos('Yellow Hue Max', 'HSV Tuning') yellow_v_min = cv2.getTrackbarPos('Yellow Brightness Min', 'HSV Tuning') # 나머지 값들은 자동 설정 white_h_min = 0 # 흰색은 모든 색상 포함 white_h_max = 180 white_s_min = 0 # 채도는 0부터 white_v_max = 255 # 밝기는 최대까지 yellow_s_min = 100 # 노란색은 채도 높게 yellow_s_max = 255 yellow_v_max = 255 # lane_detector 설정 업데이트 (numpy 배열로 변환) lane_detector.white_lower = np.array([white_h_min, white_s_min, white_v_min], dtype=np.uint8) lane_detector.white_upper = np.array([white_h_max, white_s_max, white_v_max], dtype=np.uint8) lane_detector.yellow_lower = np.array([yellow_h_min, yellow_s_min, yellow_v_min], dtype=np.uint8) lane_detector.yellow_upper = np.array([yellow_h_max, yellow_s_max, yellow_v_max], dtype=np.uint8) except: # 예외 발생 시 튜닝 모드 종료 tuning_mode = False trackbars_created = False # 차선 검출 (하이브리드: 위치 + 각도) lane_center_x, lane_angle, debug_frame = lane_detector.detect(frame) # 객체 감지 (신호등, 정지 표지판, 보행자, 차량, 자전거) # 성능 최적화: yolo_interval 프레임마다 한 번씩만 실행 detection_result = None should_stop_for_object = False speed_factor = 1.0 if object_detection_enabled and object_detector is not None: yolo_frame_counter += 1 if yolo_frame_counter >= yolo_interval: # YOLO 실행 detection_result = object_detector.detect_all_objects(frame) last_detection_result = detection_result yolo_frame_counter = 0 else: # 이전 결과 재사용 detection_result = last_detection_result if detection_result is not None: should_stop_for_object = detection_result['should_stop'] speed_factor = detection_result['speed_factor'] # 제어 계산 if manual_mode: # 수동 모드: 키보드 입력 사용 steering = manual_steering speed = manual_speed else: # 자동 모드: 차선 인식 기반 제어 steering = controller.calculate_steering(lane_center_x, frame_center_x, lane_angle) # 속도 결정 (객체 감지 결과 반영) if should_stop_for_object: speed = 0 elif is_running: base_speed = controller.calculate_speed(steering) speed = int(base_speed * speed_factor) # 속도 계수 적용 else: speed = 0 # UDP 전송 udp_sender.send(speed, steering) # 처리 시간 측정 loop_time = (time.time() - loop_start_time) * 1000 # ms processing_times.append(loop_time) if len(processing_times) > 30: processing_times.pop(0) # 객체 감지 결과를 디버그 프레임에 표시 if debug_frame is not None and detection_result is not None: debug_frame = object_detector.draw_detections(debug_frame, detection_result) # 대시보드 생성 if debug_frame is not None: # FPS 계산 fps = video_stream.get_fps() # 영상 크기 video_h, video_w = debug_frame.shape[:2] # 캔버스 크기 설정 (영상 너비에 맞춤, 아래 정보 패널 공간 추가) canvas_width = max(video_w + 40, 1000) # 최소 1000px info_panel_height = 350 # 정보 패널 높이 (280 → 350으로 증가) canvas_height = video_h + info_panel_height + 60 canvas = np.zeros((canvas_height, canvas_width, 3), dtype=np.uint8) # 영상을 상단 중앙에 배치 video_x = (canvas_width - video_w) // 2 video_y = 20 canvas[video_y:video_y+video_h, video_x:video_x+video_w] = debug_frame # 정보 패널 시작 위치 (영상 아래) info_y = video_y + video_h + 30 # 3개 컬럼으로 분할 col_width = canvas_width // 3 col1_x = 30 col2_x = col_width + 20 col3_x = col_width * 2 + 10 # === 컬럼 1: STATUS & CONTROL === y_pos = info_y cv2.putText(canvas, "STATUS", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (100, 200, 255), 2) y_pos += 30 # 모드 표시 if manual_mode: mode_text = "MANUAL" mode_color = (255, 165, 0) # 주황색 else: mode_text = "AUTO (RUNNING)" if is_running else "AUTO (STOPPED)" mode_color = (0, 255, 0) if is_running else (0, 0, 255) cv2.putText(canvas, f"Mode: {mode_text}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, mode_color, 1) y_pos += 25 cv2.putText(canvas, f"FPS: {fps:.1f}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) y_pos += 25 # 처리 시간 표시 if processing_times: avg_time = sum(processing_times) / len(processing_times) latency_color = (0, 255, 0) if avg_time < 50 else (0, 165, 255) if avg_time < 100 else (0, 0, 255) cv2.putText(canvas, f"Latency: {avg_time:.0f}ms", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, latency_color, 1) y_pos += 25 cv2.putText(canvas, f"Skip: {frame_skip}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) y_pos += 25 # 감지(초록) 영역 길이: roi_far_y가 작을수록 위로 길어짐 cv2.putText(canvas, f"ROI far: {lane_detector.roi_far_y:.2f}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (100, 255, 200), 1) y_pos += 25 # YOLO 실행 주기 표시 if object_detection_enabled: cv2.putText(canvas, f"YOLO: 1/{yolo_interval} frames", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 200, 100), 1) y_pos += 25 # 객체 감지 상태 표시 if object_detection_enabled and detection_result is not None: # 주 상태 if detection_result['should_stop']: obj_text = "OBJ: STOP!" obj_color = (0, 0, 255) # 빨간색 elif detection_result['should_slow']: obj_text = f"OBJ: SLOW ({int(detection_result['speed_factor']*100)}%)" obj_color = (0, 165, 255) # 주황색 else: obj_text = "OBJ: Clear" obj_color = (0, 255, 0) # 초록색 cv2.putText(canvas, obj_text, (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, obj_color, 1) y_pos += 25 # 감지 사유 reason = detection_result.get('reason', '') if reason and reason != "Normal": cv2.putText(canvas, f" {reason}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (200, 200, 200), 1) y_pos += 25 # 감지된 객체 카운트 detected_objs = [] if detection_result.get('person'): detected_objs.append("P") if detection_result.get('bicycle'): detected_objs.append("B") if detection_result.get('vehicle'): detected_objs.append("V") if detection_result.get('red_light'): detected_objs.append("R-Light") if detection_result.get('green_light'): detected_objs.append("G-Light") if detection_result.get('stop_sign'): detected_objs.append("Stop") if detected_objs: cv2.putText(canvas, f" Detect: {','.join(detected_objs)}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (150, 150, 150), 1) y_pos += 10 # CONTROL 제목 control_title = "CONTROL (Manual)" if manual_mode else "CONTROL (Hybrid)" control_color = (255, 165, 0) if manual_mode else (100, 200, 255) cv2.putText(canvas, control_title, (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.6, control_color, 2) y_pos += 30 cv2.putText(canvas, f"Speed: {speed}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) y_pos += 25 cv2.putText(canvas, f"Steering: {steering}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) y_pos += 25 # 조향 중립값 (e/r 트림) - 자동·수동 모두 항상 표시 cv2.putText(canvas, f"Neutral: {controller.steering_neutral} e/r", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (180, 220, 255), 1) y_pos += 25 if manual_mode: # 수동 모드: 조향 범위 표시 cv2.putText(canvas, f"Range: {controller.steering_min}-{controller.steering_max}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1) y_pos += 25 else: # 자동 모드: 차선 정보 표시 angle_text = f"Angle: {lane_angle:.1f}" if lane_angle is not None else "Angle: N/A" cv2.putText(canvas, angle_text, (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1) y_pos += 25 cv2.putText(canvas, f"k_pos: {controller.k_position:.2f}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (150, 150, 255), 1) y_pos += 25 cv2.putText(canvas, f"k_ang: {controller.k_angle:.2f}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1) y_pos += 25 cv2.putText(canvas, f"Base Speed: {controller.base_speed}", (col1_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) # === 컬럼 2: HSV VALUES === y_pos = info_y if tuning_mode: cv2.putText(canvas, "HSV TUNING", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 165, 255), 2) else: cv2.putText(canvas, "HSV VALUES", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (100, 200, 255), 2) y_pos += 30 # 현재 선택된 차선 색 (1/2/3 키로 변경) cv2.putText(canvas, f"Lane: {lane_detector.lane_color.upper()} 1/2/3", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 180), 1) y_pos += 26 cv2.putText(canvas, "White:", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1) y_pos += 25 cv2.putText(canvas, f" Brightness: {white_v_min:3d}", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 255, 255) if tuning_mode else (255, 255, 255), 1) y_pos += 22 cv2.putText(canvas, f" Saturation: {white_s_max:3d}", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 255, 255) if tuning_mode else (255, 255, 255), 1) y_pos += 30 cv2.putText(canvas, "Yellow:", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1) y_pos += 25 cv2.putText(canvas, f" Hue: {yellow_h_min:3d}~{yellow_h_max:3d}", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 255, 255) if tuning_mode else (255, 255, 255), 1) y_pos += 22 cv2.putText(canvas, f" Brightness: {yellow_v_min:3d}", (col2_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 255, 255) if tuning_mode else (255, 255, 255), 1) # === 컬럼 3: CONTROLS (인포그래픽: 카테고리별 그룹, 2열) === ctrl_top = info_y cv2.putText(canvas, "CONTROLS", (col3_x, ctrl_top), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (100, 200, 255), 2) sub_gap = (canvas_width - col3_x) // 2 # 컬럼3 영역을 좌우 2등분 left_x = col3_x right_x = col3_x + sub_gap def draw_group(x, y, title, title_color, items): """카테고리 제목 + 밑줄 + 키 목록을 그리고 다음 y를 반환""" cv2.putText(canvas, title, (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.48, title_color, 2) cv2.line(canvas, (x, y + 6), (x + sub_gap - 30, y + 6), title_color, 1) y += 24 for key, desc, color in items: cv2.putText(canvas, f"{key:5s} {desc}", (x + 8, y), cv2.FONT_HERSHEY_SIMPLEX, 0.42, color, 1) y += 21 return y + 10 # 그룹 사이 여백 # --- 그룹 데이터 --- drive_items = [ ("s", "Start/Stop", (0, 255, 0)), ("m", "Manual/Auto", (255, 165, 0)), ("w/x", "Speed", (255, 255, 255)), ("+/-", "Kp", (180, 180, 180)), ("a/d", "k_angle", (0, 255, 255)), ("z/v", "k_pos", (150, 150, 255)), ("e/r", "Neutral", (180, 220, 255)), ] vision_items = [ ("1/2/3", "LaneColor", (0, 255, 180)), ("[/]", "ROI Len", (100, 255, 200)), (",/.", "W.Bright", (0, 255, 255)), (";/'", "W.Satur", (0, 255, 255)), ("t", "Tuning", (0, 165, 255)), ("c", "Save HSV", (0, 200, 200)), ] system_items = [ ("f/g", "Skip", (255, 200, 0)), ("p", "Key Debug", (100, 255, 100)), ("h", "Help", (200, 200, 100)), ("SPACE", "E-Stop", (255, 0, 0)), ("q", "Quit", (200, 200, 200)), ] # 왼쪽 열: DRIVE (+ 수동 모드면 MANUAL) gy = ctrl_top + 34 gy = draw_group(left_x, gy, "[ DRIVE ]", (100, 200, 255), drive_items) if manual_mode: draw_group(left_x, gy, "[ MANUAL ]", (255, 165, 0), [ ("i/k", "M.Speed", (255, 200, 150)), ("j/l", "M.Steer", (255, 200, 150)), ("n", "Neutral", (255, 200, 150)), ]) # 오른쪽 열: VISION (+ YOLO면 DETECT) + SYSTEM gy = ctrl_top + 34 gy = draw_group(right_x, gy, "[ VISION ]", (0, 200, 255), vision_items) if object_detection_enabled: gy = draw_group(right_x, gy, "[ DETECT ]", (255, 200, 100), [("y/u", "YOLO Freq", (255, 200, 100))]) draw_group(right_x, gy, "[ SYSTEM ]", (100, 200, 255), system_items) # === 처음 사용자 안내 박스 (중앙 하단 빈 공간) === guide_left = col2_x - 10 guide_right = col3_x - 25 guide_top = info_y + 185 guide_bottom = guide_top + 150 # 배경(어두운 회색) + 강조 테두리 cv2.rectangle(canvas, (guide_left, guide_top), (guide_right, guide_bottom), (45, 45, 45), -1) cv2.rectangle(canvas, (guide_left, guide_top), (guide_right, guide_bottom), (0, 200, 255), 1) gx = guide_left + 12 gy_guide = guide_top + 26 cv2.putText(canvas, "START HERE (first time?)", (gx, gy_guide), cv2.FONT_HERSHEY_SIMPLEX, 0.48, (0, 200, 255), 2) gy_guide += 26 guide_lines = [ ("1.'m' MANUAL: j/l steer, i/k speed", (255, 255, 255)), ("2.'s' AUTO drive on/off", (0, 255, 0)), ("3.'t' tune lane color (HSV)", (0, 165, 255)), ("4.'h' full help (console)", (200, 200, 100)), ("SPACE stop 'q' quit", (120, 120, 255)), ] for text, color in guide_lines: cv2.putText(canvas, text, (gx, gy_guide), cv2.FONT_HERSHEY_SIMPLEX, 0.4, color, 1) gy_guide += 22 # 화면 표시 cv2.imshow('Autonomous Car', canvas) # 튜닝 모드 트랙바 윈도우 (영상 아래) if tuning_mode and trackbars_created: # 트랙바 5개를 위한 최소 높이 (약 200픽셀) cv2.imshow('HSV Tuning', np.zeros((200, 600, 3), dtype=np.uint8)) # 키 입력 처리 key = cv2.waitKey(1) key_masked = key & 0xFF # 키 디버그 모드: 모든 키 코드 출력 if key_debug_mode and key != -1: print(f"Key pressed - Full: {key}, Masked: {key_masked}, Char: {chr(key_masked) if 32 <= key_masked < 127 else 'N/A'}") if key_masked == ord('p'): # 키 디버그 모드 토글 key_debug_mode = not key_debug_mode if key_debug_mode: print("\n=== 키 디버그 모드 활성화 ===") print("이제 모든 키 입력이 콘솔에 출력됩니다.") print("방향키를 눌러 코드를 확인하세요.") print("다시 'p' 키를 누르면 비활성화됩니다.") else: print("\n키 디버그 모드 비활성화") elif key_masked == ord('q'): # 종료 print("\n종료 요청") break elif key_masked == ord('s'): # 시작/정지 토글 is_running = not is_running status = "시작" if is_running else "정지" print(f"\n자율주행 {status}") elif key_masked == ord('+') or key_masked == ord('='): # Kp 증가 controller.kp += 0.05 print(f"\nKp 증가: {controller.kp:.2f}") elif key_masked == ord('-'): # Kp 감소 controller.kp = max(0.05, controller.kp - 0.05) print(f"\nKp 감소: {controller.kp:.2f}") elif key_masked == ord('w'): # 속도 증가 controller.base_speed = min(200, controller.base_speed + 10) print(f"\n속도 증가: {controller.base_speed}") elif key_masked == ord('x'): # 속도 감소 controller.base_speed = max(50, controller.base_speed - 10) print(f"\n속도 감소: {controller.base_speed}") elif key_masked == ord('a'): # k_angle 증가 (방향 전환 더 빠르게) controller.k_angle += 0.1 print(f"\nk_angle 증가: {controller.k_angle:.2f} (방향 전환 더 민감)") elif key_masked == ord('d'): # k_angle 감소 controller.k_angle = max(0.1, controller.k_angle - 0.1) print(f"\nk_angle 감소: {controller.k_angle:.2f} (방향 전환 덜 민감)") elif key_masked == ord('z'): # k_position 증가 controller.k_position += 0.1 print(f"\nk_position 증가: {controller.k_position:.2f} (위치 오차 더 민감)") elif key_masked == ord('v'): # k_position 감소 controller.k_position = max(0.1, controller.k_position - 0.1) print(f"\nk_position 감소: {controller.k_position:.2f} (위치 오차 덜 민감)") elif key_masked == ord('f'): # 프레임 스킵 증가 (지연 감소) frame_skip = min(3, frame_skip + 1) print(f"\n프레임 스킵: {frame_skip} (처리 속도 향상, 지연 감소)") elif key_masked == ord('g'): # 프레임 스킵 감소 frame_skip = max(0, frame_skip - 1) print(f"\n프레임 스킵: {frame_skip} (더 부드러운 제어)") elif key_masked == ord('['): # 감지(초록) 영역을 위로 확장 → roi_far_y 감소 lane_detector.roi_far_y = round(max(0.30, lane_detector.roi_far_y - 0.05), 2) print(f"\n감지 영역 위로 확장: roi_far_y={lane_detector.roi_far_y:.2f} (초록 더 길게)") elif key_masked == ord(']'): # 감지(초록) 영역을 축소 → roi_far_y 증가 (near 경계 아래로는 못 감) lane_detector.roi_far_y = round(min(lane_detector.roi_near_y - 0.05, lane_detector.roi_far_y + 0.05), 2) print(f"\n감지 영역 축소: roi_far_y={lane_detector.roi_far_y:.2f} (초록 더 짧게)") elif key_masked == ord(','): # 흰색 밝기 최소값 ↓ → 어두운 흰색도 감지 (초록 더 많이/길게) white_v_min = max(0, white_v_min - 5) lane_detector.white_lower = np.array([white_h_min, white_s_min, white_v_min], dtype=np.uint8) print(f"\n흰색 밝기 최소 ↓: {white_v_min} (어두운 흰색도 감지)") elif key_masked == ord('.'): # 흰색 밝기 최소값 ↑ → 아주 밝은 흰색만 감지 (초록 줄어듦) white_v_min = min(255, white_v_min + 5) lane_detector.white_lower = np.array([white_h_min, white_s_min, white_v_min], dtype=np.uint8) print(f"\n흰색 밝기 최소 ↑: {white_v_min}") elif key_masked == ord(';'): # 흰색 채도 최대값 ↑ → 회색빛/색바랜 흰색도 포함 (초록 더 많이) white_s_max = min(255, white_s_max + 5) lane_detector.white_upper = np.array([white_h_max, white_s_max, white_v_max], dtype=np.uint8) print(f"\n흰색 채도 최대 ↑: {white_s_max} (회색빛도 포함)") elif key_masked == ord("'"): # 흰색 채도 최대값 ↓ → 순수한 흰색만 (초록 줄어듦) white_s_max = max(0, white_s_max - 5) lane_detector.white_upper = np.array([white_h_max, white_s_max, white_v_max], dtype=np.uint8) print(f"\n흰색 채도 최대 ↓: {white_s_max}") elif key_masked == ord('1'): # 차선 색: 흰색 lane_detector.set_lane_color('white') save_lane_color(config_path, 'white') print("\n차선 색 선택: 흰색 (white)") elif key_masked == ord('2'): # 차선 색: 검정 lane_detector.set_lane_color('black') save_lane_color(config_path, 'black') print("\n차선 색 선택: 검정 (black)") elif key_masked == ord('3'): # 차선 색: 주황 lane_detector.set_lane_color('orange') save_lane_color(config_path, 'orange') print("\n차선 색 선택: 주황 (orange)") elif key_masked == ord('y'): # YOLO 실행 주기 증가 (더 자주 실행 → 정확도 향상, 속도 감소) if object_detection_enabled: yolo_interval = max(1, yolo_interval - 1) print(f"\nYOLO 실행 주기: {yolo_interval}프레임마다 (더 자주 실행)") else: print("\nYOLO가 비활성화되어 있습니다.") elif key_masked == ord('u'): # YOLO 실행 주기 감소 (덜 자주 실행 → 속도 향상, 정확도 감소) if object_detection_enabled: yolo_interval = min(10, yolo_interval + 1) print(f"\nYOLO 실행 주기: {yolo_interval}프레임마다 (덜 자주 실행)") else: print("\nYOLO가 비활성화되어 있습니다.") elif key_masked == ord('m'): # 수동/자동 모드 전환 manual_mode = not manual_mode if manual_mode: manual_speed = 0 manual_steering = 90 # 중립 is_running = False # 자동 모드 정지 print("\n수동 조종 모드 활성화") print(" i/k (또는 ↑/↓): 속도 조절") print(" j/l (또는 ←/→): 조향 조절") print(" n: 조향 중립") else: print("\n자동 주행 모드 활성화") elif key_masked == ord('n') and manual_mode: # 조향 중립으로 리셋 (수동 모드) manual_steering = controller.steering_neutral print(f"\n조향 중립: {manual_steering}") elif key_masked == ord('e'): # 조향 중립 트림 ← (캘리브레이션): 중립값 감소 controller.steering_neutral = max(controller.steering_min, controller.steering_neutral - 1) if manual_mode: manual_steering = controller.steering_neutral save_steering_neutral(config_path, controller.steering_neutral) print(f"\n조향 중립 트림 ←: {controller.steering_neutral} (config 저장)") elif key_masked == ord('r'): # 조향 중립 트림 → (캘리브레이션): 중립값 증가 controller.steering_neutral = min(controller.steering_max, controller.steering_neutral + 1) if manual_mode: manual_steering = controller.steering_neutral save_steering_neutral(config_path, controller.steering_neutral) print(f"\n조향 중립 트림 →: {controller.steering_neutral} (config 저장)") # 수동 모드 조종 (키보드) elif key_masked == ord('i') and manual_mode: # 속도 증가 manual_speed = min(220, manual_speed + 10) print(f"\n수동 속도 증가: {manual_speed}") elif key_masked == ord('k') and manual_mode: # 속도 감소 manual_speed = max(0, manual_speed - 10) print(f"\n수동 속도 감소: {manual_speed}") elif key_masked == ord('j') and manual_mode: # 좌회전 (하드웨어 조향 방향에 맞춰 +방향) manual_steering = min(controller.steering_max, manual_steering + 5) print(f"\n수동 조향 좌회전: {manual_steering}") elif key_masked == ord('l') and manual_mode: # 우회전 (하드웨어 조향 방향에 맞춰 -방향) manual_steering = max(controller.steering_min, manual_steering - 5) print(f"\n수동 조향 우회전: {manual_steering}") # 방향키 처리 (수동 모드) - 작동하지 않을 경우 i/k/j/l 사용 elif key == 2490368 and manual_mode: # 위쪽 화살표 manual_speed = min(220, manual_speed + 10) print(f"\n수동 속도 증가: {manual_speed}") elif key == 2621440 and manual_mode: # 아래쪽 화살표 manual_speed = max(0, manual_speed - 10) print(f"\n수동 속도 감소: {manual_speed}") elif key == 2424832 and manual_mode: # 왼쪽 화살표 manual_steering = min(controller.steering_max, manual_steering + 5) print(f"\n수동 조향 좌회전: {manual_steering}") elif key == 2555904 and manual_mode: # 오른쪽 화살표 manual_steering = max(controller.steering_min, manual_steering - 5) print(f"\n수동 조향 우회전: {manual_steering}") elif key_masked == ord(' '): # 긴급 정지 is_running = False if manual_mode: manual_speed = 0 udp_sender.send_stop() print("\n긴급 정지!") elif key_masked == ord('h'): # 도움말 print_help() elif key_masked == ord('t'): # 튜닝 모드 토글 if not tuning_mode: # 튜닝 모드 활성화 tuning_mode = True if not trackbars_created: # 트랙바 윈도우 생성 (단순화된 버전) cv2.namedWindow('HSV Tuning') # 흰색 핵심 파라미터만 cv2.createTrackbar('White Brightness Min', 'HSV Tuning', white_v_min, 255, nothing) cv2.createTrackbar('White Saturation Max', 'HSV Tuning', white_s_max, 255, nothing) # 노란색 핵심 파라미터만 cv2.createTrackbar('Yellow Hue Min', 'HSV Tuning', yellow_h_min, 180, nothing) cv2.createTrackbar('Yellow Hue Max', 'HSV Tuning', yellow_h_max, 180, nothing) cv2.createTrackbar('Yellow Brightness Min', 'HSV Tuning', yellow_v_min, 255, nothing) trackbars_created = True print("\n튜닝 모드 활성화!") print("\n핵심 파라미터만 제공됩니다:") print(" - White Brightness Min: 흰색 인식 밝기 최소값 (낮을수록 어두운 흰색도 인식)") print(" - White Saturation Max: 흰색 채도 최대값 (높을수록 회색빛도 인식)") print(" - Yellow Hue Min/Max: 노란색 색상 범위 (보통 20-30)") print(" - Yellow Brightness Min: 노란색 인식 밝기 최소값") print("\n't' 키로 저장 후 종료, 'c' 키로 저장만") else: print("\n튜닝 모드 활성화!") else: # 튜닝 모드 종료 - 자동 저장 tuning_mode = False white_lower = [white_h_min, white_s_min, white_v_min] white_upper = [white_h_max, white_s_max, white_v_max] yellow_lower = [yellow_h_min, yellow_s_min, yellow_v_min] yellow_upper = [yellow_h_max, yellow_s_max, yellow_v_max] save_hsv_config(config_path, white_lower, white_upper, yellow_lower, yellow_upper) cv2.destroyWindow('HSV Tuning') print("\n튜닝 모드 종료 (설정 저장됨)") elif key_masked == ord('c'): # HSV 설정 저장 if tuning_mode: white_lower = [white_h_min, white_s_min, white_v_min] white_upper = [white_h_max, white_s_max, white_v_max] yellow_lower = [yellow_h_min, yellow_s_min, yellow_v_min] yellow_upper = [yellow_h_max, yellow_s_max, yellow_v_max] save_hsv_config(config_path, white_lower, white_upper, yellow_lower, yellow_upper) else: print("\n튜닝 모드가 아닙니다. 't' 키로 튜닝 모드를 활성화하세요.") except KeyboardInterrupt: print("\n\n사용자 중단 (Ctrl+C)") except Exception as e: print(f"\n오류 발생: {e}") import traceback traceback.print_exc() finally: # 정리 print("\n정리 중...") udp_sender.send_stop() time.sleep(0.2) udp_sender.disconnect() video_stream.disconnect() cv2.destroyAllWindows() # 통계 출력 elapsed_time = time.time() - start_time print(f"\n=== 실행 통계 ===") print(f"실행 시간: {elapsed_time:.1f}초") print(f"총 프레임: {frame_count}") print(f"평균 FPS: {frame_count / elapsed_time:.1f}") print("\n종료 완료") if __name__ == "__main__": main()