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-05 18:29:22 +08:00
parent f3e6bcc01f
commit 8acecadd2a
19 changed files with 18942 additions and 381 deletions
Download Patch File Download Diff File Expand all files Collapse all files

235
subtask.py
View File

@@ -1,7 +1,6 @@
'''
待办事项：
- 医院第二个方块动作适配
- find 超时
- 第一二個方塊自動識別，hanoi2 識別到物塊即停車，不需手動輸入第一個物塊（修改 fliter 使其能一次請求過濾多個標籤)
'''
from enum import Enum
from loguru import logger
@@ -15,28 +14,56 @@ import zmq
import time
import variable as var
import action as act
context = zmq.Context()
socket = context.socket(zmq.REQ)
socket.connect("tcp://localhost:6667")
logger.info("subtask yolo client init")
cfg = toml.load('cfg_subtask.toml') # 加载任务配置
logger.info("load subtask config")
import re
import threading
import ctypes
cfg = None
by_cmd = None
filter = None
llm_bot = None
# 目标检测 socket 客户端
context = None
socket = None
context1 = None
ocr_socket = None
'''
description: main.py 里执行 引入全局变量
param {*} _by_cmd 控制器对象
return {*}
'''
def import_obj(_by_cmd):
global by_cmd
global filter
global llm_bot
global context
global socket
global context1
global ocr_socket
global cfg
cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
by_cmd = _by_cmd
# 目标检测 socket 客户端
context = zmq.Context()
socket = context.socket(zmq.REQ)
socket.connect("tcp://localhost:6667")
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")
filter = label_filter(socket)
if cfg['move_area']['llm_enable']:
llm_bot = LLM()
@@ -205,12 +232,14 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
# 任务类
class task:
def __init__(self, task_template, find_counts=10, enable=True):
def __init__(self, name, task_template, find_counts=10, enable=True):
self.enable = enable
self.task_t = task_template()
self.counts = 0
self.find_counts = find_counts
self.find_counts = int(find_counts)
self.name = name
def init(self):
if hasattr(self.task_t, 'init') and callable(getattr(self.task_t, 'init', None)):
self.task_t.init()
@@ -218,12 +247,7 @@ class task:
logger.warning("[Task ]# 该任务没有 init 方法")
def find(self):
if hasattr(self.task_t, 'find') and callable(getattr(self.task_t, 'find', None)):
# 检查该任务执行标志
while True:
ret = self.task_t.find()
self.counts += ret
if self.counts >= self.find_counts:
break
return self.task_t.find()
else:
logger.warning("[Task ]# 该任务没有 find 方法")
def exec(self):
@@ -242,6 +266,7 @@ class task:
logger.warning("[Task ]# 该任务没有 nexec 方法")
def after(self):
if hasattr(self.task_t, 'after') and callable(getattr(self.task_t, 'after', None)):
logger.info(f"[Task ]# {self.name} 正在执行 after")
self.task_t.after()
logger.debug(f"[Task ]# Task completed")
else:
@@ -262,7 +287,9 @@ class task_queuem(task):
self.status = task_queuem_status.IDEL
self.busy = True
logger.info(f"[TaskM]# Task num {self.queue.qsize()}")
def exec(self):
# exec 线程
self.exec_thread = None
def exec(self, skip_queue):
# 如果空闲状态则将下一个队列任务取出
if self.status is task_queuem_status.IDEL:
if self.queue.qsize() == 0:
@@ -281,14 +308,42 @@ class task_queuem(task):
logger.info(f"[TaskM]# ---------------------->>>>")
# 阻塞搜索任务标志位
elif self.status is task_queuem_status.SEARCHING:
logger.info(f"[TaskM]# Start searching task target")
self.task_now.find()
self.status = task_queuem_status.EXECUTING
logger.info(f"[{self.task_now.name}]# Start searching task target")
while True:
if not skip_queue.empty():
_ = skip_queue.get()
logger.error(f"{self.task_now.name} 任务在 find 中已经被手动跳过")
self.status = task_queuem_status.IDEL # 空动作不需要阻塞巡线，直接置位
self.task_now.after() # 执行任务后处理
self.queue.task_done() # 弹出已执行的任务
return True
ret = self.task_now.find()
self.task_now.counts += ret
if self.task_now.counts >= self.task_now.find_counts:
self.status = task_queuem_status.EXECUTING
break
# 执行任务函数
elif self.status is task_queuem_status.EXECUTING:
if self.task_now.enable is True:
logger.info(f"[TaskM]# Start execute task function")
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() # 弹出已执行的任务
@@ -306,26 +361,41 @@ class task_queuem(task):
class get_block1():
def init(self):
var.task_speed = 0
logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
act.cmd.camera(0)
act.cmd.z2(20, 60, 0)
filter.switch_camera(1)
if cfg['get_block']['first_block'] == "blue":
self.target_label = tlabel.BBLOCK
self.another_label = tlabel.RBLOCK
else:
self.target_label = tlabel.RBLOCK
self.another_label = tlabel.BBLOCK
cfg['get_block']['first_block'] = "red"
# if cfg['get_block']['first_block'] == "blue":
# self.target_label = tlabel.BBLOCK
# self.another_label = tlabel.RBLOCK
# else:
# self.target_label = tlabel.RBLOCK
# self.another_label = tlabel.BBLOCK
# cfg['get_block']['first_block'] = "red"
self.target_label = [tlabel.RBLOCK, tlabel.BBLOCK]
self.target_counts = [0, 0]
def find(self):
# 目标检测红/蓝方块
ret = filter.find(self.target_label)
if ret > 0:
# ret = filter.find(self.target_label)
# if ret > 0:
# return True
# return False
ret = filter.find_mult(self.target_label)
self.target_counts[0] += ret[0]
self.target_counts[1] += ret[1]
if any(ret):
return True
return False
def exec(self):
car_stop()
calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5)
if self.target_counts[0] > self.target_counts[1]:
var.first_block = tlabel.RBLOCK
var.second_block = tlabel.BBLOCK
else:
var.first_block = tlabel.BBLOCK
var.second_block = tlabel.RBLOCK
calibrate_new(var.first_block, offset = 15, run = True, run_speed = 5)
logger.info("抓取块")
by_cmd.send_position_axis_z(30, 60)
@@ -363,29 +433,28 @@ class get_block1():
class get_block2():
def init(self):
logger.info(f"人员施救 2 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
filter.switch_camera(1)
if cfg['get_block']['first_block'] == "red":
self.target_label = tlabel.BBLOCK
self.another_label = tlabel.RBLOCK
else:
self.target_label = tlabel.RBLOCK
self.another_label = tlabel.BBLOCK
# if cfg['get_block']['first_block'] == "red":
# self.target_label = tlabel.BBLOCK
# self.another_label = tlabel.RBLOCK
# else:
# self.target_label = tlabel.RBLOCK
# self.another_label = tlabel.BBLOCK
def find(self):
# 目标检测红/蓝方块
ret = filter.find(self.target_label)
ret = filter.find(var.second_block)
if ret > 0:
return True
return False
def exec(self):
car_stop()
calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5)
calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5)
logger.info("抓取块")
time.sleep(0.5)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw(53)
by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_z(30, 60)
time.sleep(1)
by_cmd.send_position_axis_x(1, 20)
@@ -407,7 +476,7 @@ class get_block2():
def after(self):
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
# 任务检查间隔
time.sleep(9)
time.sleep(7)
# 紧急转移
@@ -423,7 +492,7 @@ class put_block():
ret, box = filter.get(tlabel.HOSPITAL)
if ret > 0:
width = box[0][2] - box[0][0]
if width > 145:
if width > 130:
return True
return False
def exec(self):
@@ -441,19 +510,27 @@ class put_block():
# 放置第二個塊
by_cmd.send_angle_storage(20)
by_cmd.send_distance_x(-10, 110)
time.sleep(1)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_position_axis_z(30, 150)
time.sleep(2)
by_cmd.send_angle_claw_arm(126)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_z(30, 120)
time.sleep(1.5)
by_cmd.send_angle_claw_arm(180)
by_cmd.send_angle_claw(85)
# by_cmd.send_angle_storage(0)
time.sleep(1)
by_cmd.send_position_axis_z(15,100)
by_cmd.send_position_axis_z(30,70)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
by_cmd.send_angle_claw(25)
by_cmd.send_distance_x(-10, 110)
time.sleep(1)
by_cmd.send_position_axis_z(30, 110)
time.sleep(1)
by_cmd.send_angle_claw_arm(220)
time.sleep(1)
by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(1.5)
by_cmd.send_angle_storage(0)
by_cmd.send_angle_claw(45)
pass
def nexec(self):
pass
@@ -520,11 +597,11 @@ class get_bball():
time.sleep(1)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_position_axis_x(1, 30)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, -40)
time.sleep(0.5)
by_cmd.send_angle_claw_arm(70)
by_cmd.send_angle_claw_arm(80)
time.sleep(0.5)
by_cmd.send_angle_claw(54)
time.sleep(1)
@@ -573,11 +650,10 @@ class up_tower():
by_cmd.send_distance_y(-10, 50)
time.sleep(3)
by_cmd.send_angle_zhuan(10)
time.sleep(10)
time.sleep(11)
by_cmd.send_distance_y(10, 60)
time.sleep(1)
by_cmd.send_angle_zhuan(0)
time.sleep(0.5)
# while True:
# pass
def nexec(self):
@@ -623,8 +699,9 @@ class get_rball():
# by_cmd.send_angle_scoop(15)
time.sleep(0.5)
by_cmd.send_position_axis_z(30, 170)
time.sleep(3.5)
time.sleep(3)
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)
@@ -661,16 +738,17 @@ class put_bball():
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_angle_storage(55)
logger.info("把球放篮筐里")
time.sleep(2)
by_cmd.send_distance_y(10, 55)
time.sleep(1)
by_cmd.send_distance_y(10, 55)
by_cmd.send_angle_storage(20)
time.sleep(1)
car_stop()
pass
def nexec(self):
pass
@@ -756,16 +834,16 @@ class put_hanoi1():
if utils.direction_right > utils.direction_left:
utils.direction = tlabel.RMARK
# by_cmd.send_angle_omega(-25,430)
# by_cmd.send_angle_omega(-45,238)
by_cmd.send_angle_omega(-55,194)
by_cmd.send_angle_omega(-45,238)
# by_cmd.send_angle_omega(-55,194)
time.sleep(2)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
else:
utils.direction = tlabel.LMARK
# by_cmd.send_angle_omega(25,430)
# by_cmd.send_angle_omega(45,238)
by_cmd.send_angle_omega(55,194)
by_cmd.send_angle_omega(45,238)
# by_cmd.send_angle_omega(55,194)
time.sleep(2)
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
@@ -789,14 +867,17 @@ class put_hanoi2():
logger.info("物资盘点 2 初始化")
if utils.direction == tlabel.RMARK:
self.offset = 22
self.platform_offset = -29
else:
self.offset = 10
self.platform_offset = -39
def find(self):
ret, box = filter.get(self.target_label)
# ret, box = filter.get(self.target_label)
ret, box = filter.get(tlabel.TPLATFORM)
if ret:
var.task_speed = 8.5
error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
if abs(error) < 40:
error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset
if error > 0:
return True
return False
def exec(self):
@@ -1110,6 +1191,7 @@ class move_area2():
time.sleep(delay_time)
by_cmd.send_beep(0)
def sub_move(self, x, y):
# FIXME 如果同時有 xy，是否會造成 delay 不足
self.delta_x += x
self.delta_y += y
@@ -1125,7 +1207,8 @@ class move_area2():
by_cmd.send_distance_y(-15, delay_time)
else:
by_cmd.send_distance_y(15, delay_time)
time.sleep(delay_time / 400 * 1.5)
time.sleep(delay_time / 500)
car_stop()
pass
def sub_turn(self, angle):
self.delta_omage += angle
@@ -1161,8 +1244,8 @@ class move_area2():
return
# 进入停车区域
# by_cmd.send_speed_y(15)
by_cmd.send_distance_y(15, 300)
time.sleep(2)
by_cmd.send_distance_y(25, 180)
time.sleep(1)
# time.sleep(1.25)
car_stop()
logger.info(resp_commands)
@@ -1197,11 +1280,11 @@ class move_area2():
time.sleep(delay_time / 300 * 1.5)
if self.delta_y > 0:
# 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了
delay_time = 300 - (self.delta_y * 500)
delay_time = 180 - (self.delta_y * 500)
else:
delay_time = 300 + (abs(self.delta_y) * 500)
delay_time = 180 + (abs(self.delta_y) * 500)
# 离开停车区域
by_cmd.send_distance_y(-15, delay_time)
by_cmd.send_distance_y(-25, delay_time)
time.sleep(delay_time * 5e-3)