from enum import Enum from loguru import logger from utils import label_filter from utils import tlabel from utils import LLM import utils import toml import zmq import time 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") by_cmd = None filter = None llm_bot = None ''' description: main.py 里执行 引入全局变量 param {*} _by_cmd 控制器对象 return {*} ''' def import_obj(_by_cmd): global by_cmd global filter global llm_bot by_cmd = _by_cmd filter = label_filter(socket) if cfg['move_area']['llm_enable']: llm_bot = LLM() def car_stop(): for _ in range(3): by_cmd.send_speed_x(0) time.sleep(0.2) by_cmd.send_speed_omega(0) def calibrate_right_new(label, offset, run = True, run_speed = 3.5): ret, error = filter.aim_right(label) while not ret: ret, error = filter.aim_right(label) error += offset if abs(error) > 10 and run: if error > 0: by_cmd.send_speed_x(-run_speed) else: by_cmd.send_speed_x(run_speed) # 停的位置已经很接近目标，可以直接使用 distance 校准 else: error = error * 3 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: by_cmd.send_distance_x(10, int(-error)) return while True: ret, error = filter.aim_right(label) while not ret: ret, error = filter.aim_right(label) error += offset if ret: if abs(error) <= 5: car_stop() logger.info("可以停车了") ret, error = filter.aim_right(label) while not ret: ret, error = filter.aim_right(label) error += offset if abs(error) > 8: error = error * 3 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: by_cmd.send_distance_x(10, int(-error)) break ''' description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车 param {*} label param {*} offset param {*} run param {*} run_speed return {*} ''' def calibrate_new(label, offset, run = True, run_speed = 3.5): ret, box = filter.get(label) while not ret: ret, box = filter.get(label) error = (box[0][2] + box[0][0] - 320) / 2 + offset if abs(error) > 10 and run: if error > 0: by_cmd.send_speed_x(-run_speed) else: by_cmd.send_speed_x(run_speed) # 停的位置已经很接近目标，可以直接使用 distance 校准 else: error = error * 3 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: by_cmd.send_distance_x(10, int(-error)) return while True: ret, box = filter.get(label) while not ret: ret, box = filter.get(label) error = (box[0][2] + box[0][0] - 320) / 2 + offset if ret: if abs(error) <= 5: car_stop() logger.info("可以停车了") ret, box = filter.get(label) while not ret: ret, box = filter.get(label) error = (box[0][2] + box[0][0] - 320) / 2 + offset if abs(error) > 8: error = error * 3 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: by_cmd.send_distance_x(10, int(-error)) break ''' description: 与 calibrate 一样 只不过寻找最右侧的 整装上阵时使用 param {*} label param {*} offset param {*} run_on param {*} cali_speed return {*} ''' def calibrate_right(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_right(label) while not ret: ret, error = filter.aim_right(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_right(label) while not ret: ret, error = filter.aim_right(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)) 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 ''' description: 校准函数 传入对应的标签将车校准到最接近中心的标签的位置 param {*} label param {*} offset 摄像头 error offset param {*} run_on 校准前是否继续向前行进以寻找 label param {*} cali_speed send_distance_x 校准时移动的速度 return {*} ''' 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 # 代码运行到这里代表摄像头内一定有物体 label，此时再获取一次 error 用于校准 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 abs(error) < 5: error = error * 5 logger.error("校准分段 error * 5") elif abs(error) < 20: error = error * 4 logger.error("校准分段 error * 4") else: error = error * 3 logger.error("校准分段 error * 3") if error > 0: by_cmd.send_distance_x(-cali_speed, int(error)) else: by_cmd.send_distance_x(cali_speed, int(-error)) time.sleep(2) # stop for _ in range(3): by_cmd.send_speed_x(0) time.sleep(0.2) by_cmd.send_speed_omega(0) pass # 任务类 class task: def __init__(self, task_template, find_counts=10, enable=True): self.enable = enable self.task_t = task_template() self.counts = 0 self.find_counts = find_counts def init(self): self.task_t.init() def find(self): # 检查该任执行标志 while True: # if self.func_find(): # 特殊地方可以直接跳出 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: # pass def exec(self): # 根据标志位确定是否执行该任务 if self.enable is True: logger.debug(f"[Task ]# Executing task") 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): IDEL = 0 SEARCHING = 1 EXECUTING = 2 # 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作 class task_queuem(task): # task_now = task(None, False) def __init__(self, queue): super(task_queuem, self) self.queue = queue self.status = task_queuem_status.IDEL self.busy = True logger.info(f"[TaskM]# Task num {self.queue.qsize()}") def exec(self): # 如果空闲状态则将下一个队列任务取出 if self.status is task_queuem_status.IDEL: if self.queue.qsize() == 0: self.busy = False logger.info(f"[TaskM]# Task queue empty, exit") return False self.task_now = self.queue.get() # 如果当前任务没有使能，则直接转入执行状态，由任务执行函数打印未执行信息 if self.task_now.enable is True: self.status = task_queuem_status.SEARCHING # 如果使能该任务则执行该任务的初始化动作 self.task_now.init() else: self.status = task_queuem_status.EXECUTING 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 # 执行任务函数 elif self.status is task_queuem_status.EXECUTING: logger.info(f"[TaskM]# Start execute task function") self.task_now.exec() # 执行当前任务函数 self.queue.task_done() # 弹出已执行的任务 self.status = task_queuem_status.IDEL # logger.info(f"[TaskM]# <<<<----------------------") return True # 人员施救 class get_block(): def init(self): logger.info("人员施救初始化") filter.switch_camera(1) if cfg['get_block']['first_block'] == "blue": self.target_label = tlabel.BBLOCK self.another_label = tlabel.RBLOCK cfg['get_block']['first_block'] = '' else: self.target_label = tlabel.RBLOCK self.another_label = tlabel.BBLOCK def find(self): # 目标检测红/蓝方块 ret = filter.find(self.target_label) if ret > 0: return True else: return False def exec(self): car_stop() calibrate_new(self.target_label, offset = 12) logger.info("抓取块") time.sleep(3) # 测试新方案 # 旧方案 # for _ in range(3): # by_cmd.send_speed_x(0) # time.sleep(0.2) # by_cmd.send_speed_omega(0) # calibrate(self.target_label, 12, False, 7) # logger.info("抓取块") # time.sleep(2) # by_cmd.send_distance_axis_z(20, 20*7) # time.sleep(3) # # 向内退 # by_cmd.send_distance_axis_x(3, 20*2) # time.sleep(2) # # 旋转机械臂到最右侧 # by_cmd.send_angle_claw_arm(36 + 184) # time.sleep(1.5) # # 开爪子 # by_cmd.send_angle_claw(27.0 + 9 * 3) # # 下降 # by_cmd.send_distance_axis_z(20, -20*4) # time.sleep(1.5) # # 关爪子 # by_cmd.send_angle_claw(27.0 - 2) # time.sleep(1) # by_cmd.send_distance_axis_z(15, 20*2) # time.sleep(2) # while True: # pass # by_cmd.send_distance_x(10, 500) # counts = 0 # while True: # counts += filter.find(self.another_label) # if counts >= 35: # break # for _ in range(3): # by_cmd.send_speed_x(0) # time.sleep(0.2) # by_cmd.send_speed_omega(0) # calibrate(self.another_label, 12, False, 7) # logger.info("抓取块") # time.sleep(2) pass # 调试 临时注释掉 # calibrate(tlabel.RBLOCK,15) # time.sleep(2) # by_cmd.send_position_axis_z(10, 150) # time.sleep(5) # by_cmd.send_angle_claw_arm(127) # time.sleep(1) # by_cmd.send_position_axis_x(4, 140) # time.sleep(4) # by_cmd.send_angle_claw_arm(220) # by_cmd.send_angle_claw(90) # time.sleep(1) # by_cmd.send_distance_axis_z(10, -70) # time.sleep(3) # by_cmd.send_angle_claw(27) # by_cmd.send_distance_axis_z(10, 10) # time.sleep(2) # by_cmd.send_distance_axis_x(4, -100) # time.sleep(1) # by_cmd.send_distance_axis_z(10, -40) # time.sleep(3) # by_cmd.send_angle_claw(35) # time.sleep(1) # by_cmd.send_position_axis_z(10, 150) # time.sleep(3) # by_cmd.send_position_axis_x(2, 140) # # 抓取第二个块后 收爪 # time.sleep(3) # by_cmd.send_position_axis_x(4, 0) def nexec(self): # TODO 完成不执行任务的空动作 pass # 紧急转移 class put_block(): def init(self): logger.info("紧急转移初始化") socket.send_string("1") socket.recv() def find(self): # 目标检测医院 ret1, list1 = filter.get(tlabel.HOSPITAL) if ret1 > 0: width = list1[0][2] - list1[0][0] if width > 120: return True return False else: return False def exec(self): logger.info("找到医院") car_stop() calibrate_new(tlabel.HOSPITAL, offset = 7, run = False) time.sleep(3) # calibrate(tlabel.HOSPITAL, 7, False, 6) # by_cmd.send_position_axis_z(10, 150) # time.sleep(3) # # TODO 切换爪子方向 # by_cmd.send_position_axis_x(2, 140) # time.sleep(2) # by_cmd.send_position_axis_z(10, 170) pass def nexec(self): pass # 整装上阵 class get_bball(): def init(self): # by_cmd.send_position_axis_x(2, 140) # 调试 临时换源 socket.send_string("1") socket.recv() logger.info("整装上阵初始化") time.sleep(0.5) while (by_cmd.send_angle_camera(90) == -1): by_cmd.send_angle_camera(90) def find(self): # 目标检测蓝球 ret = filter.find(tlabel.BBALL) if ret: return True else: return False def exec(self): logger.info("找到蓝色球") car_stop() time.sleep(0.5) for _ in range(3): calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 3.5) logger.info("抓蓝色球") time.sleep(5) # 原方案 # time.sleep(1) # calibrate_right(tlabel.BBALL, 27, False, 5) # logger.info("抓到了蓝色球") # time.sleep(5) # for i in range(2): # while True: # by_cmd.send_distance_x(6, 60) # ret, error = filter.aim_near(tlabel.BBALL) # if ret and abs(error) < 30: # break # else: # by_cmd.send_distance_x(6, 60) # for _ in range(3): # by_cmd.send_speed_x(0) # time.sleep(0.1) # by_cmd.send_speed_omega(0) # calibrate_right(tlabel.BBALL, 27, False, 5) # time.sleep(5) # by_cmd.send_position_axis_z(20, 160) # time.sleep(2) # by_cmd.send_position_axis_x(2, 70) # time.sleep(2) # by_cmd.send_angle_claw(90) # time.sleep(0.2) # by_cmd.send_position_axis_x(2, 0) # time.sleep(2) # by_cmd.send_angle_claw(27) # time.sleep(1) # by_cmd.send_position_axis_z(20, 180) # time.sleep(1) # by_cmd.send_position_axis_x(4, 45) # time.sleep(1) # by_cmd.send_position_axis_z(20, 140) # time.sleep(3) # by_cmd.send_position_axis_x(2, 140) # time.sleep(2) # by_cmd.send_angle_claw(90) pass def nexec(self): pass # 通信抢修 class up_tower(): def init(self): logger.info("通信抢修初始化") while (by_cmd.send_angle_camera(90) == -1): by_cmd.send_angle_camera(90) def find(self): # 目标检测通信塔 ret, error = filter.aim_near(tlabel.TOWER) if ret: return True else: return False def exec(self): logger.info("找到塔") car_stop() calibrate_new(tlabel.TOWER, offset = 27, run = True) # calibrate(tlabel.TOWER, 27, False, 6) time.sleep(3) while True: pass def nexec(self): pass # 高空排险 class get_rball(): def init(self): logger.info("高空排险初始化") while (by_cmd.send_angle_camera(0) == -1): by_cmd.send_angle_camera(0) def find(self): # 目标检测红球 ret = filter.find(tlabel.RBALL) if ret > 0: return True else: return False def exec(self): logger.info("找到红球") car_stop() calibrate_new(tlabel.RBALL,offset = -4, run = True) time.sleep(1) by_cmd.send_distance_y(-10, 65) time.sleep(1) by_cmd.send_distance_x(-10, 80) time.sleep(1) logger.info("抓红球") while True: pass pass def nexec(self): pass # 派发物资 class put_bball(): def init(self): logger.info("派发物资初始化") socket.send_string("1") socket.recv() while (by_cmd.send_angle_camera(90) == -1): by_cmd.send_angle_camera(90) def find(self): ret = filter.find(tlabel.BASKET) if ret > 0: return True else: return False def exec(self): logger.info("找到篮筐") car_stop() calibrate_new(tlabel.BASKET,offset = 12, run = True) logger.info("把球放篮筐里") time.sleep(2) pass def nexec(self): pass direction = tlabel.RMARK direction_left = 0 direction_right = 0 # 物资盘点 class put_hanoi1(): def init(self): logger.info("物资盘点 1 初始化") socket.send_string("2") socket.recv() def find(self): global direction global direction_left global direction_right # 目标检测左右转向标识 # TODO 框的大小判断距离 # ret1, list1 = filter.get(tlabel.RMARK) # ret2, list2 = filter.get(tlabel.LMARK) ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK]) if label == tlabel.RMARK: if abs(error) < 55: logger.info("向右拐") direction_right += 1 return True return False elif label == tlabel.LMARK: if abs(error) < 50: logger.info("向左拐") direction_left += 1 return True return False else: return False def exec(self): global direction for _ in range(3): by_cmd.send_speed_x(0) time.sleep(0.2) by_cmd.send_speed_omega(0) time.sleep(0.2) # 校准牌子 if direction_right > direction_left: ret, error = filter.aim_near(tlabel.RMARK) while not ret: ret, error = filter.aim_near(tlabel.RMARK) direction = tlabel.RMARK else: ret, error = filter.aim_near(tlabel.LMARK) while not ret: ret, error = filter.aim_near(tlabel.LMARK) direction = tlabel.LMARK # 校准 omega if error > 0: by_cmd.send_angle_omega(-20,abs(utils.lane_error)*13) else: by_cmd.send_angle_omega(20,abs(utils.lane_error)*13) time.sleep(0.5) by_cmd.send_speed_omega(0) time.sleep(0.5) by_cmd.send_distance_x(10, 250) time.sleep(1) if direction_right > direction_left: direction = tlabel.RMARK 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_angle_omega(20,500) time.sleep(2) while (by_cmd.send_angle_camera(180) == -1): by_cmd.send_angle_camera(180) 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,7, run_on, 10) # 抓取大红色柱体 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): time.sleep(0.001) return True 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 # 应急避险 第一阶段 找目标牌 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("找到标示牌") # 停车 ocr 识别文字 调用大模型 for _ in range(3): by_cmd.send_speed_x(0) time.sleep(0.2) by_cmd.send_speed_omega(0) time.sleep(2) 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) # TODO 调用大模型 然后执行动作 by_cmd.send_light(1) time.sleep(2) by_cmd.send_light(0) # 离开停车区域 by_cmd.send_distance_y(-10, 450) time.sleep(3) 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) < 16: 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) if utils.lane_error > 0: by_cmd.send_angle_omega(-20,abs(utils.lane_error)*1.5) else: 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) if self.target_person == 1: by_cmd.send_distance_x(10, self.pos_gap1) else: 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