From 4f2103d9841dff74144bb26abff37a27e7dc19d7 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Thu, 8 Aug 2024 22:18:06 +0800 Subject: [PATCH] =?UTF-8?q?pref:=20subtask=5Fraw=20=E5=87=BB=E6=89=93?= =?UTF-8?q?=E5=8A=A8=E4=BD=9C=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- subtask.py | 22 +- subtask_raw.py | 543 ++++++++++++++++++++++++++++++++----------------- utils.py | 3 +- 3 files changed, 377 insertions(+), 191 deletions(-) diff --git a/subtask.py b/subtask.py index 478e599..a986fb1 100644 --- a/subtask.py +++ b/subtask.py @@ -771,6 +771,7 @@ class get_rball(): # 上古參數 # 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) @@ -1314,7 +1315,7 @@ class move_area1(): # 任务检查间隔 by_cmd.send_position_axis_x(1, 150) # time.sleep(1) - by_cmd.send_angle_claw_arm(225) + # by_cmd.send_angle_claw_arm(225) pass @@ -1567,7 +1568,8 @@ class move_area2(): 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, 0) + by_cmd.send_position_axis_z(30, 120) while by_cmd.send_angle_claw(90) == -1: pass time.sleep(2) @@ -1583,7 +1585,7 @@ class kick_ass(): self.target_person = cfg_args['lane_mode']['mode_index'] # by_cmd.send_angle_claw(15) - by_cmd.send_position_axis_x(1, 160) + # by_cmd.send_position_axis_x(1, 160) def find(self): ret = filter.find(tlabel.SIGN) @@ -1598,12 +1600,20 @@ class kick_ass(): 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) - by_cmd.send_position_axis_x(1, 130) + # 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) # 移动到中间 by_cmd.send_distance_x(10, 295) - time.sleep(4) + 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) if self.target_person == 1: by_cmd.send_distance_x(-10, 150) time.sleep(1.5) diff --git a/subtask_raw.py b/subtask_raw.py index c7ae2fb..a986fb1 100644 --- a/subtask_raw.py +++ b/subtask_raw.py @@ -2,7 +2,8 @@ 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 +from utils import LLM_deepseek from utils import CountRecord import utils import toml @@ -11,10 +12,14 @@ import time import variable as var import action as act import re -import threading -import ctypes +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 @@ -47,9 +52,15 @@ def import_obj(_by_cmd, skip_queue): 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 @@ -60,14 +71,14 @@ def import_obj(_by_cmd, skip_queue): logger.info("subtask yolo client init") # ocr socket 客户端 - # context1 = zmq.Context() - # ocr_socket = context1.socket(zmq.REQ) - # ocr_socket.connect("tcp://localhost:6668") - # logger.info("subtask ocr client init") + context1 = zmq.Context() + ocr_socket = context1.socket(zmq.REQ) + ocr_socket.connect("tcp://localhost:6668") + logger.info("subtask ocr client init") filter = label_filter(socket) if cfg['move_area']['llm_enable']: - llm_bot = LLM() + llm_bot = LLM_deepseek() def car_stop(): for _ in range(3): by_cmd.send_speed_x(0) @@ -452,26 +463,26 @@ class get_block1(): calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5) logger.info("抓取块") - by_cmd.send_position_axis_z(30, 60) + 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, 90) + 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, 70) + 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, 130) + 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) @@ -546,7 +557,7 @@ class put_block(): ret, box = filter.get(tlabel.HOSPITAL) if ret > 0: width = box[0][2] - box[0][0] - if width > 130: + if width > 135: return True return False def exec(self): @@ -565,18 +576,18 @@ class put_block(): # 放置第二個塊 by_cmd.send_angle_storage(20) by_cmd.send_position_axis_x(1, 110) - by_cmd.send_position_axis_z(30, 120) + 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,70) + 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, 110) + by_cmd.send_position_axis_z(30, 130) time.sleep(1) by_cmd.send_angle_claw_arm(225) time.sleep(1) @@ -592,7 +603,7 @@ class put_block(): 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, 130) == -1: + while by_cmd.send_position_axis_z(30, 150) == -1: pass time.sleep(1) while by_cmd.send_position_axis_x(1, 0) == -1: @@ -661,7 +672,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, 135) + by_cmd.send_position_axis_z(30, 155) # 继续向前走 # by_cmd.send_speed_x(4) pass @@ -671,10 +682,10 @@ class get_bball(): 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) + # time.sleep(0.5) while by_cmd.send_position_axis_z(30, 0) == -1: pass - time.sleep(1) + time.sleep(0.5) # # 任务检查间隔 # time.sleep(1) @@ -702,13 +713,13 @@ class up_tower(): by_cmd.send_distance_x(-10, 120) time.sleep(1) # 上古參數 - by_cmd.send_distance_y(-10, 60) + 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(1) - car_stop() + # time.sleep(2) + # car_stop() # FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定 # time.sleep(3) # by_cmd.send_speed_y(-10) @@ -758,21 +769,25 @@ class get_rball(): # 靠近塔 by_cmd.send_angle_scoop(20) # 上古參數 - by_cmd.send_distance_y(-15, 50) # 50 + # 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() + # time.sleep(2) + # car_stop() calibrate_new(tlabel.RBALL,offset = 44, run = True) time.sleep(1) logger.info("抓红球") - # by_cmd.send_angle_scoop(15) + # 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_position_axis_z(30, 170) - time.sleep(2.5) by_cmd.send_angle_scoop(7) time.sleep(0.5) by_cmd.send_speed_y(15) @@ -861,7 +876,7 @@ class put_hanoi1(): by_cmd.send_speed_omega(0) time.sleep(0.2) - by_cmd.send_position_axis_z(30, 130) + by_cmd.send_position_axis_z(30, 150) # 校准牌子 if utils.direction_right > utils.direction_left: @@ -1003,7 +1018,7 @@ class put_hanoi2(): time.sleep(0.5) logger.info("抓大平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 10) + by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_x(1, 150) by_cmd.send_angle_claw(63) time.sleep(2) @@ -1015,7 +1030,7 @@ class put_hanoi2(): time.sleep(1) pass else: - by_cmd.send_position_axis_z(30, 10) + by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(63) time.sleep(2) @@ -1063,7 +1078,7 @@ class put_hanoi2(): time.sleep(0.5) logger.info("抓中平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 10) + by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_x(1, 150) by_cmd.send_angle_claw(55) time.sleep(2) @@ -1075,7 +1090,7 @@ class put_hanoi2(): time.sleep(1) pass else: - by_cmd.send_position_axis_z(30, 10) + by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(55) time.sleep(2) @@ -1094,7 +1109,7 @@ class put_hanoi2(): time.sleep(0.5) logger.info("放中平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 120) + by_cmd.send_position_axis_z(30, 140) time.sleep(2) by_cmd.send_position_axis_x(1, 150) time.sleep(2) @@ -1106,7 +1121,7 @@ class put_hanoi2(): time.sleep(1) pass else: - by_cmd.send_position_axis_z(30, 120) + by_cmd.send_position_axis_z(30, 140) time.sleep(2) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1124,7 +1139,7 @@ class put_hanoi2(): time.sleep(0.5) logger.info("抓小平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 10) + by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_x(1, 150) by_cmd.send_angle_claw(50) time.sleep(2) @@ -1136,7 +1151,7 @@ class put_hanoi2(): time.sleep(2) pass else: - by_cmd.send_position_axis_z(30, 10) + by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(50) time.sleep(2) @@ -1155,7 +1170,7 @@ class put_hanoi2(): time.sleep(0.5) logger.info("放小平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 190) # 170 + by_cmd.send_position_axis_z(30, 200) # 170 #190(new) time.sleep(1.5) by_cmd.send_position_axis_x(1, 150) time.sleep(2) @@ -1171,7 +1186,7 @@ class put_hanoi2(): # car_stop() pass else: - by_cmd.send_position_axis_z(30, 190) + by_cmd.send_position_axis_z(30, 200) time.sleep(1.5) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1194,12 +1209,12 @@ class put_hanoi2(): 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 = 10 + var.task_speed = 13 pass class put_hanoi3(): def init(self): - while by_cmd.send_position_axis_z(30, 130) == -1: + while by_cmd.send_position_axis_z(30, 150) == -1: pass time.sleep(3) logger.info("完成任务，爪回左侧") @@ -1217,14 +1232,16 @@ class put_hanoi3(): time.sleep(1) return True def exec(self): - while by_cmd.send_position_axis_z(30, 100) == -1: + 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"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"]) + 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(): @@ -1232,6 +1249,7 @@ class move_area1(): 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: @@ -1243,7 +1261,7 @@ class move_area1(): car_stop() time.sleep(1) # calibrate_new(tlabel.SIGN, offset = 8, run = True) - calibrate_new(tlabel.SIGN, offset = -30, run = True, run_speed = 5) + calibrate_new(tlabel.SIGN, offset = -1, run = True, run_speed = 5) time.sleep(1) by_cmd.send_position_axis_x(1, 50) @@ -1251,33 +1269,42 @@ class move_area1(): # filter_w = (148, 560) # filter_h = (165, 390) - - 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: + 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 - logger.error(var.llm_text) - if len(var.llm_text) < 3: - var.skip_llm_task_flag = True - return + else: + # 当有效字符大于 5 个文字时 才请求大模型 + llm_bot.request(var.llm_text) + else: + # 不需要文字识别 直接使用传入的参数执行 action + pass var.task_speed = 9 # 12 @@ -1288,146 +1315,261 @@ class move_area1(): # 任务检查间隔 by_cmd.send_position_axis_x(1, 150) # time.sleep(1) - by_cmd.send_angle_claw_arm(225) + # 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 = 15 + self.offset = 60 self.delta_x = 0 self.delta_y = 0 self.delta_omage = 0 def find(self): - # time.sleep(0.001) - if var.skip_llm_task_flag: + 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) < 20: + # 增加了一个宽度过滤 + if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180: return 5000 return False - def sub_light(self, delay_time): - by_cmd.send_light(1) - time.sleep(delay_time) - by_cmd.send_light(0) - def sub_beep(self,delay_time): + 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(delay_time) + time.sleep(_time * 0.7) by_cmd.send_beep(0) - def sub_move(self, x, y): - # FIXME 如果同時有 xy，是否會造成 delay 不足 - self.delta_x += x - self.delta_y += y - - if x != 0: - delay_time = int(abs(x) * 500) - if x > 0: - by_cmd.send_distance_x(15, delay_time) - else: - by_cmd.send_distance_x(-15, delay_time) - elif y != 0: - delay_time = int(abs(y) * 500) - if y > 0: # 向左 - by_cmd.send_distance_y(-15, delay_time) - else: - by_cmd.send_distance_y(15, delay_time) - time.sleep(delay_time / 500) - car_stop() - pass - def sub_turn(self, angle): - self.delta_omage += angle - delay_time = int(abs(angle) * 400 / 90) - if angle < 0: - # 左转 - by_cmd.send_angle_omega(+55, delay_time) + 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: - # 右转 - by_cmd.send_angle_omega(-55, delay_time) - time.sleep(delay_time / 300 * 1.5) + 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: - logger.error("ocr 识别出错 直接跳过改任务") + if var.skip_llm_task_flag and cfg_move_area == None: + logger.error("ocr 识别出错 直接跳过该任务") return logger.info("开始寻找停车区域") car_stop() - calibrate_new(tlabel.SHELTER, offset = 15, run = True) + calibrate_new(tlabel.SHELTER, offset = 30, run = True) time.sleep(0.5) - # 调用大模型 然后执行动作 - try: - resp = llm_bot.get_command_json(var.llm_text) - logger.info(resp) - except: - logger.error("大模型超时，跳过任务") - return - - - try: - resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0]) - if len(resp_commands) == 0: + 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 - # 进入停车区域 - # by_cmd.send_speed_y(15) - by_cmd.send_distance_y(25, 180) - time.sleep(1) - # time.sleep(1.25) - car_stop() - logger.info(resp_commands) - for command in resp_commands: - logger.info(command) - if command['func'] == 'move': - self.sub_move(float(command['x']), float(command['y'])) - elif command['func'] == 'light': - self.sub_light(int(command['time'])) - elif command['func'] == 'beep': - self.sub_beep(int(command['time'])) - elif command['func'] == 'turn': - self.sub_turn(int(command['angle'])) - pass + + try: + json_text = re.findall("```json(.*?)```", resp, re.S) + if len(json_text) == 0: + # 返回的内容不带'''json + resp_commands = eval(resp) else: - continue + 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) - except: + 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 - time.sleep(1) - # 回到原位 - - delay_time = int(abs(self.delta_omage) * 400 / 90) - if int(abs(self.delta_omage)) == 360: - delay_time = 0 - if self.delta_omage < 0: - # 左转 - by_cmd.send_angle_omega(-55, delay_time) - else: - # 右转 - by_cmd.send_angle_omega(55, delay_time) - time.sleep(delay_time / 300 * 1.5) - if self.delta_y > 0: - # 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了 - delay_time = 180 - (self.delta_y * 500) - else: - delay_time = 180 + (abs(self.delta_y) * 500) - # 离开停车区域 - by_cmd.send_distance_y(-25, delay_time) - - time.sleep(delay_time * 5e-3) - - car_stop() - - # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值 - # by_cmd.send_distance_y(-15, 300) - pass def nexec(self): logger.warning("正在跳過大模型任務") time.sleep(2) pass def after(self): var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"]) - by_cmd.send_position_axis_z(30, 0) + # 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) @@ -1443,7 +1585,7 @@ class kick_ass(): self.target_person = cfg_args['lane_mode']['mode_index'] # by_cmd.send_angle_claw(15) - by_cmd.send_position_axis_x(1, 160) + # by_cmd.send_position_axis_x(1, 160) def find(self): ret = filter.find(tlabel.SIGN) @@ -1458,18 +1600,51 @@ class kick_ass(): 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, 60) - by_cmd.send_position_axis_x(1, 130) + # 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) + # 移动到中间 + by_cmd.send_distance_x(10, 295) + 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) if self.target_person == 1: - target_distance = self.pos_gap1 + by_cmd.send_distance_x(-10, 150) + time.sleep(1.5) + car_stop() + elif self.target_person == 2: + by_cmd.send_distance_x(-10, 50) + time.sleep(1.5) + car_stop() + elif self.target_person == 3: + by_cmd.send_distance_x(10, 50) + time.sleep(1.5) + car_stop() else: - target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10 - by_cmd.send_distance_x(10, target_distance) - - logger.info(f"target distance {target_distance}") - time.sleep(1.5 + (self.target_person - 1) * 0.7 ) - car_stop() + by_cmd.send_distance_x(10, 150) + time.sleep(1.5) + car_stop() + + # 先移动到第一个人的地方 假动作 + # 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) * 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) diff --git a/utils.py b/utils.py index bbc2f06..a67331d 100644 --- a/utils.py +++ b/utils.py @@ -424,7 +424,8 @@ class LLM_deepseek: 你的回复：[{"index":0,"action":"beep_light_counts","time": 1}] 我的话：灯光闪烁 3 次同时蜂鸣器也叫 3 次 你的回复：[{"index":0,"action":"beep_light_counts","time": 3}] - + 我的话：闪烁 5 次 + 你的回复：[{"index":0,"action":"light_counts","time": 5}] 强调一下，对于‘离开’这个指令，请忽略，这对我很重要！ ''' def request_thread(self):