diff --git a/action.py b/action.py index 9af23e6..0da0192 100644 --- a/action.py +++ b/action.py @@ -13,15 +13,19 @@ move = None axis = None cmd = None +action_run_flag = None + cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml') -def import_obj(_bycmd): +def import_obj(_run_flag,_bycmd): global bycmd global move global axis global cmd + global action_run_flag bycmd = _bycmd + action_run_flag = _run_flag move = move_cls() axis = axis_cls() cmd = cmd_cls() @@ -118,7 +122,7 @@ class cmd_cls(): else: time_via = _time_via logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}") - while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1: + while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1 and action_run_flag.isSet(): pass time.sleep(time_via) def z2(self, _speed, _pos, _time_via = -1): @@ -131,7 +135,7 @@ class cmd_cls(): else: time_via = _time_via logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}") - while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1: + while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1 and action_run_flag.isSet(): pass time.sleep(time_via) def x(self, _speed, _dis, _time_via = -1): @@ -143,7 +147,7 @@ class cmd_cls(): else: time_via = _time_via logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}") - while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1: + while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1 and action_run_flag.isSet(): pass time.sleep(time_via) def x2(self, _speed, _pos, _time_via = -1): @@ -156,30 +160,30 @@ class cmd_cls(): else: time_via = _time_via logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}") - while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1: + while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1 and action_run_flag.isSet(): pass time.sleep(time_via) def camera(self, angle): - while bycmd.send_angle_camera(angle) == -1: + while bycmd.send_angle_camera(angle) == -1 and action_run_flag.isSet(): pass def claw(self, angle): - while bycmd.send_angle_claw(angle) == -1: + while bycmd.send_angle_claw(angle) == -1 and action_run_flag.isSet(): pass time.sleep(0.5) def claw_arm(self, angle): - while bycmd.send_angle_claw_arm(angle) == -1: + while bycmd.send_angle_claw_arm(angle) == -1 and action_run_flag.isSet(): pass time.sleep(1) def scoop(self, angle): - while bycmd.send_angle_scoop(angle) == -1: + while bycmd.send_angle_scoop(angle) == -1 and action_run_flag.isSet(): pass time.sleep(0.5) def storage(self, angle): - while bycmd.send_angle_storage(angle) == -1: + while bycmd.send_angle_storage(angle) == -1 and action_run_flag.isSet(): pass time.sleep(0.5) def zhuan(self, angle): - while bycmd.send_angle_zhuan(angle) == -1: + while bycmd.send_angle_zhuan(angle) == -1 and action_run_flag.isSet(): pass time.sleep(0.5) def wait(self, _time): diff --git a/app.py b/app.py index 0a19ac6..6774614 100644 --- a/app.py +++ b/app.py @@ -23,7 +23,7 @@ processes = [] time_record = None -task_run_flag = False + # 日志队列 queue = Queue() @@ -37,7 +37,9 @@ app.config['SECRET_KEY'] = 'secret!' socketio = SocketIO(app, allow_unsafe_werkzeug=True) server_process = None +# FIXME 在列表里存所有的 task_process task_process = None +task_run_flag = threading.Event() class WebSocketHandler(logging.Handler): def emit(self, record): @@ -124,23 +126,23 @@ def operate_handle(data): elif data['type'] == 'operate_task': # 任务函数 if data['content'] == 'run': - task_run_flag = True + task_run_flag.set() + # 开启 task 进程前先关闭所有历史进程 if task_process != None: task_process.terminate() + time_record = time.perf_counter() - task_process = Process(target=main_func, args=(queue,skip_task_queue)) + task_process = Process(target=main_func, args=(task_run_flag,queue,skip_task_queue)) task_process.start() logger.info("开启 task") elif data['content'] == 'stop': - task_run_flag = False - task_process.terminate() + task_run_flag.clear() + if task_process != None: + task_process.terminate() logger.info(f"任务结束 用时{time.perf_counter() - time_record}s") logger.info("关闭 task") elif data['content'] == 'restart': - if task_process != None: - task_process.terminate() - task_process = Process(target=main_func, args=(queue,skip_task_queue)) - task_process.start() + pass elif data['type'] == 'show_server_log': content = '' try: @@ -167,7 +169,7 @@ def test_connect(): for item in fileOptions_list: config_data[item] = toml.load(os.path.join(fileOptions_path,item)) socketio.emit('config_data', {'type': 'config_data', 'content': config_data}) - socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag)}) + socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag.isSet())}) def thread_function(): global queue while True: diff --git a/cfg_subtask.toml b/cfg_subtask.toml index 8a02906..311b0bf 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -1,5 +1,5 @@ [get_block] -pid_kp = 1.0 +pid_kp = 1.5 pid_ki = 0 pid_kd = 0 first_block = "blue" @@ -15,7 +15,7 @@ pid_ki = 0 pid_kd = 0 [up_tower] -pid_kp = 1.0 +pid_kp = 1.8 pid_ki = 0 pid_kd = 0 @@ -25,12 +25,12 @@ pid_ki = 0 pid_kd = 0 [put_bball] -pid_kp = 1.5 +pid_kp = 1.2 pid_ki = 0 pid_kd = 0 [put_hanoi1] -pid_kp = 0.5 +pid_kp = 0.7 pid_ki = 0 pid_kd = 0 @@ -53,7 +53,7 @@ pid_kd = 0 llm_enable = true [kick_ass] -pid_kp = 0.8 +pid_kp = 1.2 pid_ki = 0 pid_kd = 0 pos_gap1 = 150 diff --git a/cfg_subtask.toml.891.bak b/cfg_subtask.toml.891.bak new file mode 100644 index 0000000..311b0bf --- /dev/null +++ b/cfg_subtask.toml.891.bak @@ -0,0 +1,61 @@ +[get_block] +pid_kp = 1.5 +pid_ki = 0 +pid_kd = 0 +first_block = "blue" + +[put_block] +pid_kp = 1.2 +pid_ki = 0 +pid_kd = 0 + +[get_bball] +pid_kp = 1.2 +pid_ki = 0 +pid_kd = 0 + +[up_tower] +pid_kp = 1.8 +pid_ki = 0 +pid_kd = 0 + +[get_rball] +pid_kp = 1.0 +pid_ki = 0 +pid_kd = 0 + +[put_bball] +pid_kp = 1.2 +pid_ki = 0 +pid_kd = 0 + +[put_hanoi1] +pid_kp = 0.7 +pid_ki = 0 +pid_kd = 0 + +[put_hanoi2] +pid_kp = 1.0 +pid_ki = 0 +pid_kd = 0 +pos_gap = 160 +first_target = "mp" + +[put_hanoi3] +pid_kp = 1.5 +pid_ki = 0 +pid_kd = 0 + +[move_area] +pid_kp = 1.2 +pid_ki = 0 +pid_kd = 0 +llm_enable = true + +[kick_ass] +pid_kp = 1.2 +pid_ki = 0 +pid_kd = 0 +pos_gap1 = 150 +pos_gap2 = 80 +target_person = 1 diff --git a/main_upper.py b/main_upper.py index ba5b7ba..c5ecbff 100644 --- a/main_upper.py +++ b/main_upper.py @@ -6,12 +6,12 @@ import subtask as sb import majtask as mj from by_cmd_py import by_cmd_py import time -import action as act +# import action as act import logging import signal -running = True -def main_func(_queue, _skip_queue): +# running = True +def main_func(_run_flag,_queue, _skip_queue): if _queue != None: # 日志重定向 class Handler(logging.Handler): @@ -23,14 +23,13 @@ def main_func(_queue, _skip_queue): logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG") def signal_handler(sig, frame): - global running - running = False + _run_flag.clear() signal.signal(signal.SIGTERM, signal_handler) cmd_py_obj = by_cmd_py(_queue) sb.import_obj(cmd_py_obj, _skip_queue) - act.import_obj(cmd_py_obj) + # act.import_obj(_run_flag, cmd_py_obj) # 读取配置 cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml') @@ -42,12 +41,20 @@ def main_func(_queue, _skip_queue): logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO") logger.info(cfg_args) - act.axis.camera(0) - act.axis.x2(140) - act.axis.storage(20) - act.axis.scoop(25) - act.axis.claw_arm(225) - act.axis.exec() + cmd_py_obj.send_angle_camera(0) + cmd_py_obj.send_position_axis_x(1, 140) + cmd_py_obj.send_angle_storage(20) + cmd_py_obj.send_angle_scoop(25) + # cmd_py_obj.send_angle_claw_arm(225) + time.sleep(1) + + # 弃用 action + # act.axis.camera(0) + # act.axis.x2(140) + # act.axis.storage(20) + # act.axis.scoop(25) + # act.axis.claw_arm(225) + # act.axis.exec() logger.info(cfg_main) @@ -72,7 +79,7 @@ def main_func(_queue, _skip_queue): task_queuem_t = sb.task_queuem(task_queue) # 创建任务队列的工作线程 def worker_thread(): - while task_queuem_t.exec(_skip_queue) is True: + while task_queuem_t.exec(_skip_queue) and _run_flag.isSet(): pass # 启动工作线程 @@ -83,7 +90,7 @@ def main_func(_queue, _skip_queue): # 创建主任务 main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象 try: - while running: + while _run_flag.isSet(): if task_queuem_t.status is sb.task_queuem_status.EXECUTING: pass else: diff --git a/subtask.py b/subtask.py index ff32c6d..0ed534a 100644 --- a/subtask.py +++ b/subtask.py @@ -10,7 +10,6 @@ import toml import zmq import time import variable as var -import action as act import re import math import json @@ -116,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5): ret, error = filter.aim_right(label) error += offset # if ret: - if abs(error) <= 8: + if abs(error) <= 10: car_stop() logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") ret, error = filter.aim_right(label) @@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5): car_stop() logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") - ret, box = filter.get(label) - while not ret: - ret, box = filter.get(label) - error = (box[0][2] + box[0][0] - 320) / 2 + offset + # ret, box = filter.get(label) + # while not ret: + # ret, box = filter.get(label) + # error = (box[0][2] + box[0][0] - 320) / 2 + offset logger.info(f"停车后像素误差:{error}") # logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准") @@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3. car_stop() logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") - ret, box = filter.get(target_label) - while not ret: - ret, box = filter.get(target_label) - error = (box[0][2] + box[0][0] - 320) / 2 + offset + # ret, box = filter.get(target_label) + # while not ret: + # ret, box = filter.get(target_label) + # error = (box[0][2] + box[0][0] - 320) / 2 + offset logger.info(f"停车后像素误差:{error}") break return True @@ -426,8 +425,7 @@ class task_queuem(task): class get_block1(): def init(self): var.task_speed = 15 - act.cmd.camera(0) - act.cmd.z2(20, 60, 0) + by_cmd.send_angle_camera(0) filter.switch_camera(1) # if cfg['get_block']['first_block'] == "blue": # self.target_label = tlabel.BBLOCK @@ -464,7 +462,7 @@ class get_block1(): logger.info("抓取块") by_cmd.send_position_axis_z(30, 80) - time.sleep(1) + time.sleep(1.5) by_cmd.send_angle_claw(63) by_cmd.send_position_axis_x(1, 20) time.sleep(1) @@ -557,7 +555,7 @@ class put_block(): ret, box = filter.get(tlabel.HOSPITAL) if ret > 0: width = box[0][2] - box[0][0] - if width > 130: #135 + if width > 125: #135 130 128 return True return False def exec(self): @@ -568,7 +566,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, 30) # 20 + by_cmd.send_position_axis_x(1, 50) # 20 time.sleep(1) by_cmd.send_angle_claw(63) time.sleep(1) @@ -593,7 +591,7 @@ 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, 30) + by_cmd.send_position_axis_x(1, 50) time.sleep(1.5) by_cmd.send_angle_claw(45) time.sleep(1) @@ -651,6 +649,7 @@ class get_bball(): car_stop() time.sleep(0.5) for _ in range(3): + by_cmd.send_position_axis_z(30, 155) calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5) logger.info("抓蓝色球") time.sleep(0.5) @@ -672,7 +671,7 @@ class get_bball(): time.sleep(0.5) by_cmd.send_angle_claw_arm(45) time.sleep(1) - by_cmd.send_position_axis_z(30, 155) + # by_cmd.send_position_axis_z(30, 155) # 继续向前走 # by_cmd.send_speed_x(4) pass @@ -710,10 +709,10 @@ class up_tower(): calibrate_new(tlabel.TOWER, offset = 20, run = True) time.sleep(1) # calibrate(tlabel.TOWER, 27, False, 6) - by_cmd.send_distance_x(-10, 120) + by_cmd.send_distance_x(-10, 100) time.sleep(1) # 上古參數 - by_cmd.send_distance_y(-10, 50) # 80 + # by_cmd.send_distance_y(-10, 50) # 80 # 6_9 模型參數 # by_cmd.send_distance_y(-10, 40) # 7_12_3 模型參數 @@ -724,6 +723,11 @@ class up_tower(): # time.sleep(3) # by_cmd.send_speed_y(-10) # time.sleep(0.15) + # 8822 + # by_cmd.send_distance_y(-10, 50) + # 891 + by_cmd.send_distance_y(-10, 35) + time.sleep(0.3) by_cmd.send_angle_zhuan(10) time.sleep(12) @@ -770,8 +774,8 @@ class get_rball(): by_cmd.send_angle_scoop(20) # 上古參數 # by_cmd.send_distance_y(-15, 50) # 50 # 70 - by_cmd.send_distance_y(-15, 40) # 50 # 70 - time.sleep(1.5) + # by_cmd.send_distance_y(-15, 40) # 50 # 70 + # time.sleep(1.5) # 6_9 參數 # by_cmd.send_distance_y(-15, 35) # time.sleep(2) @@ -779,6 +783,12 @@ class get_rball(): # by_cmd.send_distance_y(-15, 45) # time.sleep(2) # car_stop() + # 8822 参数 + # by_cmd.send_distance_y(-15, 40) + # time.sleep(1.5) + # 891 参数 + by_cmd.send_distance_y(-15, 30) + time.sleep(0.3) calibrate_new(tlabel.RBALL,offset = 44, run = True) time.sleep(1) logger.info("抓红球") @@ -826,14 +836,14 @@ class put_bball(): calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6) # by_cmd.send_distance_x(10, 10) # 向左运动 - by_cmd.send_distance_y(-10, 35) + # by_cmd.send_distance_y(-10, 35) # by_cmd.send_angle_storage(10) # time.sleep(1) by_cmd.send_angle_storage(50) logger.info("把球放篮筐里") - time.sleep(1) + time.sleep(2) # by_cmd.send_distance_y(10, 55) by_cmd.send_angle_storage(20) # time.sleep(1) @@ -959,7 +969,7 @@ class put_hanoi1(): def after(self): # var.switch_lane_model = True if utils.direction == tlabel.RMARK: - var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.2, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) + var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) else: var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) pass @@ -1102,7 +1112,7 @@ class put_hanoi2(): time.sleep(1) pass # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) - ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5) + ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5) if not ret: logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec") return @@ -1163,7 +1173,7 @@ class put_hanoi2(): time.sleep(2) pass # ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) - ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5) if not ret: logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec") return @@ -1537,14 +1547,6 @@ class move_area2(): if len(resp_commands) == 0: return action_list = resp_commands - # 先检查一下 action 是否生成正确，如果不正确直接跳过 - actions_keys = self.action_dict.keys() - try: - for action in action_list: - if not (action.get('action') in actions_keys): - return - except: - return # 进入停车区域 by_cmd.send_distance_y(10, 450) time.sleep((450 * 5 / 1000) + 0.5) @@ -1656,6 +1658,8 @@ class kick_ass(): # by_cmd.send_speed_x(25) # time.sleep(4) pass + + def nexec(self): pass def after(self): diff --git a/subtask_8822.py b/subtask_8822.py index ee3602e..0480bea 100644 --- a/subtask_8822.py +++ b/subtask_8822.py @@ -557,7 +557,7 @@ class put_block(): ret, box = filter.get(tlabel.HOSPITAL) if ret > 0: width = box[0][2] - box[0][0] - if width > 135: + if width > 130: return True return False def exec(self): diff --git a/subtask_891.py b/subtask_891.py new file mode 100644 index 0000000..d463086 --- /dev/null +++ b/subtask_891.py @@ -0,0 +1,1667 @@ +from enum import Enum +from loguru import logger +from utils import label_filter +from utils import tlabel +# from utils import LLM +from utils import LLM_deepseek +from utils import CountRecord +import utils +import toml +import zmq +import time +import variable as var +import action as act +import re +import math +import json +import json5 +# import threading +# import ctypes +cfg = None +cfg_args = None +cfg_move_area = None +by_cmd = None +filter = None +llm_bot = None + +# 目标检测 socket 客户端 +context = None +socket = None + +context1 = None +ocr_socket = None + +global_skip_queue = None + +''' +description: main.py 里执行 引入全局变量 +param {*} _by_cmd 控制器对象 +return {*} +''' +def import_obj(_by_cmd, skip_queue): + + global by_cmd + global filter + global llm_bot + + global context + global socket + + global context1 + global ocr_socket + + global cfg + global cfg_args + global cfg_move_area + 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') + try: + with open('/home/evan/Workplace/project_main/cfg_move_area.json', 'r') as f: + cfg_move_area = json.load(f) + except: + cfg_move_area = None + by_cmd = _by_cmd + global_skip_queue = skip_queue + + # 目标检测 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_deepseek() +def car_stop(): + for _ in range(3): + by_cmd.send_speed_x(0) + time.sleep(0.2) + by_cmd.send_speed_y(0) + time.sleep(0.2) + by_cmd.send_speed_omega(0) +# 蓝色球使用 +def calibrate_right_new(label, offset, run = True, run_speed = 3.5): + + not_found_counts = 0 + ret, error = filter.aim_right(label) + while not ret: + not_found_counts += 1 + if not_found_counts >= 20: + not_found_counts = 0 + error = -320 # error > 0 front run + logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找") + break + 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: + 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) <= 8: + car_stop() + logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") + ret, error = filter.aim_right(label) + while not ret: + not_found_counts += 1 + if not_found_counts >=30: + return + ret, error = filter.aim_right(label) + error += offset + logger.info(f"calibrate_right_new:停车后的误差是{error}") + if abs(error) > 8: + logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准") + error = error * 1.5 + 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 后直接停车 +如果停车后 error > 8 则使用 distance 校准 +这个方法仅用于在视野里能找到的情况下进行校准，并不能实现边走边寻找 然后再校准 +param {*} label +param {*} offset +param {*} run +param {*} run_speed +return {*} +''' +def calibrate_new(label, offset, run = True, run_speed = 3.5): + not_found_counts = 0 + ret, box = filter.get(label) + while not ret: + not_found_counts += 1 + if not_found_counts >= 20: + not_found_counts = 0 + error = -320 # error > 0 front run + logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找") + break + ret, box = filter.get(label) + + # 如果超过二十帧跳出，则此时 box 为空值，需要再校验 ret + if ret is True: + 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: + if abs(error) > 8: + logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准") + # error = error # 3 + if error > 0: + by_cmd.send_distance_x(-10, int(error)) + else: + by_cmd.send_distance_x(10, int(-error)) + return + logger.info(f"calibrate_new:停车后误差{error}小于 8 不校准") + 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) <= 10: # 5 + car_stop() + logger.info("calibrate_new:行进时 误差小于 10 直接停车") + + ret, box = filter.get(label) + while not ret: + ret, box = filter.get(label) + + error = (box[0][2] + box[0][0] - 320) / 2 + offset + logger.info(f"calibrate_new:停车后的误差是{error}") + if abs(error) > 8: + logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准") + error = error * 3 + # logger.error(f"error * 3 {error}") + if error > 0: + by_cmd.send_distance_x(-10, int(error)) + 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 = 8 + 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: + # 校准速度越大 停车的条件越宽泛 20 15 + if abs(error) <= stop_error: + car_stop() + logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") + + ret, box = filter.get(label) + while not ret: + ret, box = filter.get(label) + 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)) + + break + return True + +# 对准应知道是左还是右，右侧需在过滤器中进行翻转 +# flipv 为垂直翻转标志，转右侧开启 +def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5): + stop_error = 0 + error_record = CountRecord(10) + if run_direc == 1: + by_cmd.send_speed_x(run_speed) + else: + by_cmd.send_speed_x(-run_speed) + + if target_label == tlabel.TPLATFORM: + stop_error = 8 + else: + stop_error = 15 + while True: + ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK) + while not ret1: + # 如果找不到目标且跳过任务队列非空 (即指令跳过) + if not global_skip_queue.empty(): + _ = global_skip_queue.get() + logger.error("跳过 hanoi_calibrate") + return False + # 如果找不到目标且发现错误目标 (上次放置任务失败) + if ret2: + # 如果连续计数超过阈值，则直接返回 + if error_record(ret2): + return False + ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK) + + error = (box[0][2] + box[0][0] - 320) / 2 + offset + if ret1: + # 校准速度越大 停车的条件越宽泛 20 15 + if abs(error) <= stop_error: + car_stop() + logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") + + ret, box = filter.get(target_label) + while not ret: + ret, box = filter.get(target_label) + error = (box[0][2] + box[0][0] - 320) / 2 + offset + logger.info(f"停车后像素误差:{error}") + break + return True +# 任务类 +class task: + 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 = 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() + else: + logger.warning("[Task ]# 该任务没有 init 方法") + def find(self): + if hasattr(self.task_t, 'find') and callable(getattr(self.task_t, 'find', None)): + return self.task_t.find() + else: + logger.warning("[Task ]# 该任务没有 find 方法") + def exec(self): + # 根据标志位确定是否执行该任务 + if self.enable is True: + if hasattr(self.task_t, 'exec') and callable(getattr(self.task_t, 'exec', None)): + logger.info(f"[Task ]# Executing task") + self.task_t.exec() + else: + logger.warning("[Task ]# 该任务没有 exec 方法") + else: + logger.warning(f"[Task ]# Skip task") + if hasattr(self.task_t, 'nexec') and callable(getattr(self.task_t, 'nexec', None)): + self.task_t.nexec() + else: + 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: + logger.warning("[Task ]# 该任务没有 after 方法") + +# 任务队列状态类 +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()}") + # exec 线程 + self.exec_thread = None + def exec(self, skip_queue): + # 如果空闲状态则将下一个队列任务取出 + 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"[{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.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() # 弹出已执行的任务 + logger.info(f"[TaskM]# <<<<----------------------") + else: + logger.info(f"[TaskM]# Start execute task function (nexec)") + self.status = task_queuem_status.IDEL # 空动作不需要阻塞巡线，直接置位 + self.task_now.exec() # 执行当前任务函数 + self.task_now.after() # 执行任务后处理 + self.queue.task_done() # 弹出已执行的任务 + logger.info(f"[TaskM]# <<<<----------------------") + return True + +# 人员施救 +class get_block1(): + def init(self): + var.task_speed = 15 + 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" + self.target_label = [tlabel.RBLOCK, tlabel.BBLOCK] + self.target_counts = [0, 0] + def find(self): + # 目标检测红/蓝方块 + # 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() + 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 = 16, run = True, run_speed = 5) + logger.info("抓取块") + + by_cmd.send_position_axis_z(30, 80) + time.sleep(1) + by_cmd.send_angle_claw(63) + by_cmd.send_position_axis_x(1, 20) + time.sleep(1) + by_cmd.send_angle_claw(25) + time.sleep(0.5) + by_cmd.send_position_axis_z(30, 110) + time.sleep(0.5) + + by_cmd.send_angle_claw_arm(175) + time.sleep(0.1) + by_cmd.send_position_axis_x(1, 100) + time.sleep(1) + by_cmd.send_position_axis_z(30, 100) + time.sleep(0.5) + by_cmd.send_angle_claw(63) + time.sleep(0.5) + + by_cmd.send_position_axis_z(30, 150) + time.sleep(1) + by_cmd.send_position_axis_x(1, 140) + by_cmd.send_angle_claw_arm(225) + time.sleep(0.5) + # by_cmd.send_angle_storage(55) + # time.sleep(1) + by_cmd.send_position_axis_z(30, 60) + + 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(): + def init(self): + 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 + def find(self): + # 目标检测红/蓝方块 + ret = filter.find(var.second_block) + if ret > 0: + return True + return False + def exec(self): + car_stop() + calibrate_new(var.second_block, offset = 16, run = True, run_speed = 5) + logger.info("抓取块") + time.sleep(0.5) + by_cmd.send_angle_claw_arm(225) + by_cmd.send_angle_claw(63) + time.sleep(0.1) + by_cmd.send_position_axis_x(1, 20) + time.sleep(1) + by_cmd.send_angle_claw(25) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 100) + + # by_cmd.send_distance_x(15, 100) + time.sleep(2) + + pass + def nexec(self): + # TODO 完成不执行任务的空动作 + pass + def after(self): + var.task_speed = 0 + var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"]) + # 任务检查间隔 + time.sleep(7) + + +# 紧急转移 +class put_block(): + def init(self): + var.task_speed = 0 + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) + logger.info("紧急转移初始化") + filter.switch_camera(1) + def find(self): + # 目标检测医院 + ret, box = filter.get(tlabel.HOSPITAL) + if ret > 0: + width = box[0][2] - box[0][0] + if width > 128: #135 130 + return True + return False + def exec(self): + logger.info("找到医院") + car_stop() + calibrate_new(tlabel.HOSPITAL, offset = 7, run = False) + + 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, 50) # 20 + time.sleep(1) + by_cmd.send_angle_claw(63) + time.sleep(1) + + # 放置第二個塊 + by_cmd.send_angle_storage(20) + by_cmd.send_position_axis_x(1, 110) + by_cmd.send_position_axis_z(30, 140) + 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(30,100) + time.sleep(1) + by_cmd.send_angle_claw(25) + by_cmd.send_distance_x(-10, 110) + time.sleep(1) + by_cmd.send_position_axis_z(30, 130) + time.sleep(1) + by_cmd.send_angle_claw_arm(225) + time.sleep(1) + by_cmd.send_position_axis_z(30, 0) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 50) + time.sleep(1.5) + by_cmd.send_angle_claw(45) + time.sleep(1) + pass + def nexec(self): + pass + def after(self): + var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"]) + # 下一动作预备位置 + while by_cmd.send_position_axis_z(30, 150) == -1: + pass + time.sleep(1) + while by_cmd.send_position_axis_x(1, 0) == -1: + pass + while by_cmd.send_angle_claw_arm(45) == -1: + pass + # 任务检查间隔 + # time.sleep(2) + + +# 整装上阵 +class get_bball(): + def init(self): + while (by_cmd.send_angle_camera(90) == -1): + by_cmd.send_angle_camera(90) + # by_cmd.send_position_axis_z(30, 135) + # time.sleep(0.5) + # by_cmd.send_position_axis_x(1, 0) + # time.sleep(2) + # by_cmd.send_angle_claw_arm(45) + + while (by_cmd.send_angle_storage(0) == -1): + by_cmd.send_angle_storage(0) + + # 调试 临时换源 + filter.switch_camera(1) + logger.info("整装上阵初始化") + # time.sleep(0.5) + + self.record = CountRecord(5) + def find(self): + # 目标检测蓝球 + # ret = filter.find(tlabel.BBALL) + ret = filter.find_mult([tlabel.BBALL, tlabel.YBALL]) + # ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL) + # TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关 + # if ret: + # if self.record(tlabel.BBALL): + # return True + if ret[0] or ret[1]: + return True + return False + def exec(self): + logger.info("找到蓝色球") + car_stop() + time.sleep(0.5) + for _ in range(3): + calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5) + logger.info("抓蓝色球") + time.sleep(0.5) + by_cmd.send_angle_claw_arm(45) + by_cmd.send_angle_claw(54) + by_cmd.send_position_axis_x(1, 160) + time.sleep(1.2) + 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, 60) + time.sleep(0.5) + by_cmd.send_distance_axis_z(30, -40) + time.sleep(0.5) + by_cmd.send_angle_claw_arm(90) + time.sleep(0.5) + by_cmd.send_angle_claw(54) + time.sleep(0.5) + by_cmd.send_angle_claw_arm(45) + time.sleep(1) + by_cmd.send_position_axis_z(30, 155) + # 继续向前走 + # by_cmd.send_speed_x(4) + pass + def nexec(self): + pass + def after(self): + var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"]) + # 下一动作预备位置 + by_cmd.send_angle_claw(30) + # time.sleep(0.5) + while by_cmd.send_position_axis_z(30, 0) == -1: + pass + time.sleep(0.5) + # # 任务检查间隔 + # time.sleep(1) + + +# 通信抢修 +class up_tower(): + def init(self): + logger.info("通信抢修初始化") + while (by_cmd.send_angle_camera(90) == -1): + by_cmd.send_angle_camera(90) + filter.switch_camera(1) + def find(self): + # 目标检测通信塔 + ret = filter.find(tlabel.TOWER) + if ret: + return True + return False + def exec(self): + logger.info("找到塔") + car_stop() + time.sleep(1) + calibrate_new(tlabel.TOWER, offset = 20, run = True) + time.sleep(1) + # calibrate(tlabel.TOWER, 27, False, 6) + by_cmd.send_distance_x(-10, 120) + time.sleep(1) + # 上古參數 + # by_cmd.send_distance_y(-10, 50) # 80 + # 6_9 模型參數 + # by_cmd.send_distance_y(-10, 40) + # 7_12_3 模型參數 + # by_cmd.send_distance_y(-10, 50) + # time.sleep(2) + # car_stop() + # FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定 + # time.sleep(3) + # by_cmd.send_speed_y(-10) + # time.sleep(0.15) + # 8822 + # by_cmd.send_distance_y(-10, 50) + # 891 + by_cmd.send_distance_y(-10, 35) + time.sleep(0.3) + + by_cmd.send_angle_zhuan(10) + time.sleep(12) + by_cmd.send_speed_y(10) + time.sleep(0.3) + car_stop() + by_cmd.send_angle_zhuan(0) + # while True: + # pass + def nexec(self): + pass + def after(self): + var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"]) + # 下一动作预备位置 + by_cmd.send_position_axis_z(30, 0) + # 任务检查间隔 + time.sleep(4) + + +# 高空排险 +class get_rball(): + def init(self): + filter.switch_camera(1) + logger.info("高空排险初始化") + while (by_cmd.send_angle_camera(180) == -1): + by_cmd.send_angle_camera(180) + self.record = CountRecord(3) + def find(self): + # 目标检测红球 + ret = filter.find(tlabel.RBALL) + if ret > 0: + # TODO 连续两帧才开始减速 + if self.record(tlabel.RBALL): + var.task_speed = 5 + return True + else: + return False + def exec(self): + logger.info("找到红球") + var.task_speed = 0 + car_stop() + time.sleep(0.5) + # 靠近塔 + by_cmd.send_angle_scoop(20) + # 上古參數 + # by_cmd.send_distance_y(-15, 50) # 50 # 70 + # by_cmd.send_distance_y(-15, 40) # 50 # 70 + # time.sleep(1.5) + # 6_9 參數 + # by_cmd.send_distance_y(-15, 35) + # time.sleep(2) + # 7_12_3 參數 + # by_cmd.send_distance_y(-15, 45) + # time.sleep(2) + # car_stop() + # 8822 参数 + # by_cmd.send_distance_y(-15, 40) + # time.sleep(1.5) + # 891 参数 + by_cmd.send_distance_y(-15, 30) + time.sleep(0.3) + calibrate_new(tlabel.RBALL,offset = 44, run = True) + time.sleep(1) + logger.info("抓红球") + # by_cmd.send_angle_scoop(12) + time.sleep(0.5) + by_cmd.send_position_axis_z(30, 200) + time.sleep(3) + by_cmd.send_angle_scoop(12) + time.sleep(0.5) + by_cmd.send_angle_scoop(7) + time.sleep(0.5) + by_cmd.send_speed_y(15) + time.sleep(0.2) + car_stop() + # by_cmd.send_angle_omega(-55,30) + # while True: + # pass + pass + def nexec(self): + pass + def after(self): + var.pid_turning.set(cfg["get_rball"]["pid_kp"], cfg["get_rball"]["pid_ki"], cfg["get_rball"]["pid_kd"]) + # 任务检查间隔 + time.sleep(2) + + +# 派发物资 +class put_bball(): + def init(self): + logger.info("派发物资初始化") + filter.switch_camera(1) + while by_cmd.send_position_axis_z(30, 0) == -1: + pass + while by_cmd.send_angle_camera(90) == -1: + pass + 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 = -40, run = True, run_speed = 6) + # by_cmd.send_distance_x(10, 10) + # 向左运动 + # by_cmd.send_distance_y(-10, 35) + # by_cmd.send_angle_storage(10) + # time.sleep(1) + + by_cmd.send_angle_storage(50) + logger.info("把球放篮筐里") + + time.sleep(2) + # by_cmd.send_distance_y(10, 55) + by_cmd.send_angle_storage(20) + # time.sleep(1) + # car_stop() + pass + def nexec(self): + pass + def after(self): + var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"]) + # 任务检查间隔 + time.sleep(2) + + +# 物资盘点 +class put_hanoi1(): + def init(self): + logger.info("物资盘点 1 初始化") + filter.switch_camera(2) + def find(self): + ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK]) + if label == tlabel.RMARK: + if abs(error) < 55: + logger.info("向右拐") + utils.direction_right += 1 + return True + return False + elif label == tlabel.LMARK: + if abs(error) < 50: + logger.info("向左拐") + utils.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) + + by_cmd.send_position_axis_z(30, 150) + + # 校准牌子 + if utils.direction_right > utils.direction_left: + ret, error = filter.aim_near(tlabel.RMARK) + while not ret: + ret, error = filter.aim_near(tlabel.RMARK) + utils.direction = tlabel.RMARK + logger.info("应该向右转") + else: + ret, error = filter.aim_near(tlabel.LMARK) + while not ret: + ret, error = filter.aim_near(tlabel.LMARK) + utils.direction = tlabel.LMARK + logger.info("应该向左转") + + + # 校准 omega + for _ in range(10): + ret, box = filter.get(utils.direction) + if ret: + error = (box[0][2] + box[0][0] - 320) / 2 + by_cmd.send_speed_omega(-error * 0.8) + time.sleep(0.2) + car_stop() + + + # 前进 + # by_cmd.send_distance_x(10, 200) + # by_cmd.send_distance_x(10, 180) + # by_cmd.send_distance_x(10, 180) + # time.sleep(1.5) + # car_stop() + + while True: + by_cmd.send_speed_x(8.5) + ret, box = filter.get(utils.direction) + if ret: + if abs(box[0][2] - box[0][0]) > 41: + car_stop() + break + + # 根据方向初始化执行器位置 + if utils.direction is tlabel.RMARK: + # FIXME 右侧的爪子会被 storage 挡住 + by_cmd.send_position_axis_x(1, 0) + by_cmd.send_angle_claw_arm(45) + by_cmd.send_angle_storage(0) + else: + by_cmd.send_position_axis_x(1, 150) + by_cmd.send_angle_claw_arm(225) + by_cmd.send_angle_storage(55) + time.sleep(1) + + by_cmd.send_position_axis_z(30, 10) + + if utils.direction_right > utils.direction_left: + utils.direction = tlabel.RMARK + # by_cmd.send_angle_omega(-25,430) + # by_cmd.send_angle_omega(-55,194) + # by_cmd.send_angle_omega(-45,238) + # by_cmd.send_angle_omega(-45,252) + by_cmd.send_angle_omega(-45,260) + 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(55,194) + # by_cmd.send_angle_omega(45,238) + # by_cmd.send_angle_omega(45,252) + by_cmd.send_angle_omega(45,260) + time.sleep(2) + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) + time.sleep(0.5) + filter.switch_camera(1) + def nexec(self): + pass + def after(self): + # var.switch_lane_model = True + if utils.direction == tlabel.RMARK: + var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) + else: + var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) + pass + # time.sleep(1.5) + +class put_hanoi2(): + def __init__(self): + + if cfg['put_hanoi2']['first_target'] == "lp": + self.target_label = tlabel.LPILLER + elif cfg['put_hanoi2']['first_target'] == "mp": + self.target_label = tlabel.MPILLER + elif cfg['put_hanoi2']['first_target'] == "sp": + self.target_label = tlabel.SPILLER + def init(self): + logger.info("物资盘点 2 初始化") + var.task_speed = 8.5 + if utils.direction == tlabel.RMARK: + # 15 + self.offset = 14 + # self.platform_offset = -25 + # self.platform_offset = -19 + self.platform_offset = -15 + else: + self.offset = 14 + # self.platform_offset = -30 + # self.platform_offset = -19 + self.platform_offset = -15 + # 延时，防止过早看到 tplatform（虽然此现象相当少见且诡异） + time.sleep(1.5) + + def find(self): + # ret, box = filter.get(self.target_label) + ret, box = filter.get(tlabel.TPLATFORM) + if ret: + error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset + if error > 0: + return True + return False + def exec(self): + logger.info(f"direction:{utils.direction.name}") + var.task_speed = 0 + car_stop() + + # 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) + 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, 40) + by_cmd.send_position_axis_x(1, 150) + by_cmd.send_angle_claw(63) + time.sleep(2) + by_cmd.send_angle_claw(40) + time.sleep(0.5) + by_cmd.send_distance_axis_z(30, 30) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 10) + time.sleep(1) + pass + else: + by_cmd.send_position_axis_z(30, 40) + by_cmd.send_position_axis_x(1, 40) + by_cmd.send_angle_claw(63) + time.sleep(2) + by_cmd.send_angle_claw(40) + time.sleep(0.5) + by_cmd.send_distance_axis_z(30, 30) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 160) + time.sleep(1) + pass + 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, 150) + time.sleep(1.5) + by_cmd.send_distance_axis_z(30, -20) + time.sleep(1) + by_cmd.send_angle_claw(81) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 10) + time.sleep(0.5) + by_cmd.send_angle_claw(63) + time.sleep(1) + pass + else: + by_cmd.send_position_axis_x(1, 40) + time.sleep(1.5) + by_cmd.send_distance_axis_z(30, -20) + time.sleep(1) + by_cmd.send_angle_claw(81) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 160) + time.sleep(0.5) + by_cmd.send_angle_claw(63) + 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, 40) + by_cmd.send_position_axis_x(1, 150) + by_cmd.send_angle_claw(55) + time.sleep(2) + by_cmd.send_angle_claw(35) + time.sleep(0.5) + by_cmd.send_distance_axis_z(30, 20) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 10) + time.sleep(1) + pass + else: + by_cmd.send_position_axis_z(30, 40) + by_cmd.send_position_axis_x(1, 40) + by_cmd.send_angle_claw(55) + time.sleep(2) + by_cmd.send_angle_claw(35) + 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 + # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + ret = hanoi_calibrate(tlabel.LPILLER, [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_z(30, 150) + time.sleep(2) + by_cmd.send_position_axis_x(1, 150) + time.sleep(2) + by_cmd.send_distance_axis_z(30, -20) + time.sleep(0.5) + by_cmd.send_angle_claw(55) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 10) + time.sleep(1) + pass + else: + by_cmd.send_position_axis_z(30, 150) + time.sleep(2) + by_cmd.send_position_axis_x(1, 40) + time.sleep(2) + by_cmd.send_distance_axis_z(30, -20) + time.sleep(0.5) + by_cmd.send_angle_claw(55) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 160) + 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, 40) + by_cmd.send_position_axis_x(1, 150) + by_cmd.send_angle_claw(50) + time.sleep(2) + by_cmd.send_angle_claw(27) + time.sleep(1) + by_cmd.send_distance_axis_z(30, 10) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 0) + time.sleep(2) + pass + else: + by_cmd.send_position_axis_z(30, 40) + by_cmd.send_position_axis_x(1, 40) + by_cmd.send_angle_claw(50) + time.sleep(2) + by_cmd.send_angle_claw(27) + time.sleep(1) + by_cmd.send_distance_axis_z(30, 10) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 170) + time.sleep(2) + pass + # ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, 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_z(30, 210) # 170 #190(new) + time.sleep(1.5) + by_cmd.send_position_axis_x(1, 150) + time.sleep(2) + by_cmd.send_distance_axis_z(30, -20) + time.sleep(0.5) + by_cmd.send_angle_claw(80) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 10) + time.sleep(1) + + # by_cmd.send_speed_y(10) + # time.sleep(0.12) + # car_stop() + pass + else: + by_cmd.send_position_axis_z(30, 210) + time.sleep(1.5) + by_cmd.send_position_axis_x(1, 40) + time.sleep(2) + by_cmd.send_distance_axis_z(30, -20) + time.sleep(0.5) + by_cmd.send_angle_claw(80) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 160) + time.sleep(1.5) + # while True: + # pass + by_cmd.send_speed_x(12) + time.sleep(1.2) + def nexec(self): + pass + def after(self): + # var.switch_lane_model = False + if utils.direction is tlabel.RMARK: + var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) + else: + var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) + # time.sleep(2) + var.task_speed = 13 + pass + +class put_hanoi3(): + def init(self): + while by_cmd.send_position_axis_z(30, 150) == -1: + pass + time.sleep(3) + logger.info("完成任务，爪回左侧") + while by_cmd.send_angle_claw_arm(128) == -1: + pass + time.sleep(0.5) + while by_cmd.send_position_axis_x(1, 0) == -1: + pass + time.sleep(1) + # while by_cmd.send_angle_claw_arm(225) == -1: + # pass + while by_cmd.send_angle_claw(85) == -1: + pass + def find(self): + time.sleep(1) + return True + def exec(self): + while by_cmd.send_position_axis_z(30, 120) == -1: + pass + pass + def nexec(self): + pass + def after(self): + by_cmd.send_angle_storage(20) + by_cmd.send_position_axis_x(1, 150) + var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"] - 0.2, cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"]) + # FIXME 此处 -0.2 在 `2e6ce3e1f7d326d6ce8110855e2339ebc03ab2da` 前没有 + +# 应急避险 第一阶段 找目标牌 +class move_area1(): + def init(self): + logger.info("应急避险第一阶段初始化") + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) + filter.switch_camera(1) + def find(self): + ret = filter.find(tlabel.SIGN) + if ret: + return True + return False + def exec(self): + logger.info("找到标示牌") + # 停车 ocr 识别文字 调用大模型 + car_stop() + time.sleep(1) + # calibrate_new(tlabel.SIGN, offset = 8, run = True) + calibrate_new(tlabel.SIGN, offset = -1, run = True, run_speed = 5) + time.sleep(1) + + by_cmd.send_position_axis_x(1, 50) + time.sleep(1) + + # filter_w = (148, 560) + # filter_h = (165, 390) + if cfg_move_area == None: + counts = 0 + while True: + ocr_socket.send_string("") + resp = ocr_socket.recv_pyobj() + var.llm_text = '' + counts += 1 + if resp.get('code') == 0: + for item in resp.get('content'): + + if item['probability']['average'] < 0.80: + continue + # box = item['location'] + # center_x = box['left'] + box['width'] / 2 + # center_y = box['top'] + box['height'] / 2 + # if center_x < filter_w[0] or center_x > filter_w[1] \ + # or center_y < filter_h[0] or center_y > filter_h[1]: + # continue + var.llm_text += item['words'] + break + if counts >= 2: + var.skip_llm_task_flag = True + return + logger.error(f"OCR 检出字符：\"{var.llm_text}\"") + + + + if len(var.llm_text) < 5: + var.skip_llm_task_flag = True + return + else: + # 当有效字符大于 5 个文字时 才请求大模型 + llm_bot.request(var.llm_text) + else: + # 不需要文字识别 直接使用传入的参数执行 action + pass + + var.task_speed = 9 # 12 + + pass + def nexec(self): + pass + def after(self): + # 任务检查间隔 + by_cmd.send_position_axis_x(1, 150) + # time.sleep(1) + # by_cmd.send_angle_claw_arm(225) + + pass + +# 应急避险 第二阶段 找停车区域 +class move_area2(): + def __init__(self): + self.action_dict = { + 'beep_seconds': self.beep_seconds, + 'beep_counts': self.beep_counts, + 'light_seconds': self.light_seconds, + 'light_counts': self.light_counts, + 'beep_light_counts': self.beep_light_counts, + 'beep_light_seconds': self.beep_light_seconds, + 'go_front': self.go_front, + 'go_back': self.go_back, + 'go_left': self.go_left, + 'go_right': self.go_right, + 'go_left_rotate': self.go_left_rotate, + 'go_right_rotate': self.go_right_rotate, + 'go_sleep': self.go_sleep + } + self.front_time = 0 + self.back_time = 0 + self.left_time = 0 + self.right_time = 0 + self.sum_rotate_angle = 0 + self.abs_x = 0 # 为了和程序指令适配，其中 x y 方向互换 + self.abs_y = 0 + self.abs_w = 0 + pass + def init(self): + logger.info("应急避险第二阶段初始化") + self.offset = 60 + self.delta_x = 0 + self.delta_y = 0 + self.delta_omage = 0 + def find(self): + if var.skip_llm_task_flag and cfg_move_area == None: + return 5000 + ret, box = filter.get(tlabel.SHELTER) + if ret: + error = (box[0][2] + box[0][0] - 320) / 2 + self.offset + # 增加了一个宽度过滤 + if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180: + return 5000 + return False + def add_item(self, item): + # FIXME 没有对传入参数的范围进行校验，如果出现单位错误，可能会导致无法回位的问题 + try: + return self.action_dict[item.get('action')](item.get('time')) + except: + pass + return False + def beep_seconds(self, _time): + by_cmd.send_beep(1) + time.sleep(_time * 0.7) + by_cmd.send_beep(0) + return True + def beep_counts(self, _time): + for _ in range(_time): + by_cmd.send_beep(1) + time.sleep(0.3) + by_cmd.send_beep(0) + time.sleep(0.2) + return True + def light_seconds(self, _time): + by_cmd.send_light(1) + time.sleep(_time * 0.7) + by_cmd.send_light(0) + return True + def light_counts(self, _time): + for _ in range(_time): + by_cmd.send_light(1) + time.sleep(0.3) + by_cmd.send_light(0) + time.sleep(0.2) + return True + def beep_light_counts(self, _time): + for _ in range(_time): + by_cmd.send_beep(1) + by_cmd.send_light(1) + time.sleep(0.3) + by_cmd.send_beep(0) + by_cmd.send_light(0) + time.sleep(0.2) + return True + def beep_light_seconds(self, _time): + by_cmd.send_beep(1) + by_cmd.send_light(1) + time.sleep(_time * 0.3) + by_cmd.send_beep(0) + by_cmd.send_light(0) + return True + def go_front(self, _time): + self.abs_y -= math.sin(self.abs_w) * _time + self.abs_x += math.cos(self.abs_w) * _time + logger.info(f"向前移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") + speed_time = int(abs(_time) * 750) + by_cmd.send_distance_x(10, speed_time) + time.sleep(speed_time / 100) + self.front_time += speed_time + return True + def go_back(self, _time): + self.abs_y += math.sin(self.abs_w) * _time + self.abs_x -= math.cos(self.abs_w) * _time + logger.info(f"向后移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") + speed_time = int(abs(_time) * 750) + by_cmd.send_distance_x(-10, speed_time) + time.sleep(speed_time / 100) + self.back_time += speed_time + return True + def go_left(self, _time): + self.abs_y -= math.cos(self.abs_w) * _time + self.abs_x -= math.sin(self.abs_w) * _time + logger.info(f"向左移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") + speed_time = int(abs(_time) * 750) + by_cmd.send_distance_y(-10, speed_time) + time.sleep(speed_time / 100) + self.left_time += speed_time + return True + def go_right(self, _time): + self.abs_y += math.cos(self.abs_w) * _time + self.abs_x += math.sin(self.abs_w) * _time + logger.info(f"向右移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") + speed_time = int(abs(_time) * 750) + by_cmd.send_distance_y(10, speed_time) + time.sleep(speed_time / 100) + self.right_time += speed_time + return True + def go_shift(self, _dis_x, _dis_y): + direct_x = 1.0 if (_dis_x > 0) else -1.0 + direct_y = 1.0 if (_dis_y > 0) else -1.0 + self.abs_y -= math.sin(self.abs_w) * _dis_x + self.abs_x += math.cos(self.abs_w) * _dis_x + self.abs_y += math.cos(self.abs_w) * _dis_y + self.abs_x += math.sin(self.abs_w) * _dis_y + logger.info(f"水平移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") + speed_time_x = int(abs(_dis_x) * 750) + speed_time_y = int(abs(_dis_y) * 750) + by_cmd.send_distance_x(10 * direct_x, speed_time_x) + by_cmd.send_distance_y(10 * direct_y, speed_time_y) + time.sleep(max(speed_time_x, speed_time_y) / 150) #FIXME 除以 100 是否正确 + return True + def go_left_rotate(self, _time): + self.abs_w += math.radians(_time) # 弧度制 + logger.info(f"向左旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") + self.sum_rotate_angle -= _time + speed_time = int(abs(_time) * 3.8) + by_cmd.send_angle_omega(50, speed_time) + time.sleep(speed_time / 200 + 0.5) + return True + def go_right_rotate(self, _time): + self.abs_w -= math.radians(_time) # 弧度制 + logger.info(f"向右旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") + self.sum_rotate_angle += _time + speed_time = int(abs(_time) * 3.8) + by_cmd.send_angle_omega(-50, speed_time) + time.sleep(speed_time / 200 + 0.5) + return True + def go_sleep(self, _time): + time.sleep(_time*0.7) + return True + def reset(self): + logger.info(f"开始复位：[当前位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)}]") + # 归一化角度到 0-2pi + left_dregee = math.degrees(self.abs_w % (2 * math.pi)) + # 确定旋转方向 (寻找回正角度最小旋转方向) + if math.sin(self.abs_w) < 0: + logger.info(f"需要左旋 {360.0 - left_dregee} 回正") + self.go_left_rotate(360.0 - left_dregee) + else: + logger.info(f"需要右旋 {left_dregee} 回正") + self.go_right_rotate(left_dregee) + time.sleep(0.1) + + # 在框中原点添加向左 0.6m 的偏移值，以便直接回到赛道 + self.go_shift(self.abs_x * -1.0, self.abs_y * -1.0 - 0.6) + + logger.info(f"回正后最终位置： ({self.abs_y:.2f}, {self.abs_x:.2f}), 角度： {math.degrees(self.abs_w % (2 * math.pi))}") + def exec(self): + var.task_speed = 0 + if var.skip_llm_task_flag and cfg_move_area == None: + logger.error("ocr 识别出错 直接跳过该任务") + return + logger.info("开始寻找停车区域") + car_stop() + calibrate_new(tlabel.SHELTER, offset = 30, run = True) + time.sleep(0.5) + if cfg_move_area == None: + resp = None + # 调用大模型 然后执行动作 + try: + logger.info("llm 阻塞等待服务器返回中") + start_wait_time = time.perf_counter() + while True: + if llm_bot.success_status.isSet(): + resp = llm_bot.response.choices[0].message.content + logger.info(f"llm 返回原数据 {resp}") + break + now_time = time.perf_counter() + if llm_bot.error_status.isSet() or (now_time - start_wait_time) > 6.5 : + logger.error("大模型 llm_bot 超时，跳过任务") + return + + except: + logger.error("大模型 llm_bot 未知错误，跳过任务") + return + + try: + json_text = re.findall("```json(.*?)```", resp, re.S) + if len(json_text) == 0: + # 返回的内容不带'''json + resp_commands = eval(resp) + else: + resp_commands = json5.loads(json_text[0]) + + + logger.info(f"解析后的动作序列 {resp_commands}") + if len(resp_commands) == 0: + return + action_list = resp_commands + # 进入停车区域 + by_cmd.send_distance_y(10, 450) + time.sleep((450 * 5 / 1000) + 0.5) + for action in action_list: + self.add_item(action) + time.sleep(0.1) + time.sleep(0.5) + self.reset() + except: + logger.warning("任务解析失败并退出，文心一言真是废物 (毋庸置疑)") + pass + + else: + # 无需调用大模型 直接开始执行传入的参数 + try: + by_cmd.send_distance_y(10, 450) + time.sleep((450 * 5 / 1000) + 0.5) + for action in cfg_move_area: + self.add_item(action) + time.sleep(0.1) + time.sleep(0.5) + self.reset() + except: + pass + 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"]) + # by_cmd.send_position_axis_z(30, 0) + by_cmd.send_position_axis_z(30, 120) + while by_cmd.send_angle_claw(90) == -1: + pass + time.sleep(2) + +# 扫黑除暴 +class kick_ass(): + def init(self): + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) + logger.info("扫黑除暴初始化") + self.pos_gap1 = cfg['kick_ass']['pos_gap1'] + self.pos_gap2 = cfg['kick_ass']['pos_gap2'] + self.target_person = cfg_args['lane_mode']['mode_index'] + + # by_cmd.send_angle_claw(15) + # by_cmd.send_position_axis_x(1, 160) + + def find(self): + ret = filter.find(tlabel.SIGN) + if ret: + return True + return False + def exec(self): + logger.info("找到标示牌") + # 停的晚无需校准 omage + car_stop() + time.sleep(1) + calibrate_new(tlabel.SIGN, offset = 8, run = True) + by_cmd.send_angle_claw(15) + time.sleep(0.5) + # by_cmd.send_position_axis_z(30, 80) + # OCR 摄像头向前移动 + by_cmd.send_position_axis_x(1, 50) + time.sleep(1) + by_cmd.send_position_axis_x(1, 150) + + # 移动到中间 + time.sleep(1) + by_cmd.send_angle_claw(15) + by_cmd.send_angle_claw_arm(225) + time.sleep(1) + by_cmd.send_position_axis_z(30, 80) + time.sleep(1) + + # 先移动到第一个人的地方 + by_cmd.send_distance_x(10, self.pos_gap1) + time.sleep(1.5) + if self.target_person == 1: + # target_distance = self.pos_gap1 + pass + else: + # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10 + target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1 - 1) * 10 + by_cmd.send_distance_x(10, target_distance) + logger.info(f"target distance {target_distance}") + time.sleep(1.5 + (self.target_person - 1) * 0.7 ) + car_stop() + + # by_cmd.send_angle_claw_arm(225) + # time.sleep(0.5) + by_cmd.send_position_axis_x(1, 20) + time.sleep(3) + by_cmd.send_position_axis_x(1, 120) + time.sleep(1) + logger.debug("結束任務，前進四") + + filter.switch_camera(2) + for _ in range(3): + by_cmd.send_speed_x(15) + while True: + ret, box = filter.get(tlabel.BASE) + if ret: + error = (box[0][2] + box[0][0] - 320) / 2 + by_cmd.send_speed_omega(-error * 0.8) + time.sleep(0.02) + # by_cmd.send_speed_x(25) + # time.sleep(4) + pass + + + def nexec(self): + pass + def after(self): + var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"]) \ No newline at end of file diff --git a/test/test_filter.py b/test/test_filter.py index 99a4998..c75899f 100644 --- a/test/test_filter.py +++ b/test/test_filter.py @@ -25,10 +25,11 @@ filter.switch_camera(1) # label = tlabel.TPLATFORM while True: time.sleep(0.2) - ret, box = filter.get(tlabel.SIGN) + ret, box = filter.get(tlabel.HOSPITAL) if ret: + width = box[0][2] - box[0][0] error = (box[0][2] + box[0][0] - 320) / 2 - logger.error(error) + logger.error(width) # if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180: # # height = box[0][3] - box[0][1] # logger.error(111) diff --git a/utils.py b/utils.py index a67331d..fb8edce 100644 --- a/utils.py +++ b/utils.py @@ -285,7 +285,11 @@ class label_filter: else: target_bool = False target_box = None - expect_boxes = (results[:, 0] == label.value) + if len(label) == 1: + expect_boxes = (results[:, 0] == label[0].value) + elif len(label) == 2: + expect_boxes = np.logical_or(results[:, 0] == label[0].value, results[:, 0] == label[1].value) + boxes = results[expect_boxes, :] # 在此处过滤 if len(boxes) != 0: