import os
import sys
parent_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(parent_dir)
from utils import label_filter
from loguru import logger
from utils import tlabel
import zmq
import time
from by_cmd_py import by_cmd_py
import time
import signal

context = zmq.Context()
socket = context.socket(zmq.REQ)
socket.connect("tcp://localhost:6667")
logger.info("subtask yolo client init")

filter = label_filter(socket)
filter.switch_camera(2)



cmd_py_obj = by_cmd_py()
def car_stop():
    for _ in range(3):
        cmd_py_obj.send_speed_x(0)
        time.sleep(0.2)
        cmd_py_obj.send_speed_y(0)
        time.sleep(0.2)
        cmd_py_obj.send_speed_omega(0)
def signal_handler(sig, frame):
    car_stop()

offset = 0
signal.signal(signal.SIGTERM, signal_handler)
while True:
    time.sleep(0.2)
    ret, box = filter.get(tlabel.LMARK)
    if ret:
        # 宽度大于 41 停车
        print(f"width: {box[0][2] - box[0][0]} height: {box[0][3] - box[0][1]}")
    # if ret:
    #     error = (box[0][2] + box[0][0] - 320) / 2 + offset
    #     print(error)
        
    #     cmd_py_obj.send_speed_omega(-error * 0.8)
car_stop()