btl143/project_main
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
91d6491f64983ca0d343b2686e2842acaa629161
project_main/subtask_8822.py

1659 lines
58 KiB
Python
Raw Normal View History

新建8822参数
2024-08-10 17:41:11 +08:00
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]
暂存 fix: 修复task多次开启问题 feat: 新增 891 参数（医院停早，红球靠外） pref: 弃用 action 类
2024-08-17 01:40:07 +08:00
if width > 130:
新建8822参数
2024-08-10 17:41:11 +08:00
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)
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)
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:
pref: 扫黑除暴动作修改
2024-08-12 14:56:20 +08:00
by_cmd.send_position_axis_z(30, 150)
新建8822参数
2024-08-10 17:41:11 +08:00
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:
pref: 扫黑除暴动作修改
2024-08-12 14:56:20 +08:00
by_cmd.send_position_axis_z(30, 150)
新建8822参数
2024-08-10 17:41:11 +08:00
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, 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:
pref: 扫黑除暴动作修改
2024-08-12 14:56:20 +08:00
by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
新建8822参数
2024-08-10 17:41:11 +08:00
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:
pref: 扫黑除暴动作修改
2024-08-12 14:56:20 +08:00
by_cmd.send_position_axis_z(30, 210)
新建8822参数
2024-08-10 17:41:11 +08:00
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)
pref: 扫黑除暴动作修改
2024-08-12 14:56:20 +08:00
# 先移动到第一个人的地方
by_cmd.send_distance_x(10, self.pos_gap1)
time.sleep(1.5)
新建8822参数
2024-08-10 17:41:11 +08:00
if self.target_person == 1:
pref: 扫黑除暴动作修改
2024-08-12 14:56:20 +08:00
# target_distance = self.pos_gap1
pass
新建8822参数
2024-08-10 17:41:11 +08:00
else:
pref: 扫黑除暴动作修改
2024-08-12 14:56:20 +08:00
# 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()
新建8822参数
2024-08-10 17:41:11 +08:00
# 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
pref: 扫黑除暴动作修改
2024-08-12 14:56:20 +08:00
新建8822参数
2024-08-10 17:41:11 +08:00
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"])
Reference in New Issue Copy Permalink