test4.py - master - YSLM/Path_Constructor

test4.py
903 строки39 KB
YSLM
Create: test4.py
07 авг 2026, 16:50
07 авг 2026, 16:50247b31e
#!/usr/bin/env python3
"""Движение к цели с обходом препятствий (реальный робот + HSV-детекция).

Камера сверху видит реального робота с ArUco-маркером ID 4.
Пользователь рисует виртуальные стены, а программа автоматически добавляет
синие и красные объекты (объезд синих справа, красных слева).
Физические препятствия (IR-дальномер) объезжаются поворотом на месте.
"""

from __future__ import annotations

from dataclasses import dataclass, field
import heapq
import math
import socket
import time
import os

import cv2
import numpy as np
from dotenv import load_dotenv
from enum import IntEnum

# ---------- Загрузка параметров из .env ----------
load_dotenv()

def get_env_float(key: str, default: float) -> float:
    val = os.getenv(key)
    return float(val) if val is not None else default

def get_env_int(key: str, default: int) -> int:
    val = os.getenv(key)
    return int(val) if val is not None else default

def get_env_tuple(key: str, default: tuple) -> tuple:
    val = os.getenv(key)
    if val is None:
        return default
    try:
        parts = val.split(',')
        return tuple(int(x.strip()) for x in parts)
    except:
        return default

# --------------------- Основные параметры ---------------------
CAMERA_INDEX = get_env_int('CAMERA_INDEX', 0)
ROBOT_MARKER_ID = get_env_int('ROBOT_MARKER_ID', 4)
ROBOT_ADDRESS = ("192.168.4.1", 8888)

LINEAR_SPEED_MM_S = get_env_float('LINEAR_SPEED_MM_S', 220)
ANGULAR_SPEED_MRAD_S = get_env_float('ANGULAR_SPEED_MRAD_S', 3000)

ANGLE_TOLERANCE_DEG = get_env_float('ANGLE_TOLERANCE_DEG', 15)
ANGLE_TOLERANCE_RAD = math.radians(ANGLE_TOLERANCE_DEG)
HYSTERESIS_DEG = get_env_float('HYSTERESIS_DEG', 5.0)
HYSTERESIS_RAD = math.radians(HYSTERESIS_DEG)
MIN_TURN_SWITCH_TIME = get_env_float('MIN_TURN_SWITCH_TIME', 0.3)

GOAL_TOLERANCE_PX = get_env_int('GOAL_TOLERANCE_PX', 42)
WAYPOINT_TOLERANCE_PX = get_env_int('WAYPOINT_TOLERANCE_PX', 28)

GRID_CELL_SIZE_PX = get_env_int('GRID_CELL_SIZE_PX', 25)
WALL_BRUSH_RADIUS_PX = get_env_int('WALL_BRUSH_RADIUS_PX', 18)
ROBOT_SAFETY_RADIUS_PX = get_env_int('ROBOT_SAFETY_RADIUS_PX', 40)
START_ESCAPE_RADIUS_PX = ROBOT_SAFETY_RADIUS_PX + GRID_CELL_SIZE_PX * 2

REPLAN_PERIOD_SECONDS = get_env_float('REPLAN_PERIOD_SECONDS', 0.65)
SEND_PERIOD_SECONDS = 0.05
TELEMETRY_TIMEOUT_SECONDS = 0.8

LEFT_LINE_THRESHOLD = get_env_int('LEFT_LINE_THRESHOLD', 5)
RIGHT_LINE_THRESHOLD = get_env_int('RIGHT_LINE_THRESHOLD', 5)
OBSTACLE_STOP_CM = get_env_int('OBSTACLE_STOP_CM', 14)

ROBOT_LED_CLEAR_RADIUS = get_env_int('ROBOT_LED_CLEAR_RADIUS', 45)

HSV_BLUE_LOWER = np.array(get_env_tuple('BLUE_LOWER', (95, 100, 70)), dtype=np.uint8)
HSV_BLUE_UPPER = np.array(get_env_tuple('BLUE_UPPER', (130, 255, 255)), dtype=np.uint8)
HSV_RED_LOWER1 = np.array(get_env_tuple('RED_LOWER1', (0, 110, 80)), dtype=np.uint8)
HSV_RED_UPPER1 = np.array(get_env_tuple('RED_UPPER1', (10, 255, 255)), dtype=np.uint8)
HSV_RED_LOWER2 = np.array(get_env_tuple('RED_LOWER2', (170, 110, 80)), dtype=np.uint8)
HSV_RED_UPPER2 = np.array(get_env_tuple('RED_UPPER2', (179, 255, 255)), dtype=np.uint8)
HSV_GREEN_LOWER = np.array(get_env_tuple('GREEN_LOWER', (40, 50, 50)), dtype=np.uint8)
HSV_GREEN_UPPER = np.array(get_env_tuple('GREEN_UPPER', (80, 255, 255)), dtype=np.uint8)

MIN_OBJECT_AREA = get_env_int('MIN_OBJECT_AREA', 100)
PENALTY_WEIGHT = get_env_float('PENALTY_WEIGHT', 5.0)

INFO_PANEL_HEIGHT = get_env_int('INFO_PANEL_HEIGHT', 150)

# Параметры обхода физических препятствий
OBSTACLE_AVOID_TIMEOUT = 3.0
STUCK_TIMEOUT = 2.5
SAFETY_ZONE_STUCK_TIMEOUT = 1.5
TURN_ATTEMPTS_BEFORE_REPLAN = get_env_int('TURN_ATTEMPTS_BEFORE_REPLAN', 8)   # сколько раз подряд повторяется команда поворота
print(TURN_ATTEMPTS_BEFORE_REPLAN)
Cell = tuple[int, int]

@dataclass
class SafetyData:
    line_left: int = 1023
    line_right: int = 1023
    ir_cm: int = 60

class RobotMode(IntEnum):
    IDLE = 0
    FOLLOW = 1
    AVOID_IR = 2
    STUCK = 3

@dataclass
class PlannerState:
    obstacle_mask: np.ndarray
    mask_blue: np.ndarray
    mask_red: np.ndarray
    mask_green: np.ndarray
    frame_width: int
    frame_height: int

    goal_pixel: np.ndarray | None = None
    green_centers: list[np.ndarray] = field(default_factory=list)
    path_points: list[np.ndarray] = field(default_factory=list)
    waypoint_index: int = 0
    paused: bool = True
    drawing: bool = False
    erasing: bool = False
    map_changed: bool = True
    last_plan_time: float = 0.0
    status: str = "RIGHT CLICK: SET GOAL"

    last_turn_direction: int = 0
    last_turn_time: float = 0.0

    # для обхода физических препятствий (IR)
    obstacle_avoid_active: bool = False
    obstacle_avoid_direction: int = 1
    obstacle_avoid_start_time: float = 0.0
    obstacle_avoid_switched: bool = False

    # для антизалипания
    last_command_time: float = 0.0
    stuck_rotation_active: bool = False
    stuck_rotation_start: float = 0.0
    stuck_rotation_direction: int = 1

    # счётчик повторяющихся команд
    command_counter: int = 0
    last_command_str: str = ""

    mode: int = RobotMode.IDLE
    ir_avoid_start: float = 0.0
    ir_avoid_dir: int = 1
    last_pos: np.ndarray | None = None
    stuck_start: float = 0.0
    stuck_dir: int = 1

# ------------------- Вспомогательные функции -------------------
def filter_small_objects(mask: np.ndarray, min_area: int) -> np.ndarray:
    if min_area <= 0:
        return mask
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    filtered = np.zeros_like(mask)
    for cnt in contours:
        if cv2.contourArea(cnt) >= min_area:
            cv2.drawContours(filtered, [cnt], -1, 255, -1)
    return filtered

def clear_robot_from_masks(robot_center: np.ndarray, mask_blue: np.ndarray,
                           mask_red: np.ndarray, radius_px: int = 45) -> None:
    center = tuple(np.rint(robot_center).astype(int))
    cv2.circle(mask_blue, center, radius_px, 0, -1)
    cv2.circle(mask_red, center, radius_px, 0, -1)

def parse_safety(line: str) -> SafetyData | None:
    parts = line.split()
    if len(parts) != 12 or parts[0] != "TEL":
        return None
    try:
        return SafetyData(
            line_left=int(parts[8]),
            line_right=int(parts[9]),
            ir_cm=int(parts[10]),
        )
    except ValueError:
        return None

def receive_telemetry(conn: socket.socket, buffer: bytes, current: SafetyData) -> tuple[bytes, SafetyData, bool]:
    received = False
    while True:
        try:
            packet = conn.recv(2048)
        except BlockingIOError:
            break
        if not packet:
            raise ConnectionError("TCP-соединение закрыто")
        buffer += packet
    while b"\n" in buffer:
        raw_line, buffer = buffer.split(b"\n", 1)
        line = raw_line.decode("ascii", errors="replace").strip()
        parsed = parse_safety(line)
        if parsed is not None:
            current = parsed
            received = True
    return buffer, current, received

def send(conn: socket.socket, command: str) -> None:
    try:
        conn.sendall((command + "\n").encode("ascii"))
    except OSError:
        pass

def marker_geometry(corners: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    points = corners.reshape(4, 2)
    center = points.mean(axis=0)
    front = 0.5 * (points[0] + points[1])
    heading = front - center
    heading /= max(float(np.linalg.norm(heading)), 1.0)
    return center.astype(np.float32), heading.astype(np.float32)

def signed_angle(heading: np.ndarray, vector: np.ndarray) -> float:
    hx, hy = float(heading[0]), -float(heading[1])
    vx, vy = float(vector[0]), -float(vector[1])
    return math.atan2(hx * vy - hy * vx, hx * vx + hy * vy)

# ------------------- Улучшенное распознавание ArUco -------------------
def preprocess_frame(frame: np.ndarray) -> np.ndarray:
    lab = cv2.cvtColor(frame, cv2.COLOR_BGR2LAB)
    l, a, b = cv2.split(lab)
    clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8))
    l_enhanced = clahe.apply(l)
    lab_enhanced = cv2.merge((l_enhanced, a, b))
    return cv2.cvtColor(lab_enhanced, cv2.COLOR_LAB2BGR)

def create_stable_detector() -> cv2.aruco.ArucoDetector:
    dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
    params = cv2.aruco.DetectorParameters()
    params.adaptiveThreshWinSizeMin = 3
    params.adaptiveThreshWinSizeMax = 23
    params.adaptiveThreshWinSizeStep = 10
    params.adaptiveThreshConstant = 7
    params.errorCorrectionRate = 0.8
    params.perspectiveRemovePixelPerCell = 4
    params.perspectiveRemoveIgnoredMarginPerCell = 0.13
    params.minMarkerDistanceRate = 0.05
    if hasattr(params, 'useAruco3Detection'):
        params.useAruco3Detection = True
    return cv2.aruco.ArucoDetector(dictionary, params)

def detect_markers_stable(frame: np.ndarray, detector: cv2.aruco.ArucoDetector,
                          target_id: int) -> tuple[list, np.ndarray | None]:
    enhanced = preprocess_frame(frame)
    corners, ids, _ = detector.detectMarkers(enhanced)
    if ids is not None and target_id in ids:
        idx = np.where(ids == target_id)[0][0]
        return [corners[idx]], np.array([target_id])
    corners, ids, _ = detector.detectMarkers(frame)
    if ids is not None and target_id in ids:
        idx = np.where(ids == target_id)[0][0]
        return [corners[idx]], np.array([target_id])
    gamma = 1.5
    inv_gamma = 1.0 / gamma
    table = np.array([((i / 255.0) ** inv_gamma) * 255 for i in np.arange(0, 256)]).astype("uint8")
    gamma_corrected = cv2.LUT(frame, table)
    corners, ids, _ = detector.detectMarkers(gamma_corrected)
    if ids is not None and target_id in ids:
        idx = np.where(ids == target_id)[0][0]
        return [corners[idx]], np.array([target_id])
    return None, None

# ------------------- Планировщик -------------------
def grid_shape(width: int, height: int) -> tuple[int, int]:
    return math.ceil(height / GRID_CELL_SIZE_PX), math.ceil(width / GRID_CELL_SIZE_PX)

def pixel_to_cell(point: np.ndarray, rows: int, cols: int) -> Cell:
    x, y = int(point[0]), int(point[1])
    return max(0, min(rows - 1, y // GRID_CELL_SIZE_PX)), max(0, min(cols - 1, x // GRID_CELL_SIZE_PX))

def cell_center(cell: Cell, width: int, height: int) -> np.ndarray:
    row, col = cell
    x = min(width - 1, col * GRID_CELL_SIZE_PX + GRID_CELL_SIZE_PX / 2)
    y = min(height - 1, row * GRID_CELL_SIZE_PX + GRID_CELL_SIZE_PX / 2)
    return np.array([x, y], dtype=np.float32)

def inflate_obstacles(mask: np.ndarray) -> np.ndarray:
    diameter = 2 * ROBOT_SAFETY_RADIUS_PX + 1
    kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (diameter, diameter))
    return cv2.dilate(mask, kernel)

def blocked_cells_from_mask(mask: np.ndarray) -> tuple[set[Cell], int, int]:
    height, width = mask.shape
    rows, cols = grid_shape(width, height)
    blocked = set()
    for row in range(rows):
        y0 = row * GRID_CELL_SIZE_PX
        y1 = min(height, y0 + GRID_CELL_SIZE_PX)
        for col in range(cols):
            x0 = col * GRID_CELL_SIZE_PX
            x1 = min(width, x0 + GRID_CELL_SIZE_PX)
            if np.any(mask[y0:y1, x0:x1] > 0):
                blocked.add((row, col))
    return blocked, rows, cols

def compute_penalty(mask_blue: np.ndarray, mask_red: np.ndarray,
                    rows: int, cols: int, weight: float) -> np.ndarray:
    h, w = mask_blue.shape
    cell_blue = np.zeros((rows, cols), dtype=np.float32)
    cell_red = np.zeros((rows, cols), dtype=np.float32)
    for r in range(rows):
        y0 = r * GRID_CELL_SIZE_PX
        y1 = min(h, y0 + GRID_CELL_SIZE_PX)
        for c in range(cols):
            x0 = c * GRID_CELL_SIZE_PX
            x1 = min(w, x0 + GRID_CELL_SIZE_PX)
            cell_blue[r, c] = np.sum(mask_blue[y0:y1, x0:x1])
            cell_red[r, c] = np.sum(mask_red[y0:y1, x0:x1])
    kernel = np.array([[0, 0, 0], [-1, 0, 1], [0, 0, 0]], dtype=np.float32)
    grad_blue = cv2.filter2D(cell_blue, -1, kernel, borderType=cv2.BORDER_CONSTANT)
    grad_red = cv2.filter2D(cell_red, -1, kernel, borderType=cv2.BORDER_CONSTANT)
    penalty = np.zeros((rows, cols), dtype=np.float32)
    penalty += np.maximum(0, -grad_blue) * weight
    penalty += np.maximum(0, grad_red) * weight
    return penalty

def neighbors(cell: Cell, rows: int, cols: int) -> list[tuple[Cell, float]]:
    row, col = cell
    result = []
    for dr in (-1, 0, 1):
        for dc in (-1, 0, 1):
            if dr == 0 and dc == 0:
                continue
            nr, nc = row + dr, col + dc
            if 0 <= nr < rows and 0 <= nc < cols:
                cost = math.sqrt(2.0) if dr and dc else 1.0
                result.append(((nr, nc), cost))
    return result

def heuristic(a: Cell, b: Cell) -> float:
    return math.hypot(a[0] - b[0], a[1] - b[1])

def reconstruct_path(came_from: dict[Cell, Cell], current: Cell) -> list[Cell]:
    path = [current]
    while current in came_from:
        current = came_from[current]
        path.append(current)
    path.reverse()
    return path

def astar(start: Cell, goal: Cell, blocked: set[Cell], rows: int, cols: int,
          penalty: np.ndarray) -> list[Cell] | None:
    if goal in blocked:
        return None
    blocked = set(blocked)
    blocked.discard(start)
    queue: list[tuple[float, float, Cell]] = []
    heapq.heappush(queue, (heuristic(start, goal), 0.0, start))
    came_from: dict[Cell, Cell] = {}
    best_cost: dict[Cell, float] = {start: 0.0}
    while queue:
        _priority, current_cost, current = heapq.heappop(queue)
        if current == goal:
            return reconstruct_path(came_from, current)
        if current_cost > best_cost.get(current, float("inf")):
            continue
        for candidate, step_cost in neighbors(current, rows, cols):
            if candidate in blocked:
                continue
            dr = candidate[0] - current[0]
            dc = candidate[1] - current[1]
            if dr and dc:
                side_a = (current[0] + dr, current[1])
                side_b = (current[0], current[1] + dc)
                if side_a in blocked or side_b in blocked:
                    continue
            pen = penalty[candidate[0], candidate[1]]
            new_cost = current_cost + step_cost + pen
            if new_cost >= best_cost.get(candidate, float("inf")):
                continue
            best_cost[candidate] = new_cost
            came_from[candidate] = current
            heapq.heappush(queue, (new_cost + heuristic(candidate, goal), new_cost, candidate))
    return None

def line_is_free(p1: np.ndarray, p2: np.ndarray, mask: np.ndarray) -> bool:
    dist = max(float(np.linalg.norm(p2 - p1)), 1.0)
    n = max(2, int(dist / 3.0) + 1)
    xs = np.linspace(p1[0], p2[0], n)
    ys = np.linspace(p1[1], p2[1], n)
    h, w = mask.shape
    xi = np.clip(np.rint(xs).astype(int), 0, w - 1)
    yi = np.clip(np.rint(ys).astype(int), 0, h - 1)
    return not bool(np.any(mask[yi, xi] > 0))

def simplify_path(points: list[np.ndarray], mask: np.ndarray) -> list[np.ndarray]:
    if len(points) <= 2:
        return points
    simplified = [points[0]]
    idx = 0
    MAX_GAP_PX = 150
    while idx < len(points) - 1:
        best_idx = idx + 1
        for i in range(idx + 1, min(idx + 6, len(points))):
            dist = np.linalg.norm(points[i] - points[idx])
            if dist > MAX_GAP_PX:
                break
            if line_is_free(points[idx], points[i], mask):
                best_idx = i
        simplified.append(points[best_idx])
        idx = best_idx
    return simplified

def check_path_ahead(robot_center: np.ndarray, heading: np.ndarray,
                     obstacle_mask: np.ndarray, distance_px: int = 50) -> bool:
    if robot_center is None or heading is None:
        return True
    end_point = robot_center + heading * distance_px
    h, w = obstacle_mask.shape
    if not (0 <= end_point[0] < w and 0 <= end_point[1] < h):
        return False
    return line_is_free(robot_center, end_point, obstacle_mask)

def plan_path(robot_pixel: np.ndarray, goal_pixel: np.ndarray,
              obstacle_mask: np.ndarray, mask_blue: np.ndarray,
              mask_red: np.ndarray, penalty_weight: float) -> tuple[list[np.ndarray] | None, np.ndarray]:
    height, width = obstacle_mask.shape
    inflated = inflate_obstacles(obstacle_mask)
    planning_mask = inflated.copy()
    robot_point = tuple(np.rint(robot_pixel).astype(int))
    escape = np.zeros_like(planning_mask)
    cv2.circle(escape, robot_point, START_ESCAPE_RADIUS_PX, 255, -1)
    may_clear = (escape > 0) & (obstacle_mask == 0)
    planning_mask[may_clear] = 0
    blocked, rows, cols = blocked_cells_from_mask(planning_mask)
    start = pixel_to_cell(robot_pixel, rows, cols)
    goal = pixel_to_cell(goal_pixel, rows, cols)
    penalty = compute_penalty(mask_blue, mask_red, rows, cols, penalty_weight)
    cells = astar(start, goal, blocked, rows, cols, penalty)
    if cells is None:
        return None, inflated
    points = [cell_center(c, width, height) for c in cells]
    points[0] = robot_pixel.astype(np.float32).copy()
    points[-1] = goal_pixel.astype(np.float32).copy()
    return simplify_path(points, planning_mask), inflated

# ------------------- Интерфейс рисования -------------------
def apply_brush(state: PlannerState, x: int, y: int) -> None:
    if not (0 <= x < state.frame_width and 0 <= y < state.frame_height):
        return
    if state.drawing:
        cv2.circle(state.obstacle_mask, (x, y), WALL_BRUSH_RADIUS_PX, 255, -1)
        state.map_changed = True
    if state.erasing:
        cv2.circle(state.obstacle_mask, (x, y), WALL_BRUSH_RADIUS_PX + 5, 0, -1)
        state.map_changed = True

def mouse_callback(event, x, y, flags, state: PlannerState) -> None:
    if y >= state.frame_height:
        return
    if event == cv2.EVENT_RBUTTONDOWN:
        state.goal_pixel = np.array([x, y], dtype=np.float32)
        state.path_points.clear()
        state.waypoint_index = 0
        state.map_changed = True
        state.paused = False
        state.status = "NEW GOAL"
        return
    if event == cv2.EVENT_MBUTTONDOWN:
        state.erasing = True
        state.drawing = False
        apply_brush(state, x, y)
        return
    if event == cv2.EVENT_MBUTTONUP:
        state.erasing = False
        return
    if event == cv2.EVENT_LBUTTONDOWN:
        erase = bool(flags & cv2.EVENT_FLAG_SHIFTKEY)
        state.drawing = not erase
        state.erasing = erase
        apply_brush(state, x, y)
        return
    if event == cv2.EVENT_LBUTTONUP:
        state.drawing = False
        state.erasing = False
        return
    if event == cv2.EVENT_MOUSEMOVE:
        left = bool(flags & cv2.EVENT_FLAG_LBUTTON)
        middle = bool(flags & cv2.EVENT_FLAG_MBUTTON)
        if not left and not middle:
            state.drawing = False
            state.erasing = False
            return
        if middle:
            state.drawing = False
            state.erasing = True
        elif left:
            erase = bool(flags & cv2.EVENT_FLAG_SHIFTKEY)
            state.drawing = not erase
            state.erasing = erase
        apply_brush(state, x, y)

# ------------------- Отрисовка -------------------
def overlay_mask(frame: np.ndarray, mask: np.ndarray, color: tuple, alpha: float) -> None:
    sel = mask > 0
    if not np.any(sel):
        return
    layer = np.empty_like(frame)
    layer[:] = color
    blended = cv2.addWeighted(frame, 1.0 - alpha, layer, alpha, 0)
    frame[sel] = blended[sel]

def select_waypoint(state: PlannerState, robot_pixel: np.ndarray) -> np.ndarray | None:
    while state.waypoint_index < len(state.path_points):
        wp = state.path_points[state.waypoint_index]
        dist = float(np.linalg.norm(wp - robot_pixel))
        tol = GOAL_TOLERANCE_PX if state.waypoint_index == len(state.path_points) - 1 else WAYPOINT_TOLERANCE_PX
        if dist > tol:
            return wp
        state.waypoint_index += 1
    return None

def find_green_cubes(mask_green: np.ndarray, min_area: int = 200) -> list[np.ndarray]:
    contours, _ = cv2.findContours(mask_green, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    contours = [c for c in contours if cv2.contourArea(c) >= min_area]
    if len(contours) < 2:
        return []
    contours = sorted(contours, key=cv2.contourArea, reverse=True)[:2]
    centers = []
    for c in contours:
        M = cv2.moments(c)
        if M["m00"] != 0:
            cx = int(M["m10"] / M["m00"])
            cy = int(M["m01"] / M["m00"])
            centers.append(np.array([cx, cy], dtype=np.float32))
    return centers

def render_display(frame, state, inflated, robot_center, robot_heading,
                   current_wp, command, safety) -> np.ndarray:
    display = frame.copy()
    safety_only = cv2.subtract(inflated, state.obstacle_mask)
    overlay_mask(display, safety_only, (0, 170, 255), 0.20)
    overlay_mask(display, state.obstacle_mask, (40, 40, 230), 0.48)
    overlay_mask(display, state.mask_blue, (255, 0, 0), 0.15)
    overlay_mask(display, state.mask_red, (0, 0, 255), 0.15)
    overlay_mask(display, state.mask_green, (0, 255, 0), 0.15)

    if len(state.green_centers) == 2:
        c1 = tuple(np.rint(state.green_centers[0]).astype(int))
        c2 = tuple(np.rint(state.green_centers[1]).astype(int))
        margin = 30
        x1 = min(c1[0], c2[0]) - margin
        y1 = min(c1[1], c2[1]) - margin
        x2 = max(c1[0], c2[0]) + margin
        y2 = max(c1[1], c2[1]) + margin
        overlay = display.copy()
        cv2.rectangle(overlay, (x1, y1), (x2, y2), (0, 255, 0), -1)
        cv2.addWeighted(overlay, 0.15, display, 0.85, 0, display)
        cv2.rectangle(display, (x1, y1), (x2, y2), (0, 255, 0), 2)

    if len(state.path_points) >= 2:
        pts = np.rint(np.array(state.path_points)).astype(np.int32)
        cv2.polylines(display, [pts], False, (255, 170, 0), 3)
        for p in pts[1:-1]:
            cv2.circle(display, tuple(p), 5, (255, 170, 0), -1)

    if state.goal_pixel is not None:
        g = tuple(np.rint(state.goal_pixel).astype(int))
        cv2.circle(display, g, GOAL_TOLERANCE_PX, (60, 210, 60), 2)
        cv2.circle(display, g, 7, (60, 210, 60), -1)

    if current_wp is not None:
        w = tuple(np.rint(current_wp).astype(int))
        cv2.circle(display, w, 10, (0, 230, 255), 2)

    if robot_center is not None and robot_heading is not None:
        c = tuple(np.rint(robot_center).astype(int))
        f = tuple(np.rint(robot_center + robot_heading * 55).astype(int))
        cv2.circle(display, c, 5, (255, 255, 255), -1)
        cv2.arrowedLine(display, c, f, (255, 255, 255), 3)

    panel = np.full((INFO_PANEL_HEIGHT, state.frame_width, 3), (24, 24, 24), dtype=np.uint8)
    cv2.putText(panel, f"STATE: {state.status}", (16, 31), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                (90, 230, 90) if command != "STOP" else (80, 150, 255), 2, cv2.LINE_AA)
    cv2.putText(panel, f"COMMAND: {command}    LINE: {safety.line_left}/{safety.line_right}    IR: {safety.ir_cm} cm",
                (16, 63), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (235, 235, 235), 1, cv2.LINE_AA)
    cv2.putText(panel, "LMB draw | Shift+LMB/MMB erase | RMB goal | Space pause",
                (16, 98), cv2.FONT_HERSHEY_SIMPLEX, 0.49, (210, 210, 210), 1, cv2.LINE_AA)
    cv2.putText(panel, "C clear all | R clear goal | Esc stop",
                (16, 128), cv2.FONT_HERSHEY_SIMPLEX, 0.49, (210, 210, 210), 1, cv2.LINE_AA)
    return np.vstack((display, panel))

# ------------------- Основной цикл -------------------
def main() -> None:
    detector = create_stable_detector()
    camera = cv2.VideoCapture(CAMERA_INDEX)
    if not camera.isOpened():
        raise RuntimeError("Камера не открылась")
    ok, first_frame = camera.read()
    if not ok:
        camera.release()
        raise RuntimeError("Не удалось получить первый кадр")
    h, w = first_frame.shape[:2]
    state = PlannerState(
        obstacle_mask=np.zeros((h, w), dtype=np.uint8),
        mask_blue=np.zeros((h, w), dtype=np.uint8),
        mask_red=np.zeros((h, w), dtype=np.uint8),
        mask_green=np.zeros((h, w), dtype=np.uint8),
        frame_width=w,
        frame_height=h,
        mode=RobotMode.IDLE,
        ir_avoid_start=0.0,
        ir_avoid_dir=1,
        last_pos=None,
        stuck_start=0.0,
        stuck_dir=1,
    )
    connection = socket.create_connection(ROBOT_ADDRESS, timeout=3.0)
    connection.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
    connection.setblocking(False)
    send(connection, "GET")
    window = "Path planner with color priorities"
    cv2.namedWindow(window)
    cv2.setMouseCallback(window, mouse_callback, state)
    telemetry_buffer = b""
    safety = SafetyData()
    last_telemetry_time = 0.0
    prev_send_time = 0.0
    use_first = True

    # Новые константы управления
    KP_ANGLE = 1.8
    MAX_ANGULAR = ANGULAR_SPEED_MRAD_S
    IR_AVOID_TURN_TIME = 1.2
    REPLAN_PERIOD = 3.0
    STUCK_TIME = 2.5

    try:
        while True:
            if use_first:
                frame = first_frame
                use_first = False
            else:
                ok, frame = camera.read()
                if not ok:
                    break

            telemetry_buffer, safety, telemetry_received = receive_telemetry(
                connection, telemetry_buffer, safety
            )
            if telemetry_received:
                last_telemetry_time = time.monotonic()

            corners, ids = detect_markers_stable(frame, detector, ROBOT_MARKER_ID)
            markers = {}
            if ids is not None:
                for c, i in zip(corners, ids.flatten()):
                    markers[int(i)] = c
                cv2.aruco.drawDetectedMarkers(frame, corners, ids)

            command = "STOP"
            robot_center = None
            robot_heading = None
            current_wp = None
            inflated = inflate_obstacles(state.obstacle_mask)
            now = time.monotonic()

            telemetry_stale = (last_telemetry_time == 0.0 or now - last_telemetry_time > TELEMETRY_TIMEOUT_SECONDS)
            black_border = (safety.line_left < LEFT_LINE_THRESHOLD or safety.line_right < RIGHT_LINE_THRESHOLD)

            # --- Автомат состояний ---
            if telemetry_stale:
                command = "STOP"
                state.status = "NO TELEMETRY"
            elif black_border:
                command = "STOP"
                state.status = "BLACK BORDER"
                state.paused = True
                state.mode = RobotMode.IDLE
            elif state.paused:
                command = "STOP"
                state.status = "PAUSED"
            elif state.goal_pixel is None:
                command = "STOP"
                state.status = "SET GOAL (RMB)"
            elif ROBOT_MARKER_ID not in markers:
                command = "STOP"
                state.status = f"WAIT ARUCO {ROBOT_MARKER_ID}"
            else:
                robot_center, robot_heading = marker_geometry(markers[ROBOT_MARKER_ID])
                clear_robot_from_masks(robot_center, state.mask_blue,
                                       state.mask_red, ROBOT_LED_CLEAR_RADIUS)

                # --- Перепланирование ---
                need_replan = (state.map_changed or
                               now - state.last_plan_time > REPLAN_PERIOD or
                               state.mode == RobotMode.STUCK)
                if need_replan:
                    path, inflated = plan_path(
                        robot_center, state.goal_pixel,
                        state.obstacle_mask, state.mask_blue, state.mask_red,
                        PENALTY_WEIGHT
                    )
                    state.last_plan_time = now
                    state.map_changed = False
                    if path is None:
                        state.path_points.clear()
                        state.status = "NO PATH"
                        command = "STOP"
                        state.mode = RobotMode.IDLE
                    else:
                        state.path_points = path
                        state.waypoint_index = 1 if len(path) > 1 else 0
                        state.mode = RobotMode.FOLLOW
                        state.status = "PATH FOUND"

                # --- Обработка физического препятствия (IR) ---
                if safety.ir_cm <= OBSTACLE_STOP_CM and not telemetry_stale:
                    if state.mode != RobotMode.AVOID_IR:
                        state.mode = RobotMode.AVOID_IR
                        state.ir_avoid_start = now
                        left_free = safety.line_left > LEFT_LINE_THRESHOLD
                        right_free = safety.line_right > RIGHT_LINE_THRESHOLD
                        state.ir_avoid_dir = 1 if left_free else -1
                        if not left_free and not right_free:
                            state.ir_avoid_dir = 1
                        state.status = "AVOID IR"
                    # Выполняем поворот
                    if now - state.ir_avoid_start < IR_AVOID_TURN_TIME:
                        angular = state.ir_avoid_dir * MAX_ANGULAR
                        command = f"VEL 0 {angular}"
                    else:
                        state.mode = RobotMode.FOLLOW
                        state.map_changed = True
                        state.status = "AVOID DONE"
                else:
                    if state.mode == RobotMode.AVOID_IR:
                        state.mode = RobotMode.FOLLOW
                        state.map_changed = True

                # --- Следование по пути (FOLLOW) ---
                if state.mode == RobotMode.FOLLOW and state.path_points:
                    # Проверка цели
                    goal_dist = np.linalg.norm(state.goal_pixel - robot_center)
                    if goal_dist <= GOAL_TOLERANCE_PX:
                        state.status = "GOAL REACHED"
                        command = "STOP"
                        state.paused = True
                        state.mode = RobotMode.IDLE
                    else:
                        wp = select_waypoint(state, robot_center)
                        if wp is None:
                            state.status = "REPLAN (no waypoint)"
                            state.map_changed = True
                        else:
                            current_wp = wp
                            vec = wp - robot_center
                            error_angle = signed_angle(robot_heading, vec)
                            # Если угол велик – поворачиваем на месте
                            if abs(error_angle) > math.radians(20):
                                angular = np.clip(error_angle * KP_ANGLE, -MAX_ANGULAR, MAX_ANGULAR)
                                command = f"VEL 0 {int(angular)}"
                                state.status = "TURNING"
                            else:
                                # Движение с коррекцией курса
                                angular = np.clip(error_angle * KP_ANGLE, -MAX_ANGULAR, MAX_ANGULAR)
                                lin_speed = LINEAR_SPEED_MM_S
                                if abs(angular) > 0.5 * MAX_ANGULAR:
                                    lin_speed *= 0.6
                                command = f"VEL {int(lin_speed)} {int(angular)}"
                                state.status = "DRIVING"
                else:
                    if command == "":
                        command = "STOP"

                # --- Антизалипание (STUCK) ---
                if robot_center is not None:
                    if state.last_pos is None:
                        state.last_pos = robot_center.copy()
                        state.stuck_start = now
                    else:
                        moved = np.linalg.norm(robot_center - state.last_pos)
                        if moved < 10:  # пикселей
                            if now - state.stuck_start > STUCK_TIME:
                                state.mode = RobotMode.STUCK
                                state.status = "STUCK - FORCE TURN"
                                if not hasattr(state, 'stuck_dir'):
                                    state.stuck_dir = 1 if np.random.rand() > 0.5 else -1
                                angular = state.stuck_dir * MAX_ANGULAR
                                command = f"VEL 0 {angular}"
                                state.stuck_start = now
                                state.map_changed = True
                        else:
                            state.last_pos = robot_center.copy()
                            state.stuck_start = now

                # Если мы в STUCK, продолжаем поворачивать, пока не сдвинемся
                if state.mode == RobotMode.STUCK:
                    if now - state.stuck_start < 1.5:
                        angular = state.stuck_dir * MAX_ANGULAR
                        command = f"VEL 0 {angular}"
                    else:
                        state.mode = RobotMode.FOLLOW
                        state.map_changed = True
                        state.status = "UNSTUCK"

                # Авто-финиш по зелёным кубикам (оставлено как было)
                if len(state.green_centers) == 2 and robot_center is not None:
                    dist_to_c1 = np.linalg.norm(robot_center - state.green_centers[0])
                    dist_to_c2 = np.linalg.norm(robot_center - state.green_centers[1])
                    if (dist_to_c1 < 100 and dist_to_c2 < 100) or (
                        state.goal_pixel is not None and
                        np.linalg.norm(robot_center - state.goal_pixel) <= GOAL_TOLERANCE_PX and
                        dist_to_c1 < 120 and dist_to_c2 < 120
                    ):
                        state.status = "FINISH: BETWEEN GREEN CUBES"
                        command = "STOP"
                        state.paused = True
                        state.goal_pixel = None
                        state.path_points.clear()
                        state.mode = RobotMode.IDLE

            # --- Отрисовка и отправка команд ---
            display = render_display(frame, state, inflated, robot_center, robot_heading,
                                     current_wp, command, safety)

            if now - prev_send_time >= SEND_PERIOD_SECONDS:
                send(connection, command)
                prev_send_time = now

            cv2.imshow(window, display)
            key = cv2.waitKey(1) & 0xFF

            if key == 27:
                break
            if key == ord(' '):
                state.paused = not state.paused
                if state.paused:
                    send(connection, "STOP")
                    state.mode = RobotMode.IDLE
                else:
                    state.map_changed = True
                    state.mode = RobotMode.FOLLOW
            if key in (ord('c'), ord('C')):
                # Очистка и авто-поиск зелёных кубиков (без изменений)
                state.obstacle_mask.fill(0)
                state.mask_blue.fill(0)
                state.mask_red.fill(0)
                state.mask_green.fill(0)
                state.path_points.clear()
                state.waypoint_index = 0
                state.goal_pixel = None
                state.green_centers.clear()
                state.map_changed = True
                state.mode = RobotMode.IDLE
                state.last_pos = None

                hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
                blue_mask = cv2.inRange(hsv, HSV_BLUE_LOWER, HSV_BLUE_UPPER)
                red_mask1 = cv2.inRange(hsv, HSV_RED_LOWER1, HSV_RED_UPPER1)
                red_mask2 = cv2.inRange(hsv, HSV_RED_LOWER2, HSV_RED_UPPER2)
                red_mask = cv2.bitwise_or(red_mask1, red_mask2)
                green_mask = cv2.inRange(hsv, HSV_GREEN_LOWER, HSV_GREEN_UPPER)

                blue_mask = filter_small_objects(blue_mask, MIN_OBJECT_AREA)
                red_mask = filter_small_objects(red_mask, MIN_OBJECT_AREA)
                green_mask = filter_small_objects(green_mask, MIN_OBJECT_AREA)

                state.mask_blue = cv2.bitwise_or(state.mask_blue, blue_mask)
                state.mask_red = cv2.bitwise_or(state.mask_red, red_mask)
                state.mask_green = cv2.bitwise_or(state.mask_green, green_mask)

                combined = cv2.bitwise_or(state.mask_blue, state.mask_red)
                state.obstacle_mask = cv2.bitwise_or(state.obstacle_mask, combined)

                centers = find_green_cubes(state.mask_green, min_area=200)
                state.green_centers = centers
                if len(centers) == 2:
                    state.goal_pixel = (centers[0] + centers[1]) / 2.0
                    state.paused = False
                    state.status = "AUTO GOAL SET (GREEN CUBES)"
                    state.mode = RobotMode.FOLLOW
                    state.map_changed = True
                else:
                    state.goal_pixel = None
                    state.status = "NO GREEN CUBES FOUND"

            if key in (ord('r'), ord('R')):
                send(connection, "STOP")
                state.goal_pixel = None
                state.path_points.clear()
                state.waypoint_index = 0
                state.paused = True
                state.map_changed = True
                state.mode = RobotMode.IDLE
                state.last_pos = None
                state.status = "RIGHT CLICK: SET GOAL"

    finally:
        send(connection, "STOP")
        connection.close()
        camera.release()
        cv2.destroyAllWindows()

if __name__ == "__main__":
    main()