utils.py

# utils.py
# Helper functions for drawing and UDP command sending.

import socket, json, cv2
import numpy as np

ESP_HOST = ""
ESP_PORT = 0
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

def send_cmd(v, omega):
    msg = {"cmd": "drive", "v": float(v), "omega": float(omega)}
    sock.sendto(json.dumps(msg).encode(), (ESP_HOST, ESP_PORT))

def send_stop():
    sock.sendto(json.dumps({"cmd": "stop"}).encode(), (ESP_HOST, ESP_PORT))

# desc: Draws person box, depth mini-view, and current robot mode.
def draw_hud(frame, depth_map, box, mode, wall_cm):
    disp = frame.copy()

    if box is not None:
        x1, y1, x2, y2 = map(int, (box["x1"], box["y1"], box["x2"], box["y2"]))
        cv2.rectangle(disp, (x1, y1), (x2, y2), (0, 255, 0), 2)

    h, w = disp.shape[:2]
    overlay = disp.copy()
    cv2.rectangle(overlay, (0, 0), (w, 40), (0, 0, 0), -1)
    disp = cv2.addWeighted(overlay, 0.35, disp, 0.65, 0)

    cv2.putText(disp, f"{mode} | range={wall_cm}", (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)

    depth_small = cv2.resize(depth_map, (w // 4, h // 4))
    depth_vis = cv2.applyColorMap((depth_small * 255).astype(np.uint8), cv2.COLORMAP_INFERNO)
    disp[0:depth_vis.shape[0], 0:depth_vis.shape[1]] = depth_vis

    return disp