btl143/project_main
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

暫存版本

Browse Source
This commit is contained in:
bmy
2024-07-09 00:28:41 +08:00
parent 8acecadd2a
commit da67c9a62b
11 changed files with 379 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

290
subtask.py
View File

@@ -1,7 +1,3 @@
'''
待办事项：
- 第一二個方塊自動識別，hanoi2 識別到物塊即停車，不需手動輸入第一個物塊（修改 fliter 使其能一次請求過濾多個標籤)
'''
from enum import Enum
from loguru import logger
from utils import label_filter
@@ -18,7 +14,7 @@ import re
import threading
import ctypes
cfg = None
cfg_args = None
by_cmd = None
filter = None
llm_bot = None
@@ -30,12 +26,14 @@ socket = None
context1 = None
ocr_socket = None
global_skip_queue = None
'''
description: main.py 里执行 引入全局变量
param {*} _by_cmd 控制器对象
return {*}
'''
def import_obj(_by_cmd):
def import_obj(_by_cmd, skip_queue):
global by_cmd
global filter
@@ -48,9 +46,12 @@ def import_obj(_by_cmd):
global ocr_socket
global cfg
global cfg_args
global global_skip_queue
cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.toml')
by_cmd = _by_cmd
global_skip_queue = skip_queue
# 目标检测 socket 客户端
context = zmq.Context()
@@ -59,10 +60,10 @@ def import_obj(_by_cmd):
logger.info("subtask yolo client init")
# ocr socket 客户端
context1 = zmq.Context()
ocr_socket = context1.socket(zmq.REQ)
ocr_socket.connect("tcp://localhost:6668")
logger.info("subtask ocr client init")
# context1 = zmq.Context()
# ocr_socket = context1.socket(zmq.REQ)
# ocr_socket.connect("tcp://localhost:6668")
# logger.info("subtask ocr client init")
filter = label_filter(socket)
if cfg['move_area']['llm_enable']:
@@ -192,25 +193,34 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5):
else:
by_cmd.send_distance_x(10, int(-error))
break
# 弃用 distance 校准
def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
# run_direc == 1 向前
stop_error = 0
if run_direc == 1:
by_cmd.send_speed_x(run_speed)
else:
by_cmd.send_speed_x(-run_speed)
if label == tlabel.TPLATFORM:
stop_error = 10
else:
stop_error = 15
while True:
ret, box = filter.get(label)
while not ret:
if not global_skip_queue.empty():
_ = global_skip_queue.get()
logger.error("跳过 explore_calibrate_new")
return False
ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
if ret:
# 校准速度越大 停车的条件越宽泛
if abs(error) <= 20:
# 校准速度越大 停车的条件越宽泛 20 15
if abs(error) <= stop_error:
car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 10 直接停车")
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(label)
while not ret:
@@ -218,17 +228,18 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
if abs(error) < 8:
error = error * 3
else:
error = error * 2
if error > 0:
by_cmd.send_distance_x(-10, int(error))
else:
by_cmd.send_distance_x(10, int(-error))
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
# if abs(error) < 8:
# error = error * 3
# else:
# error = error * 2
# if error > 0:
# by_cmd.send_distance_x(-10, int(error))
# else:
# by_cmd.send_distance_x(10, int(-error))
break
return True
# 任务类
class task:
@@ -326,24 +337,24 @@ class task_queuem(task):
elif self.status is task_queuem_status.EXECUTING:
if self.task_now.enable is True:
logger.info(f"[TaskM]# Start execute task function")
self.exec_thread = threading.Thread(target=self.task_now.exec)
# 启动线程
self.exec_thread.start()
while True:
if not self.exec_thread.is_alive():
break
else:
if not skip_queue.empty():
car_stop()
thread_id = self.exec_thread.ident
res = ctypes.pythonapi.PyThreadState_SetAsyncExc(thread_id, ctypes.py_object(SystemExit))
_ = skip_queue.get()
logger.error(f"{self.task_now.name} 任务在 exec 中已经被手动跳过")
self.status = task_queuem_status.IDEL # 空动作不需要阻塞巡线，直接置位
self.task_now.after() # 执行任务后处理
self.queue.task_done() # 弹出已执行的任务
return True
# self.task_now.exec() # 执行当前任务函数
# self.exec_thread = threading.Thread(target=self.task_now.exec)
# # 启动线程
# self.exec_thread.start()
# while True:
# if not self.exec_thread.is_alive():
# break
# else:
# if not skip_queue.empty():
# car_stop()
# thread_id = self.exec_thread.ident
# res = ctypes.pythonapi.PyThreadState_SetAsyncExc(thread_id, ctypes.py_object(SystemExit))
# _ = skip_queue.get()
# logger.error(f"{self.task_now.name} 任务在 exec 中已经被手动跳过")
# self.status = task_queuem_status.IDEL # 空动作不需要阻塞巡线，直接置位
# self.task_now.after() # 执行任务后处理
# self.queue.task_done() # 弹出已执行的任务
# return True
self.task_now.exec() # 执行当前任务函数
self.status = task_queuem_status.IDEL # 执行完成后为退出巡线阻塞
self.task_now.after() # 执行任务后处理
self.queue.task_done() # 弹出已执行的任务
@@ -405,12 +416,12 @@ class get_block1():
time.sleep(1)
by_cmd.send_angle_claw(25)
time.sleep(0.5)
by_cmd.send_position_axis_z(30, 80)
by_cmd.send_position_axis_z(30, 90)
time.sleep(0.5)
by_cmd.send_angle_claw_arm(175)
time.sleep(0.1)
by_cmd.send_position_axis_x(1, 110)
by_cmd.send_position_axis_x(1, 120)
time.sleep(1)
by_cmd.send_position_axis_z(30, 70)
time.sleep(0.1)
@@ -423,12 +434,14 @@ class get_block1():
by_cmd.send_angle_claw_arm(220)
time.sleep(0.5)
by_cmd.send_angle_storage(55)
time.sleep(1)
pass
def nexec(self):
# TODO 完成不执行任务的空动作
pass
def after(self):
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass
class get_block2():
@@ -460,9 +473,6 @@ class get_block2():
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
by_cmd.send_angle_claw(25)
by_cmd.send_beep(1)
time.sleep(0.5)
by_cmd.send_beep(0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 100)
@@ -503,7 +513,7 @@ class put_block():
by_cmd.send_distance_x(10, 100)
by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
by_cmd.send_angle_claw(63)
time.sleep(1)
@@ -528,9 +538,10 @@ class put_block():
time.sleep(1)
by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1.5)
by_cmd.send_angle_claw(45)
time.sleep(1)
pass
def nexec(self):
pass
@@ -590,14 +601,12 @@ class get_bball():
by_cmd.send_angle_claw_arm(36)
by_cmd.send_angle_claw(54)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
by_cmd.send_angle_claw(60)
time.sleep(1)
time.sleep(1.5)
by_cmd.send_angle_claw(25)
time.sleep(1)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(1, 30)
by_cmd.send_position_axis_x(1, 60)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, -40)
time.sleep(0.5)
@@ -650,9 +659,10 @@ class up_tower():
by_cmd.send_distance_y(-10, 50)
time.sleep(3)
by_cmd.send_angle_zhuan(10)
time.sleep(11)
time.sleep(12)
by_cmd.send_distance_y(10, 60)
time.sleep(1)
car_stop()
by_cmd.send_angle_zhuan(0)
# while True:
# pass
@@ -691,7 +701,7 @@ class get_rball():
time.sleep(0.5)
# 靠近塔
by_cmd.send_angle_scoop(20)
by_cmd.send_distance_y(-15, 50) # 50
by_cmd.send_distance_y(-15, 45) # 50
time.sleep(2)
calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1)
@@ -699,12 +709,13 @@ class get_rball():
# by_cmd.send_angle_scoop(15)
time.sleep(0.5)
by_cmd.send_position_axis_z(30, 170)
time.sleep(3)
time.sleep(2.5)
by_cmd.send_angle_scoop(7)
time.sleep(0.5)
by_cmd.send_distance_y(15, 70)
time.sleep(1)
by_cmd.send_angle_omega(-55,30)
car_stop()
# by_cmd.send_angle_omega(-55,30)
# while True:
# pass
pass
@@ -737,18 +748,18 @@ class put_bball():
calibrate_new(tlabel.BASKET,offset = -21, run = True, run_speed = 6)
by_cmd.send_distance_x(10, 20)
# 向左运动
by_cmd.send_distance_y(-10, 35)
by_cmd.send_angle_storage(15)
time.sleep(1)
# by_cmd.send_distance_y(-10, 35)
# by_cmd.send_angle_storage(10)
# time.sleep(1)
by_cmd.send_angle_storage(55)
by_cmd.send_angle_storage(50)
logger.info("把球放篮筐里")
time.sleep(1)
by_cmd.send_distance_y(10, 55)
# by_cmd.send_distance_y(10, 55)
by_cmd.send_angle_storage(20)
time.sleep(1)
car_stop()
# time.sleep(1)
# car_stop()
pass
def nexec(self):
pass
@@ -761,7 +772,6 @@ class put_bball():
# 物资盘点
class put_hanoi1():
def init(self):
var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
logger.info("物资盘点 1 初始化")
filter.switch_camera(2)
def find(self):
@@ -805,17 +815,19 @@ class put_hanoi1():
logger.info("应该向左转")
# 校准 omega
if error > 0:
by_cmd.send_angle_omega(-20,abs(var.lane_error))
else:
by_cmd.send_angle_omega(20,abs(var.lane_error))
time.sleep(0.5)
car_stop()
time.sleep(0.5)
# # 校准 omega
# if error > 0:
# by_cmd.send_angle_omega(-20,abs(var.lane_error))
# else:
# by_cmd.send_angle_omega(20,abs(var.lane_error))
# time.sleep(0.5)
# car_stop()
# time.sleep(0.5)
# by_cmd.send_distance_x(10, 200)
by_cmd.send_distance_x(10, 180)
time.sleep(0.5)
time.sleep(1)
car_stop()
# 根据方向初始化执行器位置
if utils.direction is tlabel.RMARK:
@@ -825,11 +837,11 @@ class put_hanoi1():
by_cmd.send_angle_storage(0)
else:
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw_arm(217)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_storage(55)
time.sleep(1.5)
time.sleep(1)
by_cmd.send_position_axis_z(30, 0)
by_cmd.send_position_axis_z(30, 10)
if utils.direction_right > utils.direction_left:
utils.direction = tlabel.RMARK
@@ -852,7 +864,7 @@ class put_hanoi1():
def nexec(self):
pass
def after(self):
var.pid_turning.set(0.8, 0, 0)
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
pass
class put_hanoi2():
@@ -866,11 +878,14 @@ class put_hanoi2():
def init(self):
logger.info("物资盘点 2 初始化")
if utils.direction == tlabel.RMARK:
self.offset = 22
self.platform_offset = -29
# 15
self.offset = 19
# self.platform_offset = -25
self.platform_offset = -10
else:
self.offset = 10
self.platform_offset = -39
#self.platform_offset = -30
self.platform_offset = -15
def find(self):
# ret, box = filter.get(self.target_label)
ret, box = filter.get(tlabel.TPLATFORM)
@@ -884,21 +899,29 @@ class put_hanoi2():
logger.info(f"direction:{utils.direction.name}")
var.task_speed = 0
car_stop()
time.sleep(0.5)
# if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_y(10, 50)
# time.sleep(1)
# time.sleep(0.5)
# by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
time.sleep(1)
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
time.sleep(1)
# time.sleep(1)
ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
if not ret:
logger.error("跳过物资盘点 2 exec")
return
time.sleep(0.5)
logger.info("抓大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(81)
time.sleep(1.5)
by_cmd.send_angle_claw(40)
by_cmd.send_angle_claw(78)
time.sleep(1)
by_cmd.send_angle_claw(40)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
@@ -909,17 +932,20 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(81)
time.sleep(1.5)
by_cmd.send_angle_claw(40)
by_cmd.send_angle_claw(78)
time.sleep(1)
by_cmd.send_angle_claw(40)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
ret = explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("跳过物资盘点 2 exec")
return
time.sleep(0.5)
logger.info("放大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_x(1, 130)
@@ -945,18 +971,21 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
time.sleep(1)
ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
if not ret:
logger.error("跳过物资盘点 2 exec")
return
time.sleep(0.5)
logger.info("抓中平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(75)
by_cmd.send_angle_claw(70)
time.sleep(1)
by_cmd.send_angle_claw(35)
time.sleep(1)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
@@ -967,17 +996,20 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(75)
by_cmd.send_angle_claw(70)
time.sleep(1)
by_cmd.send_angle_claw(35)
time.sleep(1)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
pass
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("跳过物资盘点 2 exec")
return
time.sleep(0.5)
logger.info("放中平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 100)
@@ -1003,15 +1035,18 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
time.sleep(1)
ret = explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
if not ret:
logger.error("跳过物资盘点 2 exec")
return
time.sleep(0.5)
logger.info("抓小平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_angle_claw(63)
by_cmd.send_angle_claw(50)
time.sleep(1)
by_cmd.send_angle_claw(70)
by_cmd.send_angle_claw(58)
time.sleep(1)
by_cmd.send_angle_claw(27)
time.sleep(1)
@@ -1023,9 +1058,9 @@ class put_hanoi2():
else:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
by_cmd.send_angle_claw(50)
time.sleep(1)
by_cmd.send_angle_claw(70)
by_cmd.send_angle_claw(58)
time.sleep(1)
by_cmd.send_angle_claw(27)
time.sleep(1)
@@ -1034,8 +1069,11 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
pass
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("跳过物资盘点 2 exec")
return
time.sleep(0.5)
logger.info("放小平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 170)
@@ -1044,8 +1082,8 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
by_cmd.send_angle_claw(90)
time.sleep(1)
by_cmd.send_angle_claw(80)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
time.sleep(1.5)
pass
@@ -1056,8 +1094,8 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
by_cmd.send_angle_claw(90)
time.sleep(1)
by_cmd.send_angle_claw(80)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1.5)
# while True:
@@ -1294,6 +1332,8 @@ class move_area2():
# by_cmd.send_distance_y(-15, 300)
pass
def nexec(self):
logger.warning("正在跳過大模型任務")
time.sleep(2)
pass
def after(self):
var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
@@ -1310,7 +1350,7 @@ class kick_ass():
logger.info("扫黑除暴初始化")
self.pos_gap1 = cfg['kick_ass']['pos_gap1']
self.pos_gap2 = cfg['kick_ass']['pos_gap2']
self.target_person = cfg['kick_ass']['target_person']
self.target_person = cfg_args['kick_ass']['target_person']
# by_cmd.send_angle_claw(15)
by_cmd.send_position_axis_x(1, 160)
@@ -1328,25 +1368,23 @@ class kick_ass():
calibrate_new(tlabel.SIGN, offset = 8, run = True)
time.sleep(0.5)
by_cmd.send_position_axis_z(30, 60)
by_cmd.send_position_axis_x(1, 120)
if self.target_person == 1:
target_distance = self.pos_gap1
else:
target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
by_cmd.send_distance_x(10, target_distance)
time.sleep(4)
logger.info(f"target distance {target_distance}")
time.sleep(1.5 + (self.target_person - 1) * 0.7 )
car_stop()
while by_cmd.send_angle_claw_arm(220) == -1:
pass
while by_cmd.send_angle_claw(15) == -1:
pass
while by_cmd.send_position_axis_x(1, 160) == -1:
pass
time.sleep(2)
by_cmd.send_position_axis_x(1, 0)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw(15)
by_cmd.send_position_axis_x(1, 20)
time.sleep(3)
by_cmd.send_position_axis_x(1, 160)
time.sleep(2)
by_cmd.send_position_axis_x(1, 120)
time.sleep(1)
pass
def nexec(self):
pass