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

feat: 增加部分任务

Browse Source
This commit is contained in:
bmy
2024-06-02 17:49:52 +08:00
parent 49c0499f24
commit 4c1cf9ceb0
6 changed files with 458 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

426
subtask.py
View File

@@ -2,6 +2,7 @@ from enum import Enum
from loguru import logger
from utils import label_filter
from utils import tlabel
import utils
import toml
import zmq
import time
@@ -9,18 +10,67 @@ import time
context = zmq.Context()
socket = context.socket(zmq.REQ)
socket.connect("tcp://localhost:6667")
logger.info("socket init")
logger.info("subtask yolo client init")
cfg = toml.load('cfg_subtask.toml') # 加载任务配置
logger.info("load subtask config")
by_cmd = None
filter = None
def import_obj(_by_cmd):
global by_cmd
global filter
by_cmd = _by_cmd
filter = label_filter(socket)
def car_stop():
pass
# run_on 是否继续向前行进 等待 error<5 后停止 如果不需要前进（在校准前车已经停下）
# 则不需要等待 error < 5 可以直接 aim_near
def calibrate(label, offset, run_on = True, cali_speed = 10):
logger.info("开始校准")
# go on
if run_on:
for _ in range(3):
by_cmd.send_speed_x(7)
by_cmd.send_speed_omega(0)
time.sleep(0.1)
while True:
ret, error = filter.aim_near(label)
while not ret:
ret, error = filter.aim_near(label)
error += offset
if run_on:
if abs(error) < 5:
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
break
else:
break
ret, error = filter.aim_near(label)
while not ret:
ret, error = filter.aim_near(label)
error += offset
time.sleep(1)
logger.error(error)
if abs(error) > 5:
logger.info("校准中")
if error > 0:
by_cmd.send_distance_x(-cali_speed, int(error*4))
else:
by_cmd.send_distance_x(cali_speed, int(-error*4))
logger.error(error)
time.sleep(1)
# stop
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
pass
# 任务类
class task:
@@ -36,8 +86,11 @@ class task:
# 检查该任执行标志
while True:
# if self.func_find():
if self.task_t.find():
self.counts += 1
# 特殊地方可以直接跳出 find
ret = self.task_t.find()
self.counts += ret
# if self.task_t.find():
# self.counts += 1
if self.counts >= self.find_counts:
break
# while self.func_find() is False:
@@ -47,11 +100,11 @@ class task:
# 根据标志位确定是否执行该任务
if self.enable is True:
logger.debug(f"[Task ]# Executing task")
# self.func_exec()
self.task_t.exec()
logger.debug(f"[Task ]# Task completed")
else:
logger.warning(f"[Task ]# Skip task")
self.task_t.nexec()
# 任务队列状态类
class task_queuem_status(Enum):
@@ -115,41 +168,40 @@ class get_block():
else:
return False
def exec(self):
for _ in range(3):
by_cmd.send_speed_x(7)
by_cmd.send_speed_omega(0)
time.sleep(0.1)
logger.info("abcd")
cfg = toml.load('cfg_subtask.toml') # 加载任务配置
while True:
# logger.info("等待进入准确区域")
ret, error = filter.aim_near(tlabel.RBLOCK)
while not ret:
ret, error = filter.aim_near(tlabel.RBLOCK)
# logger.info(error)
if abs(error) < 5:
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
break
ret, error = filter.aim_near(tlabel.RBLOCK)
while not ret:
ret, error = filter.aim_near(tlabel.RBLOCK)
time.sleep(1)
logger.error(error)
if abs(error) > 5:
logger.info("校准中")
if error > 0:
by_cmd.send_distance_x(-10, int(error*3))
else:
by_cmd.send_distance_x(10, int(-error*3))
logger.error(error)
time.sleep(1)
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
calibrate(tlabel.RBLOCK,15)
# for _ in range(3):
# by_cmd.send_speed_x(7)
# by_cmd.send_speed_omega(0)
# time.sleep(0.1)
# while True:
# # logger.info("等待进入准确区域")
# ret, error = filter.aim_near(tlabel.RBLOCK)
# while not ret:
# ret, error = filter.aim_near(tlabel.RBLOCK)
# # logger.info(error)
# if abs(error) < 5:
# for _ in range(3):
# by_cmd.send_speed_x(0)
# time.sleep(0.2)
# by_cmd.send_speed_omega(0)
# break
# ret, error = filter.aim_near(tlabel.RBLOCK)
# while not ret:
# ret, error = filter.aim_near(tlabel.RBLOCK)
# time.sleep(1)
# logger.error(error)
# if abs(error) > 5:
# logger.info("校准中")
# if error > 0:
# by_cmd.send_distance_x(-10, int(error*3))
# else:
# by_cmd.send_distance_x(10, int(-error*3))
# logger.error(error)
# time.sleep(1)
# for _ in range(3):
# by_cmd.send_speed_x(0)
# time.sleep(0.2)
# by_cmd.send_speed_omega(0)
time.sleep(2)
by_cmd.send_position_axis_z(10, 150)
@@ -179,6 +231,7 @@ class get_block():
time.sleep(3)
by_cmd.send_position_axis_x(4, 0)
def nexec(self):
# TODO 完成不执行任务的空动作
pass
@@ -194,13 +247,39 @@ class put_block():
else:
return False
def exec(self):
cfg = toml.load('cfg_subtask.toml') # 加载任务配置
# TODO 测试对准效果
logger.info("找到医院")
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
time.sleep(0.1)
while True:
# logger.info("等待进入准确区域")
ret, error = filter.aim_near(tlabel.HOSPITAL)
while not ret:
ret, error = filter.aim_near(tlabel.HOSPITAL)
# logger.info(error)
if abs(error) < 5:
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
break
ret, error = filter.aim_near(tlabel.HOSPITAL)
while not ret:
ret, error = filter.aim_near(tlabel.HOSPITAL)
time.sleep(1)
logger.error(error)
if abs(error) > 5:
logger.info("校准中")
if error > 0:
by_cmd.send_distance_x(-10, int(error*3))
else:
by_cmd.send_distance_x(10, int(-error*3))
logger.error(error)
time.sleep(1)
by_cmd.send_position_axis_z(10, 150)
time.sleep(3)
# TODO 切换爪子方向
@@ -208,6 +287,8 @@ class put_block():
time.sleep(2)
by_cmd.send_position_axis_z(10, 170)
pass
def nexec(self):
pass
# 整装上阵
class get_bball():
@@ -215,7 +296,7 @@ class get_bball():
by_cmd.send_position_axis_x(2, 140)
logger.info("整装上阵初始化")
time.sleep(0.5)
if (by_cmd.send_angle_camera(90) == -1):
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
def find(self):
# 目标检测黄球
@@ -255,7 +336,7 @@ class get_bball():
by_cmd.send_distance_x(10, int(-error*3))
logger.error(error)
time.sleep(1)
if (by_cmd.send_angle_camera(0) == -1):
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
by_cmd.send_position_axis_z(20, 160)
time.sleep(2)
@@ -277,6 +358,8 @@ class get_bball():
time.sleep(2)
by_cmd.send_angle_claw(90)
pass
def nexec(self):
pass
# 通信抢修
class up_tower():
@@ -322,12 +405,15 @@ class up_tower():
by_cmd.send_distance_x(10, int(-error*3))
logger.error(error)
time.sleep(1)
def nexec(self):
pass
# 高空排险
class get_rball():
def init(self):
logger.info("高空排险初始化")
if (by_cmd.send_angle_camera(0) == -1):
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
def find(self):
# 目标检测红球
@@ -338,18 +424,17 @@ class get_rball():
return False
def exec(self):
logger.info("找到红球")
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
time.sleep(1)
# TODO 高空排险 offset 需要调整
calibrate(tlabel.RBALL,15, True, 6)
pass
def nexec(self):
pass
# 派发物资
class put_bball():
def init(self):
logger.info("派发物资初始化")
if (by_cmd.send_angle_camera(90) == -1):
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
def find(self):
# 目标检测通信塔
@@ -365,8 +450,10 @@ class put_bball():
time.sleep(0.2)
by_cmd.send_speed_omega(0)
time.sleep(1)
pass
def nexec(self):
pass
direction = tlabel.RMARK
direction_left = 0
direction_right = 0
@@ -386,14 +473,25 @@ class put_hanoi1():
ret2, list2 = filter.get(tlabel.LMARK)
if ret1:
logger.info("向右拐")
direction_right += 1
return True
list1 = list1[0]
area = (list1[2] - list1[0]) * (list1[3] - list1[1])
logger.info(area)
if area > 2500:
direction_right += 1
return True
return False
elif ret2:
logger.info("向左拐")
direction_left += 1
return True
list2 = list2[0]
area = (list2[2] - list2[0]) * (list2[3] - list2[1])
logger.info(area)
if area > 2500:
direction_left += 1
return True
return False
return False
def exec(self):
global direction
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
@@ -401,70 +499,204 @@ class put_hanoi1():
time.sleep(0.2)
# if direction == tlabel.RMARK:
if direction_right > direction_left:
by_cmd.send_angle_omega(-20,500)
direction = tlabel.RMARK
# by_cmd.send_distance_x(6, 200)
by_cmd.send_angle_omega(-20,380)
time.sleep(2)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
else:
direction = tlabel.LMARK
# by_cmd.send_distance_x(6, 200)
by_cmd.send_angle_omega(20,500)
time.sleep(0.2)
if (by_cmd.send_angle_camera(180) == -1):
time.sleep(2)
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
time.sleep(2)
time.sleep(0.5)
socket.send_string("1")
socket.recv()
pass
def nexec(self):
pass
def sub_put_hanoi2(label,distance_type,run_on,back_flag):
# logger.info("找到大红色柱体")
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
time.sleep(2)
# 对准大红色柱体
calibrate(label,15, run_on, 6)
# 抓取大红色柱体
logger.info("抓取柱体")
# 根据 direction 确定移动方向
by_cmd.send_distance_x(-8, distance_type)
# 移动 self.distance_lp 距离
# 放置物块
logger.info("开始放置柱体")
time.sleep(1)
if back_flag:
pass
# 回移相同距离
# if direction == tlabel.RMARK:
# by_cmd.send_distance_x(8, distance_type)
# else:
# by_cmd.send_distance_x(-8, distance_type)
class put_hanoi2():
def __init__(self):
self.pos_lp = cfg['put_hanoi2']['pos_lp']
self.pos_mp = cfg['put_hanoi2']['pos_mp']
self.pos_sp = 6 - self.pos_lp - self.pos_mp
self.distance_lp = self.pos_lp * cfg['put_hanoi2']['pos_gap']
self.distance_mp = self.pos_mp * cfg['put_hanoi2']['pos_gap']
self.distance_sp = self.pos_sp * cfg['put_hanoi2']['pos_gap']
logger.info(f"setting hanoi pos_lp[{self.pos_lp}] pos_mp[{self.pos_mp}] pos_sp[{self.pos_sp}]")
def init(self):
logger.info("物资盘点 2 初始化")
def find(self):
# 目标检测左右转向标识
ret1, list1 = filter.get(tlabel.LPILLER)
time.sleep(0.001)
return True
# # 目标检测左右转向标识
# ret1, list1 = filter.get(tlabel.LPILLER)
# ret, _ = filter.get_mult([tlabel.LPILLER])
# if ret:
# logger.info("看到了三个 直接停车")
# return 100
# if ret1 > 0:
# list1 = list1[0]
# area = (list1[2] - list1[0]) * (list1[3] - list1[1])
# logger.info(area)
# if area > 3300:
# return True
# return False
# else:
# return False
def exec(self):
# TODO 延时需要根据移动的 distance 判断
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
# 往回走一段 然后向前行进校准
by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
time.sleep(1)
logger.info(f"方向{direction}")
if utils.lane_error > 0:
by_cmd.send_angle_omega(-20,abs(utils.lane_error))
else:
by_cmd.send_angle_omega(20,abs(utils.lane_error))
time.sleep(1)
sub_put_hanoi2(tlabel.LPILLER, self.distance_lp, True, True)
time.sleep(5)
# 对准中蓝色柱体
sub_put_hanoi2(tlabel.MPILLER, self.distance_mp, True, True)
time.sleep(5)
# 根据 direction 确定移动方向
# 移动 self.distance_mp 距离
# 放置物块
# 回移相同距离
sub_put_hanoi2(tlabel.SPILLER, self.distance_sp, True, True)
time.sleep(5)
# 对准小红色柱体
# 根据 direction 确定移动方向
# 移动 self.distance_sp 距离
# 放置物块
pass
def nexec(self):
pass
if ret1 > 0:
# 应急避险 第一阶段 找目标牌
class move_area1():
def init(self):
logger.info("应急避险第一阶段初始化")
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
def find(self):
# 目标检测标志牌
ret, error = filter.aim_near(tlabel.SIGN)
if ret > 0 and abs(error) < 8:
return True
else:
return False
def exec(self):
logger.info("找到最大块")
logger.info("找到标示牌")
# for _ in range(3):
# by_cmd.send_speed_x(0)
# time.sleep(0.2)
# by_cmd.send_speed_omega(0)
pass
def nexec(self):
pass
# 应急避险 第二阶段 找停车区域
class move_area2():
def init(self):
logger.info("应急避险第二阶段初始化")
def find(self):
time.sleep(0.001)
return True
def exec(self):
logger.info("开始寻找停车区域")
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
calibrate(tlabel.SHELTER, 15, False, 6)
time.sleep(1)
# 进入停车区域
by_cmd.send_distance_y(10, 450)
time.sleep(3)
# 离开停车区域
by_cmd.send_distance_y(-10, 450)
time.sleep(3)
while True:
pass
pass
def nexec(self):
pass
# 扫黑除暴
class kick_ass():
def init(self):
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']
def find(self):
ret, error = filter.aim_near(tlabel.SIGN)
if ret > 0 and abs(error) < 8:
return True
else:
return False
def exec(self):
logger.info("找到标示牌")
for _ in range(3):
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
time.sleep(1)
pass
# 应急避险
class move_area():
def init(self):
logger.info("应急避险初始化")
if (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
def find(self):
# 目标检测标志牌
# TODO 如何确保在都检测标志牌的情况下，和下一个任务进行区分
ret1, list1 = filter.get(tlabel.SIGN)
if ret1 > 0:
return True
if utils.lane_error > 0:
by_cmd.send_angle_omega(-20,abs(utils.lane_error)*1.5)
else:
return False
def exec(self):
logger.info("找到标示牌")
pass
by_cmd.send_angle_omega(20,abs(utils.lane_error)*1.5)
time.sleep(1)
calibrate(tlabel.SIGN, 8, False, 6)
time.sleep(0.5)
# 扫黑除暴
class kick_ass():
def init(self):
logger.info("扫黑除暴初始化")
def find(self):
# 目标检测标志牌
# TODO 如何确保在都检测标志牌的情况下，和上一个任务进行区分
ret1, list1 = filter.get(tlabel.SIGN)
if ret1 > 0:
return True
if self.target_person == 1:
by_cmd.send_distance_x(10, self.pos_gap1)
else:
return False
def exec(self):
logger.info("找到标示牌")
by_cmd.send_distance_x(10, self.pos_gap1 + (self.target_person - 1) * self.pos_gap2)
time.sleep(2)
time.sleep(5)
pass
def nexec(self):
pass