49 lines
1.2 KiB
Python
49 lines
1.2 KiB
Python
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)
|
|
cmd_py_obj.send_speed_x(15)
|
|
while True:
|
|
|
|
time.sleep(0.02)
|
|
ret, box = filter.get(tlabel.BASE)
|
|
# 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() |