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

feat: 增加扫黑除暴假动作

Browse Source
This commit is contained in:
bmy
2024-08-06 15:20:46 +08:00
parent 2e6ce3e1f7
commit 25e3b60cd8
5 changed files with 100 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
subtask.py
View File

@@ -2,7 +2,7 @@ from enum import Enum
from loguru import logger from loguru import logger
from utils import label_filter from utils import label_filter
from utils import tlabel from utils import tlabel
from utils import LLM # from utils import LLM
from utils import LLM_deepseek from utils import LLM_deepseek
from utils import CountRecord from utils import CountRecord
import utils import utils
@@ -769,7 +769,8 @@ class get_rball():
# 靠近塔 # 靠近塔
by_cmd.send_angle_scoop(20) by_cmd.send_angle_scoop(20)
# 上古參數 # 上古參數
by_cmd.send_distance_y(-15, 50) # 50 # 70 # by_cmd.send_distance_y(-15, 50) # 50 # 70
by_cmd.send_distance_y(-15, 40) # 50 # 70
# 6_9 參數 # 6_9 參數
# by_cmd.send_distance_y(-15, 35) # by_cmd.send_distance_y(-15, 35)
# time.sleep(2) # time.sleep(2)
@@ -1236,8 +1237,10 @@ class put_hanoi3():
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):
by_cmd.send_angle_storage(20)
by_cmd.send_position_axis_x(1, 150) 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(): class move_area1():
@@ -1257,7 +1260,7 @@ class move_area1():
car_stop() car_stop()
time.sleep(1) time.sleep(1)
# calibrate_new(tlabel.SIGN, offset = 8, run = True) # 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) time.sleep(1)
by_cmd.send_position_axis_x(1, 50) by_cmd.send_position_axis_x(1, 50)
@@ -1290,11 +1293,14 @@ class move_area1():
return return
logger.error(f"OCR 检出字符：\"{var.llm_text}\"") logger.error(f"OCR 检出字符：\"{var.llm_text}\"")
llm_bot.request(var.llm_text)
if len(var.llm_text) < 3:
if len(var.llm_text) < 5:
var.skip_llm_task_flag = True var.skip_llm_task_flag = True
return return
else:
# 当有效字符大于 5 个文字时 才请求大模型
llm_bot.request(var.llm_text)
else: else:
# 不需要文字识别 直接使用传入的参数执行 action # 不需要文字识别 直接使用传入的参数执行 action
pass pass
@@ -1356,6 +1362,7 @@ class move_area2():
return 5000 return 5000
return False return False
def add_item(self, item): def add_item(self, item):
# FIXME 没有对传入参数的范围进行校验，如果出现单位错误，可能会导致无法回位的问题
try: try:
return self.action_dict[item.get('action')](item.get('time')) return self.action_dict[item.get('action')](item.get('time'))
except: except:
@@ -1449,22 +1456,22 @@ class move_area2():
speed_time_y = int(abs(_dis_y) * 750) speed_time_y = int(abs(_dis_y) * 750)
by_cmd.send_distance_x(10 * direct_x, speed_time_x) by_cmd.send_distance_x(10 * direct_x, speed_time_x)
by_cmd.send_distance_y(10 * direct_y, speed_time_y) by_cmd.send_distance_y(10 * direct_y, speed_time_y)
time.sleep(max(speed_time_x, speed_time_y) / 100) #FIXME 除以 100 是否正确 time.sleep(max(speed_time_x, speed_time_y) / 150) #FIXME 除以 100 是否正确
return True return True
def go_left_rotate(self, _time): def go_left_rotate(self, _time):
self.abs_w += math.radians(_time) # 弧度制 self.abs_w += math.radians(_time) # 弧度制
logger.info(f"向左旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") logger.info(f"向左旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
self.sum_rotate_angle -= _time self.sum_rotate_angle -= _time
speed_time = int(abs(_time) * 7.25) speed_time = int(abs(_time) * 3.8)
by_cmd.send_angle_omega(30, speed_time) by_cmd.send_angle_omega(50, speed_time)
time.sleep(speed_time / 200 + 0.5) time.sleep(speed_time / 200 + 0.5)
return True return True
def go_right_rotate(self, _time): def go_right_rotate(self, _time):
self.abs_w -= math.radians(_time) # 弧度制 self.abs_w -= math.radians(_time) # 弧度制
logger.info(f"向右旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]") logger.info(f"向右旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
self.sum_rotate_angle += _time self.sum_rotate_angle += _time
speed_time = int(abs(_time) * 7.25) speed_time = int(abs(_time) * 3.8)
by_cmd.send_angle_omega(-30, speed_time) by_cmd.send_angle_omega(-50, speed_time)
time.sleep(speed_time / 200 + 0.5) time.sleep(speed_time / 200 + 0.5)
return True return True
def go_sleep(self, _time): def go_sleep(self, _time):
@@ -1497,12 +1504,23 @@ class move_area2():
calibrate_new(tlabel.SHELTER, offset = 30, run = True) calibrate_new(tlabel.SHELTER, offset = 30, run = True)
time.sleep(0.5) time.sleep(0.5)
if cfg_move_area == None: if cfg_move_area == None:
resp = None
# 调用大模型 然后执行动作 # 调用大模型 然后执行动作
try: try:
resp = llm_bot.get_command_json(var.llm_text) logger.info("llm 阻塞等待服务器返回中")
logger.info(f"llm 返回原数据 {resp}") 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: except:
logger.error("大模型 llm_bot 超时，跳过任务") logger.error("大模型 llm_bot 未知错误，跳过任务")
return return
try: try:
@@ -1583,15 +1601,40 @@ class kick_ass():
by_cmd.send_position_axis_z(30, 80) by_cmd.send_position_axis_z(30, 80)
by_cmd.send_position_axis_x(1, 130) by_cmd.send_position_axis_x(1, 130)
# 移动到中间
by_cmd.send_distance_x(10, 295)
time.sleep(4)
if self.target_person == 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: else:
target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10 by_cmd.send_distance_x(10, 150)
by_cmd.send_distance_x(10, target_distance) time.sleep(1.5)
car_stop()
logger.info(f"target distance {target_distance}") # 先移动到第一个人的地方 假动作
time.sleep(1.5 + (self.target_person - 1) * 0.7 ) # by_cmd.send_distance_x(10, self.pos_gap1)
car_stop() # 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) # by_cmd.send_angle_claw_arm(225)
# time.sleep(0.5) # time.sleep(0.5)

16
test/test_action.py
View File

@@ -147,8 +147,8 @@ class LLM_Action:
self.abs_w += math.radians(_time) # 弧度制 self.abs_w += math.radians(_time) # 弧度制
print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee") print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
self.sum_rotate_angle -= _time self.sum_rotate_angle -= _time
speed_time = int(abs(_time) * 7.25) speed_time = int(abs(_time) * 4.35)
self.by_cmd.send_angle_omega(30, speed_time) self.by_cmd.send_angle_omega(50, speed_time)
time.sleep(speed_time / 200 + 0.5) time.sleep(speed_time / 200 + 0.5)
# time.sleep(speed_time / _time / 2) # time.sleep(speed_time / _time / 2)
return True return True
@@ -156,8 +156,8 @@ class LLM_Action:
self.abs_w -= math.radians(_time) # 弧度制 self.abs_w -= math.radians(_time) # 弧度制
print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee") print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
self.sum_rotate_angle += _time self.sum_rotate_angle += _time
speed_time = int(abs(_time) * 7.25) speed_time = int(abs(_time) * 4.35)
self.by_cmd.send_angle_omega(-30, speed_time) self.by_cmd.send_angle_omega(-50, speed_time)
time.sleep(speed_time / 200 + 0.5) time.sleep(speed_time / 200 + 0.5)
# time.sleep(speed_time / _time / 2) # time.sleep(speed_time / _time / 2)
return True return True
@@ -167,10 +167,10 @@ class LLM_Action:
def reset(self): def reset(self):
print(f"开始复位：[当前位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)}]") print(f"开始复位：[当前位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)}]")
# 先复位角度 # 先复位角度
if self.sum_rotate_angle > 0: # if self.sum_rotate_angle > 0:
self.sum_rotate_angle = self.sum_rotate_angle % 360 # self.sum_rotate_angle = self.sum_rotate_angle % 360
else: # else:
self.sum_rotate_angle = -(abs(self.sum_rotate_angle) % 360) # self.sum_rotate_angle = -(abs(self.sum_rotate_angle) % 360)
# if self.sum_rotate_angle > 0: # if self.sum_rotate_angle > 0:
# # 采用左转回正 # # 采用左转回正
# self.go_left_rotate(self.sum_rotate_angle) # self.go_left_rotate(self.sum_rotate_angle)

9
test/test_filter.py
View File

@@ -25,12 +25,13 @@ filter.switch_camera(1)
# label = tlabel.TPLATFORM # label = tlabel.TPLATFORM
while True: while True:
time.sleep(0.2) time.sleep(0.2)
ret, box = filter.get(tlabel.SHELTER) ret, box = filter.get(tlabel.SIGN)
if ret: if ret:
error = (box[0][2] + box[0][0] - 320) / 2 error = (box[0][2] + box[0][0] - 320) / 2
if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180: logger.error(error)
# height = box[0][3] - box[0][1] # if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180:
logger.error(111) # # height = box[0][3] - box[0][1]
# logger.error(111)
# label = tlabel.HOSPITAL # label = tlabel.HOSPITAL
# ret, box = filter.get(label) # ret, box = filter.get(label)

17
test/test_ocr_camera.py Normal file
View File

@@ -0,0 +1,17 @@
import cv2
import time
cap = cv2.VideoCapture(20)
cap.set(cv2.CAP_PROP_AUTOFOCUS, 0)
cap.set(cv2.CAP_PROP_FOCUS, 280)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
while True:
ret, frame = cap.read()
if ret:
cv2.imshow('src', frame)
time.sleep(0.2)
if cv2.waitKey(25) & 0xFF == ord('q'):
break

8
utils.py
View File

@@ -376,7 +376,10 @@ class label_filter:
class LLM_deepseek: class LLM_deepseek:
def __init__(self): def __init__(self):
self.response = None self.response = None
self.status = False self.success_status = threading.Event()
self.error_status = threading.Event()
self.success_status.clear()
self.error_status.clear()
self.chat = '' self.chat = ''
self.client = OpenAI(api_key="sk-c2e1073883304143981a9750b97c3518", base_url="https://api.deepseek.com") self.client = OpenAI(api_key="sk-c2e1073883304143981a9750b97c3518", base_url="https://api.deepseek.com")
self.prompt = ''' self.prompt = '''
@@ -437,9 +440,10 @@ class LLM_deepseek:
temperature=0.7 temperature=0.7
) )
logger.info("llm 远程服务器正常返回 (request_thread)") logger.info("llm 远程服务器正常返回 (request_thread)")
self.success_status.set()
except: except:
logger.warning("llm 请求失败或返回异常，先检查网络连接 (request_thread)") logger.warning("llm 请求失败或返回异常，先检查网络连接 (request_thread)")
self.status = True self.error_status.set()
def request(self, _chat): def request(self, _chat):
self.chat = _chat self.chat = _chat
thread = threading.Thread(target=self.request_thread, daemon=True) thread = threading.Thread(target=self.request_thread, daemon=True)