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

Compare commits

base: btl143:de8012a590d902d47afcb85ead9a8676376aa1c2
Branches Tags
btl143:master
btl143:v1
...
compare: btl143:master
Branches Tags
btl143:master
btl143:v1

15 Commits
de8012a590 ... master

Author SHA1 Message Date
bmy
5aef09bb63 国赛出发前版本 
fix: 补充 action 列表检查
pref: 修改 BBALL offset 值 (未同步到其他备份 subtask.py 中)
套用8822参数
 2024-08-17 22:18:49 +08:00
bmy
7dd0d6677e update: 更新891/8822/raw的subtask文件 2024-08-17 01:59:00 +08:00
bmy
91d6491f64 暂存 
fix: 修复task多次开启问题
feat: 新增 891 参数（医院停早，红球靠外）
pref: 弃用 action 类
 2024-08-17 01:40:07 +08:00
bmy
7ae8162da7 pref: 扫黑除暴动作修改 2024-08-12 14:56:20 +08:00
bmy
18154917a1 新建8822参数 2024-08-10 17:41:11 +08:00
bmy
7e3e280257 补交raw参数更改 2024-08-08 22:23:57 +08:00
bmy
4f2103d984 pref: subtask_raw 击打动作优化 2024-08-08 22:18:06 +08:00
bmy
25e3b60cd8 feat: 增加扫黑除暴假动作 2024-08-06 15:20:46 +08:00
bmy
2e6ce3e1f7 pref: 增加llm异步请求 2024-08-04 11:20:34 +08:00
bmy
f3bb720bed feat: 增加base64传入动作指令 
feat: 换用 deepseek 大模型
pref: 修改应急避险2停车条件
 2024-08-04 10:04:27 +08:00
bmy
55f5b13d8c fix: 修复上次提交中的错误和删除测试代码 2024-08-01 23:30:37 +08:00
bmy
d0b02a66e6 feat: 启用llm任务初版 2024-08-01 22:54:05 +08:00
bmy
e51c126f1f 新结构动作修改 2024-08-01 14:40:53 +08:00
bmy
34504d9ff9 区域赛版本 2024-07-25 23:17:27 +08:00
bmy
211b4767c1 feat: 添加新遠程頁面 2024-07-16 17:36:48 +08:00
26 changed files with 5183 additions and 552 deletions
Expand all files Collapse all files

26
action.py
View File

@@ -13,15 +13,19 @@ move = None
axis = None axis = None
cmd = None cmd = None
action_run_flag = None
cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml') cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml')
def import_obj(_bycmd): def import_obj(_run_flag,_bycmd):
global bycmd global bycmd
global move global move
global axis global axis
global cmd global cmd
global action_run_flag
bycmd = _bycmd bycmd = _bycmd
action_run_flag = _run_flag
move = move_cls() move = move_cls()
axis = axis_cls() axis = axis_cls()
cmd = cmd_cls() cmd = cmd_cls()
@@ -118,7 +122,7 @@ class cmd_cls():
else: else:
time_via = _time_via time_via = _time_via
logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}") 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 pass
time.sleep(time_via) time.sleep(time_via)
def z2(self, _speed, _pos, _time_via = -1): def z2(self, _speed, _pos, _time_via = -1):
@@ -131,7 +135,7 @@ class cmd_cls():
else: else:
time_via = _time_via time_via = _time_via
logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}") 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 pass
time.sleep(time_via) time.sleep(time_via)
def x(self, _speed, _dis, _time_via = -1): def x(self, _speed, _dis, _time_via = -1):
@@ -143,7 +147,7 @@ class cmd_cls():
else: else:
time_via = _time_via time_via = _time_via
logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}") 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 pass
time.sleep(time_via) time.sleep(time_via)
def x2(self, _speed, _pos, _time_via = -1): def x2(self, _speed, _pos, _time_via = -1):
@@ -156,30 +160,30 @@ class cmd_cls():
else: else:
time_via = _time_via time_via = _time_via
logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}") 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 pass
time.sleep(time_via) time.sleep(time_via)
def camera(self, angle): def camera(self, angle):
while bycmd.send_angle_camera(angle) == -1: while bycmd.send_angle_camera(angle) == -1 and action_run_flag.isSet():
pass pass
def claw(self, angle): def claw(self, angle):
while bycmd.send_angle_claw(angle) == -1: while bycmd.send_angle_claw(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(0.5) time.sleep(0.5)
def claw_arm(self, angle): 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 pass
time.sleep(1) time.sleep(1)
def scoop(self, angle): def scoop(self, angle):
while bycmd.send_angle_scoop(angle) == -1: while bycmd.send_angle_scoop(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(0.5) time.sleep(0.5)
def storage(self, angle): def storage(self, angle):
while bycmd.send_angle_storage(angle) == -1: while bycmd.send_angle_storage(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(0.5) time.sleep(0.5)
def zhuan(self, angle): def zhuan(self, angle):
while bycmd.send_angle_zhuan(angle) == -1: while bycmd.send_angle_zhuan(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(0.5) time.sleep(0.5)
def wait(self, _time): def wait(self, _time):

68
app.py
View File

@@ -1,4 +1,4 @@
from flask import Flask, render_template from flask import Flask, render_template, request
from flask_socketio import SocketIO from flask_socketio import SocketIO
import toml import toml
from loguru import logger from loguru import logger
@@ -10,19 +10,21 @@ import os
import time import time
import subprocess import subprocess
import signal import signal
import importlib import base64
import json
from main_upper import main_func from main_upper import main_func
server_command = [ server_command = [
{"path": "/home/evan/Workplace/project_capture/build/", "script": "./capture"}, {"path": "/home/evan/Workplace/project_capture/build/", "script": "./capture"},
{"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server.py"}, {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server.py"},
# {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server1.py"},
{"path": "/home/evan/Workplace/project_infer/yolo_server/", "script": "yolo_infer_server.py"}, {"path": "/home/evan/Workplace/project_infer/yolo_server/", "script": "yolo_infer_server.py"},
{"path": "/home/evan/Workplace/project_infer/ocr_server/", "script": "ocr_infer_server.py"},
] ]
processes = [] processes = []
time_record = None time_record = None
# 日志队列 # 日志队列
queue = Queue() queue = Queue()
# 跳过任务 干预任务调度 # 跳过任务 干预任务调度
@@ -35,7 +37,9 @@ app.config['SECRET_KEY'] = 'secret!'
socketio = SocketIO(app, allow_unsafe_werkzeug=True) socketio = SocketIO(app, allow_unsafe_werkzeug=True)
server_process = None server_process = None
# FIXME 在列表里存所有的 task_process
task_process = None task_process = None
task_run_flag = threading.Event()
class WebSocketHandler(logging.Handler): class WebSocketHandler(logging.Handler):
def emit(self, record): def emit(self, record):
@@ -49,14 +53,36 @@ logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG")
fileOptions_path = '/home/evan/Workplace/project_main' fileOptions_path = '/home/evan/Workplace/project_main'
fileOptions_list = ['cfg_args.toml','cfg_main.toml', 'cfg_subtask.toml'] fileOptions_list = ['cfg_args.toml','cfg_main.toml', 'cfg_subtask.toml']
cfg_args_path = os.path.join(fileOptions_path, 'cfg_args.toml')
cfg_move_area_path = os.path.join(fileOptions_path, 'cfg_move_area.json')
@app.route('/') @app.route('/')
def index(): def index():
return render_template('index.html') return render_template('index.html')
@app.route('/csdn') @app.route('/run')
def csdn(): def run():
return render_template('csdn.html') mode_index = request.args.get('mode')
config_args = toml.load(cfg_args_path)
config_args['lane_mode']['mode_index'] = int(mode_index)
with open(cfg_args_path, 'w') as config_file:
toml.dump(config_args, config_file)
try:
action_base64 = request.args.get('action')
decoded_bytes = base64.b64decode(action_base64)
decoded_str = decoded_bytes.decode('utf-8')
json_data = json.loads(decoded_str)
with open(cfg_move_area_path, 'w') as json_file:
json.dump(json_data, json_file)
except:
# 当该字段没有传入参数时 清空配置文件 该任务按照正常流程去做
with open(cfg_move_area_path, 'w') as json_file:
pass
return render_template('index2.html')
# @app.route('/csdn')
# def csdn():
# return render_template('csdn.html')
@socketio.on('operate') @socketio.on('operate')
def operate_handle(data): def operate_handle(data):
@@ -64,6 +90,7 @@ def operate_handle(data):
global task_process global task_process
global processes global processes
global time_record global time_record
global task_run_flag
if data['type'] == 'save_config': if data['type'] == 'save_config':
f = open(os.path.join(fileOptions_path,data['file_name']), 'w') f = open(os.path.join(fileOptions_path,data['file_name']), 'w')
ret = toml.dump(data['content'], f) ret = toml.dump(data['content'], f)
@@ -99,21 +126,23 @@ def operate_handle(data):
elif data['type'] == 'operate_task': elif data['type'] == 'operate_task':
# 任务函数 # 任务函数
if data['content'] == 'run': if data['content'] == 'run':
task_run_flag.set()
# 开启 task 进程前先关闭所有历史进程
if task_process != None: if task_process != None:
task_process.terminate() task_process.terminate()
time_record = time.perf_counter() 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() task_process.start()
logger.info("开启 task") logger.info("开启 task")
elif data['content'] == 'stop': elif data['content'] == 'stop':
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(f"任务结束 用时{time.perf_counter() - time_record}s")
logger.info("关闭 task") logger.info("关闭 task")
elif data['content'] == 'restart': elif data['content'] == 'restart':
if task_process != None: pass
task_process.terminate()
task_process = Process(target=main_func, args=(queue,skip_task_queue))
task_process.start()
elif data['type'] == 'show_server_log': elif data['type'] == 'show_server_log':
content = '' content = ''
try: try:
@@ -125,6 +154,12 @@ def operate_handle(data):
elif data['type'] == 'skip_task': elif data['type'] == 'skip_task':
logger.info(data) logger.info(data)
skip_task_queue.put(1) skip_task_queue.put(1)
# elif data['type'] == 'save_target_person':
# config_path = os.path.join(fileOptions_path, 'cfg_args.toml')
# config_args = toml.load(config_path)
# config_args['lane_mode']['mode_index'] = int(data['content'])
# with open(config_path, 'w') as config_file:
# toml.dump(config_args, config_file)
@socketio.on('connect') @socketio.on('connect')
def test_connect(): def test_connect():
@@ -134,6 +169,7 @@ def test_connect():
for item in fileOptions_list: for item in fileOptions_list:
config_data[item] = toml.load(os.path.join(fileOptions_path,item)) config_data[item] = toml.load(os.path.join(fileOptions_path,item))
socketio.emit('config_data', {'type': 'config_data', 'content': config_data}) socketio.emit('config_data', {'type': 'config_data', 'content': config_data})
socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag.isSet())})
def thread_function(): def thread_function():
global queue global queue
while True: while True:
@@ -146,6 +182,12 @@ def thread_function():
if __name__ == '__main__': if __name__ == '__main__':
config_path = os.path.join(fileOptions_path, 'cfg_args.toml')
config_args = toml.load(config_path)
config_args['lane_mode']['mode_index'] = 1
with open(config_path, 'w') as config_file:
toml.dump(config_args, config_file)
log_file = "server_processes.log" log_file = "server_processes.log"
log = open(log_file, "w") log = open(log_file, "w")
time.sleep(2) time.sleep(2)

4
cfg_args.toml
View File

@@ -1,5 +1,5 @@
[lane_mode] [lane_mode]
mode_index = 3 mode_index = 1
[task] [task]
Subtask_enable = true Subtask_enable = true
@@ -10,5 +10,5 @@ UpTower_enable = true
GetRBall_enable = true GetRBall_enable = true
PutBBall_enable = true PutBBall_enable = true
PutHanoi_enable = true PutHanoi_enable = true
MoveArea_enable = false MoveArea_enable = true
KickAss_enable = true KickAss_enable = true

8
cfg_main.toml
View File

@@ -1,19 +1,19 @@
[debug] [debug]
logger_filename = "log/file_{time}.log" logger_filename = "log/file_{time}.log"
logger_format = "{time} {level} {message}" logger_format = "[{level}] {file}:{line} <{time}> {message}"
[find_counts] [find_counts]
GetBlock_counts = 5 GetBlock_counts = 5
PutBlock_counts = 8 PutBlock_counts = 12
GetBBall_counts = 5 GetBBall_counts = 5
UpTower_counts = 3 UpTower_counts = 3
GetRBall_counts = 10 GetRBall_counts = 10
PutBBall_counts = 15 PutBBall_counts = 15
PutHanoi1_counts = 7 PutHanoi1_counts = 7
PutHanoi2_counts = 2 PutHanoi2_counts = 2
PutHanoi3_counts = 1 PutHanoi3_counts = 2
MoveArea1_counts = 6 MoveArea1_counts = 6
MoveArea2_counts = 1700 MoveArea2_counts = 10
KickAss_counts = 10 KickAss_counts = 10

0
cfg_move_area.json Normal file
View File

14
cfg_subtask.toml
View File

@@ -1,5 +1,5 @@
[get_block] [get_block]
pid_kp = 1.2 pid_kp = 1.5
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
first_block = "blue" first_block = "blue"
@@ -15,17 +15,17 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
[up_tower] [up_tower]
pid_kp = 1.1 pid_kp = 1.3
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
[get_rball] [get_rball]
pid_kp = 0.8 pid_kp = 1.0
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
[put_bball] [put_bball]
pid_kp = 1.6 pid_kp = 1.2
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
@@ -42,7 +42,7 @@ pos_gap = 160
first_target = "mp" first_target = "mp"
[put_hanoi3] [put_hanoi3]
pid_kp = 1.3 pid_kp = 1.5
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
@@ -50,10 +50,10 @@ pid_kd = 0
pid_kp = 1.2 pid_kp = 1.2
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
llm_enable = false llm_enable = true
[kick_ass] [kick_ass]
pid_kp = 0.8 pid_kp = 1.2
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
pos_gap1 = 150 pos_gap1 = 150

2
cfg_subtask.toml.6_9.bak
View File

@@ -35,7 +35,7 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
[put_hanoi2] [put_hanoi2]
pid_kp = 1.0 pid_kp = 2.0
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
pos_gap = 160 pos_gap = 160

61
cfg_subtask.toml.8822.bak Normal file
View File

@@ -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.3
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

61
cfg_subtask.toml.891.bak Normal file
View File

@@ -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

10
cfg_subtask.toml.raw.bak
View File

@@ -1,5 +1,5 @@
[get_block] [get_block]
pid_kp = 1.2 pid_kp = 1.0
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
first_block = "blue" first_block = "blue"
@@ -20,17 +20,17 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
[get_rball] [get_rball]
pid_kp = 1.2 pid_kp = 1.0
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
[put_bball] [put_bball]
pid_kp = 1.8 pid_kp = 1.5
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
[put_hanoi1] [put_hanoi1]
pid_kp = 0.7 pid_kp = 0.5
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
@@ -50,7 +50,7 @@ pid_kd = 0
pid_kp = 1.2 pid_kp = 1.2
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
llm_enable = false llm_enable = true
[kick_ass] [kick_ass]
pid_kp = 0.8 pid_kp = 0.8

30
main.py
View File

@@ -27,21 +27,21 @@ act.axis.exec()
# 向任务队列添加任务 # 向任务队列添加任务
task_queue = queue.Queue() task_queue = queue.Queue()
if cfg_main['task']['Subtask_enable'] is True: # if cfg_main['task']['Subtask_enable'] is True:
task_queue.put(sb.task("人员施救第一块", sb.get_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) # task_queue.put(sb.task("人员施救第一块", sb.get_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable']))
task_queue.put(sb.task("人员施救第二块", sb.get_block2, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) # task_queue.put(sb.task("人员施救第二块", sb.get_block2, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable']))
task_queue.put(sb.task("紧急转移", sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'])) # task_queue.put(sb.task("紧急转移", sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable']))
task_queue.put(sb.task("整装上阵", sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) # task_queue.put(sb.task("整装上阵", sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable']))
# TODO 添加一个空任务用于提前降 z 轴 # # TODO 添加一个空任务用于提前降 z 轴
task_queue.put(sb.task("通信抢修", sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable'])) # task_queue.put(sb.task("通信抢修", sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable']))
task_queue.put(sb.task("高控排险", sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable'])) # task_queue.put(sb.task("高控排险", sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable']))
task_queue.put(sb.task("派发物资", sb.put_bball, cfg_main['find_counts']['PutBBall_counts'], cfg_main['task']['GetBBall_enable'] and cfg_main['task']['PutBBall_enable'])) # task_queue.put(sb.task("派发物资", sb.put_bball, cfg_main['find_counts']['PutBBall_counts'], cfg_main['task']['GetBBall_enable'] and cfg_main['task']['PutBBall_enable']))
task_queue.put(sb.task("物资盘点一阶段", sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], enable = True)) # 无论是否进行任务，检测标识并转向都是必须进行的 # task_queue.put(sb.task("物资盘点一阶段", sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], enable = True)) # 无论是否进行任务，检测标识并转向都是必须进行的
task_queue.put(sb.task("物资盘点二阶段", sb.put_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable'])) # task_queue.put(sb.task("物资盘点二阶段", sb.put_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable']))
task_queue.put(sb.task("物资盘点三阶段", sb.put_hanoi3, cfg_main['find_counts']['PutHanoi3_counts'], enable = True)) # task_queue.put(sb.task("物资盘点三阶段", sb.put_hanoi3, cfg_main['find_counts']['PutHanoi3_counts'], enable = True))
task_queue.put(sb.task("应急避险一阶段", sb.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable'])) # task_queue.put(sb.task("应急避险一阶段", sb.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable']))
task_queue.put(sb.task("应急避险二阶段", sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable'])) # task_queue.put(sb.task("应急避险二阶段", sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable']))
task_queue.put(sb.task("扫黑除暴", sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable'])) # task_queue.put(sb.task("扫黑除暴", sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable']))
# 将任务队列传入调度模块中 # 将任务队列传入调度模块中
task_queuem_t = sb.task_queuem(task_queue) task_queuem_t = sb.task_queuem(task_queue)

38
main_upper.py
View File

@@ -6,12 +6,12 @@ import subtask as sb
import majtask as mj import majtask as mj
from by_cmd_py import by_cmd_py from by_cmd_py import by_cmd_py
import time import time
import action as act # import action as act
import logging import logging
import signal import signal
running = True # running = True
def main_func(_queue, _skip_queue): def main_func(_run_flag,_queue, _skip_queue):
if _queue != None: if _queue != None:
# 日志重定向 # 日志重定向
class Handler(logging.Handler): 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") logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG")
def signal_handler(sig, frame): def signal_handler(sig, frame):
global running _run_flag.clear()
running = False
signal.signal(signal.SIGTERM, signal_handler) signal.signal(signal.SIGTERM, signal_handler)
cmd_py_obj = by_cmd_py(_queue) cmd_py_obj = by_cmd_py(_queue)
sb.import_obj(cmd_py_obj, _skip_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') cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml')
@@ -40,13 +39,22 @@ def main_func(_queue, _skip_queue):
# 配置日志输出 # 配置日志输出
logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO") 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) cmd_py_obj.send_angle_camera(0)
act.axis.x2(140) cmd_py_obj.send_position_axis_x(1, 140)
act.axis.storage(20) cmd_py_obj.send_angle_storage(20)
act.axis.scoop(25) cmd_py_obj.send_angle_scoop(25)
act.axis.claw_arm(225) # cmd_py_obj.send_angle_claw_arm(225)
act.axis.exec() 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) logger.info(cfg_main)
@@ -59,7 +67,7 @@ def main_func(_queue, _skip_queue):
task_queue.put(sb.task("整装上阵", sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_args['task']['GetBBall_enable'])) task_queue.put(sb.task("整装上阵", sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_args['task']['GetBBall_enable']))
# # TODO 添加一个空任务用于提前降 z 轴 # # TODO 添加一个空任务用于提前降 z 轴
task_queue.put(sb.task("通信抢修", sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_args['task']['UpTower_enable'])) task_queue.put(sb.task("通信抢修", sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_args['task']['UpTower_enable']))
task_queue.put(sb.task("高控排险", sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_args['task']['GetRBall_enable'])) task_queue.put(sb.task("高空排险", sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_args['task']['GetRBall_enable']))
task_queue.put(sb.task("派发物资", sb.put_bball, cfg_main['find_counts']['PutBBall_counts'], cfg_args['task']['GetBBall_enable'] and cfg_args['task']['PutBBall_enable'])) task_queue.put(sb.task("派发物资", sb.put_bball, cfg_main['find_counts']['PutBBall_counts'], cfg_args['task']['GetBBall_enable'] and cfg_args['task']['PutBBall_enable']))
task_queue.put(sb.task("物资盘点一阶段", sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], enable = True)) # 无论是否进行任务，检测标识并转向都是必须进行的 task_queue.put(sb.task("物资盘点一阶段", sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], enable = True)) # 无论是否进行任务，检测标识并转向都是必须进行的
task_queue.put(sb.task("物资盘点二阶段", sb.put_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_args['task']['PutHanoi_enable'])) task_queue.put(sb.task("物资盘点二阶段", sb.put_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_args['task']['PutHanoi_enable']))
@@ -71,7 +79,7 @@ def main_func(_queue, _skip_queue):
task_queuem_t = sb.task_queuem(task_queue) task_queuem_t = sb.task_queuem(task_queue)
# 创建任务队列的工作线程 # 创建任务队列的工作线程
def worker_thread(): def worker_thread():
while task_queuem_t.exec(_skip_queue) is True: while task_queuem_t.exec(_skip_queue) and _run_flag.isSet():
pass pass
# 启动工作线程 # 启动工作线程
@@ -82,7 +90,7 @@ def main_func(_queue, _skip_queue):
# 创建主任务 # 创建主任务
main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象 main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
try: try:
while running: while _run_flag.isSet():
if task_queuem_t.status is sb.task_queuem_status.EXECUTING: if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
pass pass
else: else:

595
subtask.py
View File

@@ -2,19 +2,23 @@ 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 CountRecord from utils import CountRecord
import utils import utils
import toml import toml
import zmq import zmq
import time import time
import variable as var import variable as var
import action as act
import re import re
import threading import math
import ctypes import json
import json5
# import threading
# import ctypes
cfg = None cfg = None
cfg_args = None cfg_args = None
cfg_move_area = None
by_cmd = None by_cmd = None
filter = None filter = None
llm_bot = None llm_bot = None
@@ -47,9 +51,15 @@ def import_obj(_by_cmd, skip_queue):
global cfg global cfg
global cfg_args global cfg_args
global cfg_move_area
global global_skip_queue global global_skip_queue
cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置 cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.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 by_cmd = _by_cmd
global_skip_queue = skip_queue global_skip_queue = skip_queue
@@ -60,14 +70,14 @@ def import_obj(_by_cmd, skip_queue):
logger.info("subtask yolo client init") logger.info("subtask yolo client init")
# ocr socket 客户端 # ocr socket 客户端
# context1 = zmq.Context() context1 = zmq.Context()
# ocr_socket = context1.socket(zmq.REQ) ocr_socket = context1.socket(zmq.REQ)
# ocr_socket.connect("tcp://localhost:6668") ocr_socket.connect("tcp://localhost:6668")
# logger.info("subtask ocr client init") logger.info("subtask ocr client init")
filter = label_filter(socket) filter = label_filter(socket)
if cfg['move_area']['llm_enable']: if cfg['move_area']['llm_enable']:
llm_bot = LLM() llm_bot = LLM_deepseek()
def car_stop(): def car_stop():
for _ in range(3): for _ in range(3):
by_cmd.send_speed_x(0) by_cmd.send_speed_x(0)
@@ -105,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
error += offset error += offset
# if ret: # if ret:
if abs(error) <= 8: if abs(error) <= 10:
car_stop() car_stop()
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
@@ -222,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(label) # ret, box = filter.get(label)
while not ret: # while not ret:
ret, box = filter.get(label) # ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset # error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}") logger.info(f"停车后像素误差:{error}")
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准") # logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -277,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(target_label) # ret, box = filter.get(target_label)
while not ret: # while not ret:
ret, box = filter.get(target_label) # ret, box = filter.get(target_label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset # error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}") logger.info(f"停车后像素误差:{error}")
break break
return True return True
@@ -415,8 +425,7 @@ class task_queuem(task):
class get_block1(): class get_block1():
def init(self): def init(self):
var.task_speed = 15 var.task_speed = 15
act.cmd.camera(0) by_cmd.send_angle_camera(0)
act.cmd.z2(20, 60, 0)
filter.switch_camera(1) filter.switch_camera(1)
# if cfg['get_block']['first_block'] == "blue": # if cfg['get_block']['first_block'] == "blue":
# self.target_label = tlabel.BBLOCK # self.target_label = tlabel.BBLOCK
@@ -452,26 +461,26 @@ class get_block1():
calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5) calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
logger.info("抓取块") logger.info("抓取块")
by_cmd.send_position_axis_z(30, 60) by_cmd.send_position_axis_z(30, 80)
time.sleep(1) time.sleep(1.5)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 20)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(25) by_cmd.send_angle_claw(25)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 90) by_cmd.send_position_axis_z(30, 110)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(175) by_cmd.send_angle_claw_arm(175)
time.sleep(0.1) time.sleep(0.1)
by_cmd.send_position_axis_x(1, 100) by_cmd.send_position_axis_x(1, 100)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 70) by_cmd.send_position_axis_z(30, 100)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 130) by_cmd.send_position_axis_z(30, 150)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_x(1, 140) by_cmd.send_position_axis_x(1, 140)
by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw_arm(225)
@@ -546,7 +555,7 @@ class put_block():
ret, box = filter.get(tlabel.HOSPITAL) ret, box = filter.get(tlabel.HOSPITAL)
if ret > 0: if ret > 0:
width = box[0][2] - box[0][0] width = box[0][2] - box[0][0]
if width > 130: if width > 125: #FIXME maybe 125 batter
return True return True
return False return False
def exec(self): def exec(self):
@@ -557,7 +566,7 @@ class put_block():
by_cmd.send_distance_x(10, 100) by_cmd.send_distance_x(10, 100)
by_cmd.send_position_axis_z(30, 0) by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 50) # 20
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(1) time.sleep(1)
@@ -565,24 +574,24 @@ class put_block():
# 放置第二個塊 # 放置第二個塊
by_cmd.send_angle_storage(20) by_cmd.send_angle_storage(20)
by_cmd.send_position_axis_x(1, 110) 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) time.sleep(1.5)
by_cmd.send_angle_claw_arm(180) by_cmd.send_angle_claw_arm(180)
by_cmd.send_angle_claw(85) by_cmd.send_angle_claw(85)
# by_cmd.send_angle_storage(0) # by_cmd.send_angle_storage(0)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30,70) by_cmd.send_position_axis_z(30,100)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(25) by_cmd.send_angle_claw(25)
by_cmd.send_distance_x(-10, 110) by_cmd.send_distance_x(-10, 110)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 110) by_cmd.send_position_axis_z(30, 130)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw_arm(225)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 0) by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 50)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_angle_claw(45) by_cmd.send_angle_claw(45)
time.sleep(1) time.sleep(1)
@@ -592,7 +601,7 @@ class put_block():
def after(self): def after(self):
var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"]) 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 pass
time.sleep(1) time.sleep(1)
while by_cmd.send_position_axis_x(1, 0) == -1: while by_cmd.send_position_axis_x(1, 0) == -1:
@@ -640,7 +649,8 @@ class get_bball():
car_stop() car_stop()
time.sleep(0.5) time.sleep(0.5)
for _ in range(3): for _ in range(3):
calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5) by_cmd.send_position_axis_z(30, 155)
calibrate_right_new(tlabel.BBALL, offset = 10, run = True, run_speed = 5)
logger.info("抓蓝色球") logger.info("抓蓝色球")
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
@@ -655,13 +665,13 @@ class get_bball():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_distance_axis_z(30, -40) by_cmd.send_distance_axis_z(30, -40)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(80) by_cmd.send_angle_claw_arm(90)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(54) by_cmd.send_angle_claw(54)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
time.sleep(1) 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) # by_cmd.send_speed_x(4)
pass pass
@@ -671,10 +681,10 @@ class get_bball():
var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"]) 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) by_cmd.send_angle_claw(30)
time.sleep(0.5) # time.sleep(0.5)
while by_cmd.send_position_axis_z(30, 0) == -1: while by_cmd.send_position_axis_z(30, 0) == -1:
pass pass
time.sleep(1) time.sleep(0.5)
# # 任务检查间隔 # # 任务检查间隔
# time.sleep(1) # time.sleep(1)
@@ -699,20 +709,26 @@ class up_tower():
calibrate_new(tlabel.TOWER, offset = 20, run = True) calibrate_new(tlabel.TOWER, offset = 20, run = True)
time.sleep(1) time.sleep(1)
# calibrate(tlabel.TOWER, 27, False, 6) # calibrate(tlabel.TOWER, 27, False, 6)
by_cmd.send_distance_x(-10, 120) by_cmd.send_distance_x(-10, 100) # used to be 120
time.sleep(1) time.sleep(1)
# 上古參數 # 上古參數
# by_cmd.send_distance_y(-10, 50) # by_cmd.send_distance_y(-10, 50) # 80
# 6_9 模型參數 # 6_9 模型參數
by_cmd.send_distance_y(-10, 40) # by_cmd.send_distance_y(-10, 40)
# 7_12_3 模型參數 # 7_12_3 模型參數
# by_cmd.send_distance_y(-10, 50) # by_cmd.send_distance_y(-10, 50)
# time.sleep(1) # time.sleep(2)
car_stop() # car_stop()
# FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定 # FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定
# time.sleep(3) # time.sleep(3)
# by_cmd.send_speed_y(-10) # by_cmd.send_speed_y(-10)
# time.sleep(0.15) # time.sleep(0.15)
# 8822
by_cmd.send_distance_y(-10, 50)
time.sleep(0.3)
# 891
# by_cmd.send_distance_y(-10, 35)
# time.sleep(0.3)
by_cmd.send_angle_zhuan(10) by_cmd.send_angle_zhuan(10)
time.sleep(12) time.sleep(12)
@@ -758,21 +774,31 @@ class get_rball():
# 靠近塔 # 靠近塔
by_cmd.send_angle_scoop(20) by_cmd.send_angle_scoop(20)
# 上古參數 # 上古參數
# by_cmd.send_distance_y(-15, 45) # 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 參數 # 6_9 參數
by_cmd.send_distance_y(-15, 35) # by_cmd.send_distance_y(-15, 35)
time.sleep(2) # time.sleep(2)
# 7_12_3 參數 # 7_12_3 參數
# by_cmd.send_distance_y(-15, 45) # by_cmd.send_distance_y(-15, 45)
# time.sleep(2) # time.sleep(2)
car_stop() # car_stop()
# 8822 参数
by_cmd.send_distance_y(-15, 40)
time.sleep(0.5)
# 891 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(0.3)
calibrate_new(tlabel.RBALL,offset = 44, run = True) calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1) time.sleep(1)
logger.info("抓红球") 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) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 170)
time.sleep(2.5)
by_cmd.send_angle_scoop(7) by_cmd.send_angle_scoop(7)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_speed_y(15) by_cmd.send_speed_y(15)
@@ -818,7 +844,7 @@ class put_bball():
by_cmd.send_angle_storage(50) by_cmd.send_angle_storage(50)
logger.info("把球放篮筐里") logger.info("把球放篮筐里")
time.sleep(1) time.sleep(2)
# by_cmd.send_distance_y(10, 55) # by_cmd.send_distance_y(10, 55)
by_cmd.send_angle_storage(20) by_cmd.send_angle_storage(20)
# time.sleep(1) # time.sleep(1)
@@ -861,7 +887,7 @@ class put_hanoi1():
by_cmd.send_speed_omega(0) by_cmd.send_speed_omega(0)
time.sleep(0.2) 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: if utils.direction_right > utils.direction_left:
@@ -1003,7 +1029,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓大平台") logger.info("抓大平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(2) time.sleep(2)
@@ -1015,7 +1041,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(2) time.sleep(2)
@@ -1063,7 +1089,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓中平台") logger.info("抓中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(55) by_cmd.send_angle_claw(55)
time.sleep(2) time.sleep(2)
@@ -1075,7 +1101,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(55) by_cmd.send_angle_claw(55)
time.sleep(2) time.sleep(2)
@@ -1087,14 +1113,14 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
# ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) # 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: if not ret:
logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放中平台") logger.info("放中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 120) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1106,7 +1132,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 120) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1124,7 +1150,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓小平台") logger.info("抓小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(50) by_cmd.send_angle_claw(50)
time.sleep(2) time.sleep(2)
@@ -1136,7 +1162,7 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(50) by_cmd.send_angle_claw(50)
time.sleep(2) time.sleep(2)
@@ -1148,14 +1174,14 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass pass
# ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) # 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: if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放小平台") logger.info("放小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 190) # 170 by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1171,7 +1197,7 @@ class put_hanoi2():
# car_stop() # car_stop()
pass pass
else: else:
by_cmd.send_position_axis_z(30, 190) by_cmd.send_position_axis_z(30, 210)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1184,7 +1210,7 @@ class put_hanoi2():
# while True: # while True:
# pass # pass
by_cmd.send_speed_x(12) by_cmd.send_speed_x(12)
time.sleep(1.5) time.sleep(1.2)
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):
@@ -1194,11 +1220,12 @@ class put_hanoi2():
else: else:
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
# time.sleep(2) # time.sleep(2)
var.task_speed = 13
pass pass
class put_hanoi3(): class put_hanoi3():
def init(self): def init(self):
while by_cmd.send_position_axis_z(30, 130) == -1: while by_cmd.send_position_axis_z(30, 150) == -1:
pass pass
time.sleep(3) time.sleep(3)
logger.info("完成任务，爪回左侧") logger.info("完成任务，爪回左侧")
@@ -1216,14 +1243,16 @@ class put_hanoi3():
time.sleep(1) time.sleep(1)
return True return True
def exec(self): 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
pass pass
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():
@@ -1231,6 +1260,7 @@ class move_area1():
logger.info("应急避险第一阶段初始化") logger.info("应急避险第一阶段初始化")
while (by_cmd.send_angle_camera(0) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0) by_cmd.send_angle_camera(0)
filter.switch_camera(1)
def find(self): def find(self):
ret = filter.find(tlabel.SIGN) ret = filter.find(tlabel.SIGN)
if ret: if ret:
@@ -1242,7 +1272,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)
@@ -1250,33 +1280,42 @@ class move_area1():
# filter_w = (148, 560) # filter_w = (148, 560)
# filter_h = (165, 390) # filter_h = (165, 390)
if cfg_move_area == None:
counts = 0 counts = 0
while True: while True:
ocr_socket.send_string("") ocr_socket.send_string("")
resp = ocr_socket.recv_pyobj() resp = ocr_socket.recv_pyobj()
var.llm_text = '' var.llm_text = ''
counts += 1 counts += 1
if resp.get('code') == 0: if resp.get('code') == 0:
for item in resp.get('content'): for item in resp.get('content'):
if item['probability']['average'] < 0.80: if item['probability']['average'] < 0.80:
continue continue
# box = item['location'] # box = item['location']
# center_x = box['left'] + box['width'] / 2 # center_x = box['left'] + box['width'] / 2
# center_y = box['top'] + box['height'] / 2 # center_y = box['top'] + box['height'] / 2
# if center_x < filter_w[0] or center_x > filter_w[1] \ # if center_x < filter_w[0] or center_x > filter_w[1] \
# or center_y < filter_h[0] or center_y > filter_h[1]: # or center_y < filter_h[0] or center_y > filter_h[1]:
# continue # continue
var.llm_text += item['words'] var.llm_text += item['words']
break break
if counts >= 2: 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 var.skip_llm_task_flag = True
return return
logger.error(var.llm_text) else:
if len(var.llm_text) < 3: # 当有效字符大于 5 个文字时 才请求大模型
var.skip_llm_task_flag = True llm_bot.request(var.llm_text)
return else:
# 不需要文字识别 直接使用传入的参数执行 action
pass
var.task_speed = 9 # 12 var.task_speed = 9 # 12
@@ -1287,146 +1326,271 @@ class move_area1():
# 任务检查间隔 # 任务检查间隔
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
# time.sleep(1) # time.sleep(1)
by_cmd.send_angle_claw_arm(225) # by_cmd.send_angle_claw_arm(225)
pass pass
# 应急避险 第二阶段 找停车区域 # 应急避险 第二阶段 找停车区域
class move_area2(): 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): def init(self):
logger.info("应急避险第二阶段初始化") logger.info("应急避险第二阶段初始化")
self.offset = 15 self.offset = 60
self.delta_x = 0 self.delta_x = 0
self.delta_y = 0 self.delta_y = 0
self.delta_omage = 0 self.delta_omage = 0
def find(self): def find(self):
# time.sleep(0.001) if var.skip_llm_task_flag and cfg_move_area == None:
if var.skip_llm_task_flag:
return 5000 return 5000
ret, box = filter.get(tlabel.SHELTER) ret, box = filter.get(tlabel.SHELTER)
if ret: if ret:
error = (box[0][2] + box[0][0] - 320) / 2 + self.offset 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 5000
return False return False
def sub_light(self, delay_time): def add_item(self, item):
by_cmd.send_light(1) # FIXME 没有对传入参数的范围进行校验，如果出现单位错误，可能会导致无法回位的问题
time.sleep(delay_time) try:
by_cmd.send_light(0) return self.action_dict[item.get('action')](item.get('time'))
def sub_beep(self,delay_time): except:
pass
return False
def beep_seconds(self, _time):
by_cmd.send_beep(1) by_cmd.send_beep(1)
time.sleep(delay_time) time.sleep(_time * 0.7)
by_cmd.send_beep(0) by_cmd.send_beep(0)
def sub_move(self, x, y): return True
# FIXME 如果同時有 xy，是否會造成 delay 不足 def beep_counts(self, _time):
self.delta_x += x for _ in range(_time):
self.delta_y += y by_cmd.send_beep(1)
time.sleep(0.3)
if x != 0: by_cmd.send_beep(0)
delay_time = int(abs(x) * 500) time.sleep(0.2)
if x > 0: return True
by_cmd.send_distance_x(15, delay_time) def light_seconds(self, _time):
else: by_cmd.send_light(1)
by_cmd.send_distance_x(-15, delay_time) time.sleep(_time * 0.7)
elif y != 0: by_cmd.send_light(0)
delay_time = int(abs(y) * 500) return True
if y > 0: # 向左 def light_counts(self, _time):
by_cmd.send_distance_y(-15, delay_time) for _ in range(_time):
else: by_cmd.send_light(1)
by_cmd.send_distance_y(15, delay_time) time.sleep(0.3)
time.sleep(delay_time / 500) by_cmd.send_light(0)
car_stop() time.sleep(0.2)
pass return True
def sub_turn(self, angle): def beep_light_counts(self, _time):
self.delta_omage += angle for _ in range(_time):
delay_time = int(abs(angle) * 400 / 90) by_cmd.send_beep(1)
if angle < 0: by_cmd.send_light(1)
# 左转 time.sleep(0.3)
by_cmd.send_angle_omega(+55, delay_time) 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: else:
# 右转 logger.info(f"需要右旋 {left_dregee} 回正")
by_cmd.send_angle_omega(-55, delay_time) self.go_right_rotate(left_dregee)
time.sleep(delay_time / 300 * 1.5) 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): def exec(self):
var.task_speed = 0 var.task_speed = 0
if var.skip_llm_task_flag: if var.skip_llm_task_flag and cfg_move_area == None:
logger.error("ocr 识别出错 直接跳过改任务") logger.error("ocr 识别出错 直接跳过该任务")
return return
logger.info("开始寻找停车区域") logger.info("开始寻找停车区域")
car_stop() car_stop()
calibrate_new(tlabel.SHELTER, offset = 15, run = True) calibrate_new(tlabel.SHELTER, offset = 30, run = True)
time.sleep(0.5) time.sleep(0.5)
# 调用大模型 然后执行动作 if cfg_move_area == None:
try: resp = None
resp = llm_bot.get_command_json(var.llm_text) # 调用大模型 然后执行动作
logger.info(resp) try:
except: logger.info("llm 阻塞等待服务器返回中")
logger.error("大模型超时，跳过任务") start_wait_time = time.perf_counter()
return while True:
if llm_bot.success_status.isSet():
resp = llm_bot.response.choices[0].message.content
try: logger.info(f"llm 返回原数据 {resp}")
resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0]) break
if len(resp_commands) == 0: 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 return
# 进入停车区域
# by_cmd.send_speed_y(15) try:
by_cmd.send_distance_y(25, 180) json_text = re.findall("```json(.*?)```", resp, re.S)
time.sleep(1) if len(json_text) == 0:
# time.sleep(1.25) # 返回的内容不带'''json
car_stop() resp_commands = eval(resp)
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
else: else:
continue resp_commands = json5.loads(json_text[0])
logger.info(f"解析后的动作序列 {resp_commands}")
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)
for action in action_list:
self.add_item(action)
time.sleep(0.1)
time.sleep(0.5) 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 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): def nexec(self):
logger.warning("正在跳過大模型任務") logger.warning("正在跳過大模型任務")
time.sleep(2) time.sleep(2)
pass pass
def after(self): def after(self):
var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"]) 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: while by_cmd.send_angle_claw(90) == -1:
pass pass
time.sleep(2) time.sleep(2)
@@ -1442,7 +1606,7 @@ class kick_ass():
self.target_person = cfg_args['lane_mode']['mode_index'] self.target_person = cfg_args['lane_mode']['mode_index']
# by_cmd.send_angle_claw(15) # 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): def find(self):
ret = filter.find(tlabel.SIGN) ret = filter.find(tlabel.SIGN)
@@ -1457,17 +1621,32 @@ class kick_ass():
calibrate_new(tlabel.SIGN, offset = 8, run = True) calibrate_new(tlabel.SIGN, offset = 8, run = True)
by_cmd.send_angle_claw(15) by_cmd.send_angle_claw(15)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 60) # by_cmd.send_position_axis_z(30, 80)
by_cmd.send_position_axis_x(1, 130) # 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: if self.target_person == 1:
target_distance = self.pos_gap1 # target_distance = self.pos_gap1
pass
else: else:
target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10 # 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) 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}") logger.info(f"target distance {target_distance}")
time.sleep(1.5 + (self.target_person - 1) * 0.7 ) time.sleep(1.5 + (self.target_person - 1) * 0.7 )
car_stop() car_stop()
# by_cmd.send_angle_claw_arm(225) # by_cmd.send_angle_claw_arm(225)
@@ -1485,11 +1664,13 @@ class kick_ass():
ret, box = filter.get(tlabel.BASE) ret, box = filter.get(tlabel.BASE)
if ret: if ret:
error = (box[0][2] + box[0][0] - 320) / 2 error = (box[0][2] + box[0][0] - 320) / 2
by_cmd.send_speed_omega(-error * 0.8) by_cmd.send_speed_omega(-error * 0.8)
time.sleep(0.02) time.sleep(0.02)
# by_cmd.send_speed_x(25) # by_cmd.send_speed_x(25)
# time.sleep(4) # time.sleep(4)
pass pass
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):

15
subtask_69.py
View File

@@ -415,8 +415,8 @@ class task_queuem(task):
class get_block1(): class get_block1():
def init(self): def init(self):
var.task_speed = 15 var.task_speed = 15
act.cmd.camera(0) while (by_cmd.send_angle_camera(0) == -1):
act.cmd.z2(20, 60, 0) pass
filter.switch_camera(1) filter.switch_camera(1)
# if cfg['get_block']['first_block'] == "blue": # if cfg['get_block']['first_block'] == "blue":
# self.target_label = tlabel.BBLOCK # self.target_label = tlabel.BBLOCK
@@ -439,8 +439,7 @@ class get_block1():
if any(ret): if any(ret):
return True return True
return False return False
def exec(self): def exec(self):
car_stop() car_stop()
if self.target_counts[0] > self.target_counts[1]: if self.target_counts[0] > self.target_counts[1]:
@@ -771,8 +770,8 @@ class get_rball():
logger.info("抓红球") logger.info("抓红球")
# by_cmd.send_angle_scoop(15) # by_cmd.send_angle_scoop(15)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 170) by_cmd.send_position_axis_z(30, 190) #170
time.sleep(2.5) time.sleep(3.5)
by_cmd.send_angle_scoop(7) by_cmd.send_angle_scoop(7)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_speed_y(15) by_cmd.send_speed_y(15)
@@ -944,7 +943,7 @@ class put_hanoi1():
def after(self): def after(self):
# var.switch_lane_model = True # var.switch_lane_model = True
if utils.direction == tlabel.RMARK: 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"]) var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.4, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
else: else:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
pass pass
@@ -1185,6 +1184,7 @@ class put_hanoi2():
# pass # pass
by_cmd.send_speed_x(12) by_cmd.send_speed_x(12)
time.sleep(1.5) time.sleep(1.5)
var.task_speed = 10
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):
@@ -1222,6 +1222,7 @@ class put_hanoi3():
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):
var.task_speed = 0
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"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])

1677
subtask_8822.py Normal file
View File

File diff suppressed because it is too large Load Diff

1676
subtask_891.py Normal file
View File

File diff suppressed because it is too large Load Diff

595
subtask_raw.py
View File

@@ -2,19 +2,23 @@ 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 CountRecord from utils import CountRecord
import utils import utils
import toml import toml
import zmq import zmq
import time import time
import variable as var import variable as var
import action as act
import re import re
import threading import math
import ctypes import json
import json5
# import threading
# import ctypes
cfg = None cfg = None
cfg_args = None cfg_args = None
cfg_move_area = None
by_cmd = None by_cmd = None
filter = None filter = None
llm_bot = None llm_bot = None
@@ -47,9 +51,15 @@ def import_obj(_by_cmd, skip_queue):
global cfg global cfg
global cfg_args global cfg_args
global cfg_move_area
global global_skip_queue global global_skip_queue
cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置 cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.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 by_cmd = _by_cmd
global_skip_queue = skip_queue global_skip_queue = skip_queue
@@ -60,14 +70,14 @@ def import_obj(_by_cmd, skip_queue):
logger.info("subtask yolo client init") logger.info("subtask yolo client init")
# ocr socket 客户端 # ocr socket 客户端
# context1 = zmq.Context() context1 = zmq.Context()
# ocr_socket = context1.socket(zmq.REQ) ocr_socket = context1.socket(zmq.REQ)
# ocr_socket.connect("tcp://localhost:6668") ocr_socket.connect("tcp://localhost:6668")
# logger.info("subtask ocr client init") logger.info("subtask ocr client init")
filter = label_filter(socket) filter = label_filter(socket)
if cfg['move_area']['llm_enable']: if cfg['move_area']['llm_enable']:
llm_bot = LLM() llm_bot = LLM_deepseek()
def car_stop(): def car_stop():
for _ in range(3): for _ in range(3):
by_cmd.send_speed_x(0) by_cmd.send_speed_x(0)
@@ -105,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
error += offset error += offset
# if ret: # if ret:
if abs(error) <= 8: if abs(error) <= 10:
car_stop() car_stop()
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
@@ -222,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(label) # ret, box = filter.get(label)
while not ret: # while not ret:
ret, box = filter.get(label) # ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset # error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}") logger.info(f"停车后像素误差:{error}")
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准") # logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -277,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(target_label) # ret, box = filter.get(target_label)
while not ret: # while not ret:
ret, box = filter.get(target_label) # ret, box = filter.get(target_label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset # error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}") logger.info(f"停车后像素误差:{error}")
break break
return True return True
@@ -415,8 +425,7 @@ class task_queuem(task):
class get_block1(): class get_block1():
def init(self): def init(self):
var.task_speed = 15 var.task_speed = 15
act.cmd.camera(0) by_cmd.send_angle_camera(0)
act.cmd.z2(20, 60, 0)
filter.switch_camera(1) filter.switch_camera(1)
# if cfg['get_block']['first_block'] == "blue": # if cfg['get_block']['first_block'] == "blue":
# self.target_label = tlabel.BBLOCK # self.target_label = tlabel.BBLOCK
@@ -452,26 +461,26 @@ class get_block1():
calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5) calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
logger.info("抓取块") logger.info("抓取块")
by_cmd.send_position_axis_z(30, 60) by_cmd.send_position_axis_z(30, 80)
time.sleep(1) time.sleep(1.5)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 20)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(25) by_cmd.send_angle_claw(25)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 90) by_cmd.send_position_axis_z(30, 110)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(175) by_cmd.send_angle_claw_arm(175)
time.sleep(0.1) time.sleep(0.1)
by_cmd.send_position_axis_x(1, 100) by_cmd.send_position_axis_x(1, 100)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 70) by_cmd.send_position_axis_z(30, 100)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 130) by_cmd.send_position_axis_z(30, 150)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_x(1, 140) by_cmd.send_position_axis_x(1, 140)
by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw_arm(225)
@@ -546,7 +555,7 @@ class put_block():
ret, box = filter.get(tlabel.HOSPITAL) ret, box = filter.get(tlabel.HOSPITAL)
if ret > 0: if ret > 0:
width = box[0][2] - box[0][0] width = box[0][2] - box[0][0]
if width > 130: if width > 125: #135
return True return True
return False return False
def exec(self): def exec(self):
@@ -557,7 +566,7 @@ class put_block():
by_cmd.send_distance_x(10, 100) by_cmd.send_distance_x(10, 100)
by_cmd.send_position_axis_z(30, 0) by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 30) # 20
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(1) time.sleep(1)
@@ -565,24 +574,24 @@ class put_block():
# 放置第二個塊 # 放置第二個塊
by_cmd.send_angle_storage(20) by_cmd.send_angle_storage(20)
by_cmd.send_position_axis_x(1, 110) 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) time.sleep(1.5)
by_cmd.send_angle_claw_arm(180) by_cmd.send_angle_claw_arm(180)
by_cmd.send_angle_claw(85) by_cmd.send_angle_claw(85)
# by_cmd.send_angle_storage(0) # by_cmd.send_angle_storage(0)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30,70) by_cmd.send_position_axis_z(30,100)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(25) by_cmd.send_angle_claw(25)
by_cmd.send_distance_x(-10, 110) by_cmd.send_distance_x(-10, 110)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 110) by_cmd.send_position_axis_z(30, 130)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw_arm(225)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 0) by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 30)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_angle_claw(45) by_cmd.send_angle_claw(45)
time.sleep(1) time.sleep(1)
@@ -592,7 +601,7 @@ class put_block():
def after(self): def after(self):
var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"]) 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 pass
time.sleep(1) time.sleep(1)
while by_cmd.send_position_axis_x(1, 0) == -1: while by_cmd.send_position_axis_x(1, 0) == -1:
@@ -640,7 +649,8 @@ class get_bball():
car_stop() car_stop()
time.sleep(0.5) time.sleep(0.5)
for _ in range(3): for _ in range(3):
calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5) by_cmd.send_position_axis_z(30, 155)
calibrate_right_new(tlabel.BBALL, offset = 10, run = True, run_speed = 5)
logger.info("抓蓝色球") logger.info("抓蓝色球")
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
@@ -655,13 +665,13 @@ class get_bball():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_distance_axis_z(30, -40) by_cmd.send_distance_axis_z(30, -40)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(80) by_cmd.send_angle_claw_arm(90)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(54) by_cmd.send_angle_claw(54)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
time.sleep(1) 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) # by_cmd.send_speed_x(4)
pass pass
@@ -671,10 +681,10 @@ class get_bball():
var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"]) 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) by_cmd.send_angle_claw(30)
time.sleep(0.5) # time.sleep(0.5)
while by_cmd.send_position_axis_z(30, 0) == -1: while by_cmd.send_position_axis_z(30, 0) == -1:
pass pass
time.sleep(1) time.sleep(0.5)
# # 任务检查间隔 # # 任务检查间隔
# time.sleep(1) # time.sleep(1)
@@ -699,20 +709,26 @@ class up_tower():
calibrate_new(tlabel.TOWER, offset = 20, run = True) calibrate_new(tlabel.TOWER, offset = 20, run = True)
time.sleep(1) time.sleep(1)
# calibrate(tlabel.TOWER, 27, False, 6) # calibrate(tlabel.TOWER, 27, False, 6)
by_cmd.send_distance_x(-10, 120) by_cmd.send_distance_x(-10, 100)
time.sleep(1) time.sleep(1)
# 上古參數 # 上古參數
# by_cmd.send_distance_y(-10, 50) by_cmd.send_distance_y(-10, 50) # 80
time.sleep(0.5)
# 6_9 模型參數 # 6_9 模型參數
by_cmd.send_distance_y(-10, 40) # by_cmd.send_distance_y(-10, 40)
# 7_12_3 模型參數 # 7_12_3 模型參數
# by_cmd.send_distance_y(-10, 50) # by_cmd.send_distance_y(-10, 50)
# time.sleep(1) # time.sleep(2)
car_stop() # car_stop()
# FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定 # FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定
# time.sleep(3) # time.sleep(3)
# by_cmd.send_speed_y(-10) # by_cmd.send_speed_y(-10)
# time.sleep(0.15) # 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) by_cmd.send_angle_zhuan(10)
time.sleep(12) time.sleep(12)
@@ -758,21 +774,31 @@ class get_rball():
# 靠近塔 # 靠近塔
by_cmd.send_angle_scoop(20) by_cmd.send_angle_scoop(20)
# 上古參數 # 上古參數
# by_cmd.send_distance_y(-15, 45) # 50 # by_cmd.send_distance_y(-15, 50) # 50 # 70
by_cmd.send_distance_y(-15, 40) # 50 # 70
time.sleep(0.5)
# 6_9 參數 # 6_9 參數
by_cmd.send_distance_y(-15, 35) # by_cmd.send_distance_y(-15, 35)
time.sleep(2) # time.sleep(2)
# 7_12_3 參數 # 7_12_3 參數
# by_cmd.send_distance_y(-15, 45) # by_cmd.send_distance_y(-15, 45)
# time.sleep(2) # time.sleep(2)
car_stop() # car_stop()
# 8822 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(1.5)
# 891 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(0.3)
calibrate_new(tlabel.RBALL,offset = 44, run = True) calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1) time.sleep(1)
logger.info("抓红球") 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) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 170)
time.sleep(2.5)
by_cmd.send_angle_scoop(7) by_cmd.send_angle_scoop(7)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_speed_y(15) by_cmd.send_speed_y(15)
@@ -818,7 +844,7 @@ class put_bball():
by_cmd.send_angle_storage(50) by_cmd.send_angle_storage(50)
logger.info("把球放篮筐里") logger.info("把球放篮筐里")
time.sleep(1) time.sleep(2)
# by_cmd.send_distance_y(10, 55) # by_cmd.send_distance_y(10, 55)
by_cmd.send_angle_storage(20) by_cmd.send_angle_storage(20)
# time.sleep(1) # time.sleep(1)
@@ -861,7 +887,7 @@ class put_hanoi1():
by_cmd.send_speed_omega(0) by_cmd.send_speed_omega(0)
time.sleep(0.2) 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: if utils.direction_right > utils.direction_left:
@@ -944,7 +970,7 @@ class put_hanoi1():
def after(self): def after(self):
# var.switch_lane_model = True # var.switch_lane_model = True
if utils.direction == tlabel.RMARK: if utils.direction == tlabel.RMARK:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.1, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.2, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
else: else:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
pass pass
@@ -1003,7 +1029,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓大平台") logger.info("抓大平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(2) time.sleep(2)
@@ -1015,7 +1041,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(2) time.sleep(2)
@@ -1063,7 +1089,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓中平台") logger.info("抓中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(55) by_cmd.send_angle_claw(55)
time.sleep(2) time.sleep(2)
@@ -1075,7 +1101,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(55) by_cmd.send_angle_claw(55)
time.sleep(2) time.sleep(2)
@@ -1087,14 +1113,14 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
# ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) # 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: if not ret:
logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放中平台") logger.info("放中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 120) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1106,7 +1132,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 120) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1124,7 +1150,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓小平台") logger.info("抓小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(50) by_cmd.send_angle_claw(50)
time.sleep(2) time.sleep(2)
@@ -1136,7 +1162,7 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(50) by_cmd.send_angle_claw(50)
time.sleep(2) time.sleep(2)
@@ -1148,14 +1174,14 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass pass
# ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) # 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: if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放小平台") logger.info("放小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 190) # 170 by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1171,7 +1197,7 @@ class put_hanoi2():
# car_stop() # car_stop()
pass pass
else: else:
by_cmd.send_position_axis_z(30, 190) by_cmd.send_position_axis_z(30, 210)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1194,11 +1220,12 @@ class put_hanoi2():
else: else:
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
# time.sleep(2) # time.sleep(2)
var.task_speed = 13
pass pass
class put_hanoi3(): class put_hanoi3():
def init(self): def init(self):
while by_cmd.send_position_axis_z(30, 130) == -1: while by_cmd.send_position_axis_z(30, 150) == -1:
pass pass
time.sleep(3) time.sleep(3)
logger.info("完成任务，爪回左侧") logger.info("完成任务，爪回左侧")
@@ -1216,14 +1243,16 @@ class put_hanoi3():
time.sleep(1) time.sleep(1)
return True return True
def exec(self): 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
pass pass
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():
@@ -1231,6 +1260,7 @@ class move_area1():
logger.info("应急避险第一阶段初始化") logger.info("应急避险第一阶段初始化")
while (by_cmd.send_angle_camera(0) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0) by_cmd.send_angle_camera(0)
filter.switch_camera(1)
def find(self): def find(self):
ret = filter.find(tlabel.SIGN) ret = filter.find(tlabel.SIGN)
if ret: if ret:
@@ -1242,7 +1272,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)
@@ -1250,33 +1280,42 @@ class move_area1():
# filter_w = (148, 560) # filter_w = (148, 560)
# filter_h = (165, 390) # filter_h = (165, 390)
if cfg_move_area == None:
counts = 0 counts = 0
while True: while True:
ocr_socket.send_string("") ocr_socket.send_string("")
resp = ocr_socket.recv_pyobj() resp = ocr_socket.recv_pyobj()
var.llm_text = '' var.llm_text = ''
counts += 1 counts += 1
if resp.get('code') == 0: if resp.get('code') == 0:
for item in resp.get('content'): for item in resp.get('content'):
if item['probability']['average'] < 0.80: if item['probability']['average'] < 0.80:
continue continue
# box = item['location'] # box = item['location']
# center_x = box['left'] + box['width'] / 2 # center_x = box['left'] + box['width'] / 2
# center_y = box['top'] + box['height'] / 2 # center_y = box['top'] + box['height'] / 2
# if center_x < filter_w[0] or center_x > filter_w[1] \ # if center_x < filter_w[0] or center_x > filter_w[1] \
# or center_y < filter_h[0] or center_y > filter_h[1]: # or center_y < filter_h[0] or center_y > filter_h[1]:
# continue # continue
var.llm_text += item['words'] var.llm_text += item['words']
break break
if counts >= 2: 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 var.skip_llm_task_flag = True
return return
logger.error(var.llm_text) else:
if len(var.llm_text) < 3: # 当有效字符大于 5 个文字时 才请求大模型
var.skip_llm_task_flag = True llm_bot.request(var.llm_text)
return else:
# 不需要文字识别 直接使用传入的参数执行 action
pass
var.task_speed = 9 # 12 var.task_speed = 9 # 12
@@ -1287,146 +1326,271 @@ class move_area1():
# 任务检查间隔 # 任务检查间隔
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
# time.sleep(1) # time.sleep(1)
by_cmd.send_angle_claw_arm(225) # by_cmd.send_angle_claw_arm(225)
pass pass
# 应急避险 第二阶段 找停车区域 # 应急避险 第二阶段 找停车区域
class move_area2(): 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): def init(self):
logger.info("应急避险第二阶段初始化") logger.info("应急避险第二阶段初始化")
self.offset = 15 self.offset = 60
self.delta_x = 0 self.delta_x = 0
self.delta_y = 0 self.delta_y = 0
self.delta_omage = 0 self.delta_omage = 0
def find(self): def find(self):
# time.sleep(0.001) if var.skip_llm_task_flag and cfg_move_area == None:
if var.skip_llm_task_flag:
return 5000 return 5000
ret, box = filter.get(tlabel.SHELTER) ret, box = filter.get(tlabel.SHELTER)
if ret: if ret:
error = (box[0][2] + box[0][0] - 320) / 2 + self.offset 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 5000
return False return False
def sub_light(self, delay_time): def add_item(self, item):
by_cmd.send_light(1) # FIXME 没有对传入参数的范围进行校验，如果出现单位错误，可能会导致无法回位的问题
time.sleep(delay_time) try:
by_cmd.send_light(0) return self.action_dict[item.get('action')](item.get('time'))
def sub_beep(self,delay_time): except:
pass
return False
def beep_seconds(self, _time):
by_cmd.send_beep(1) by_cmd.send_beep(1)
time.sleep(delay_time) time.sleep(_time * 0.7)
by_cmd.send_beep(0) by_cmd.send_beep(0)
def sub_move(self, x, y): return True
# FIXME 如果同時有 xy，是否會造成 delay 不足 def beep_counts(self, _time):
self.delta_x += x for _ in range(_time):
self.delta_y += y by_cmd.send_beep(1)
time.sleep(0.3)
if x != 0: by_cmd.send_beep(0)
delay_time = int(abs(x) * 500) time.sleep(0.2)
if x > 0: return True
by_cmd.send_distance_x(15, delay_time) def light_seconds(self, _time):
else: by_cmd.send_light(1)
by_cmd.send_distance_x(-15, delay_time) time.sleep(_time * 0.7)
elif y != 0: by_cmd.send_light(0)
delay_time = int(abs(y) * 500) return True
if y > 0: # 向左 def light_counts(self, _time):
by_cmd.send_distance_y(-15, delay_time) for _ in range(_time):
else: by_cmd.send_light(1)
by_cmd.send_distance_y(15, delay_time) time.sleep(0.3)
time.sleep(delay_time / 500) by_cmd.send_light(0)
car_stop() time.sleep(0.2)
pass return True
def sub_turn(self, angle): def beep_light_counts(self, _time):
self.delta_omage += angle for _ in range(_time):
delay_time = int(abs(angle) * 400 / 90) by_cmd.send_beep(1)
if angle < 0: by_cmd.send_light(1)
# 左转 time.sleep(0.3)
by_cmd.send_angle_omega(+55, delay_time) 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: else:
# 右转 logger.info(f"需要右旋 {left_dregee} 回正")
by_cmd.send_angle_omega(-55, delay_time) self.go_right_rotate(left_dregee)
time.sleep(delay_time / 300 * 1.5) 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): def exec(self):
var.task_speed = 0 var.task_speed = 0
if var.skip_llm_task_flag: if var.skip_llm_task_flag and cfg_move_area == None:
logger.error("ocr 识别出错 直接跳过改任务") logger.error("ocr 识别出错 直接跳过该任务")
return return
logger.info("开始寻找停车区域") logger.info("开始寻找停车区域")
car_stop() car_stop()
calibrate_new(tlabel.SHELTER, offset = 15, run = True) calibrate_new(tlabel.SHELTER, offset = 30, run = True)
time.sleep(0.5) time.sleep(0.5)
# 调用大模型 然后执行动作 if cfg_move_area == None:
try: resp = None
resp = llm_bot.get_command_json(var.llm_text) # 调用大模型 然后执行动作
logger.info(resp) try:
except: logger.info("llm 阻塞等待服务器返回中")
logger.error("大模型超时，跳过任务") start_wait_time = time.perf_counter()
return while True:
if llm_bot.success_status.isSet():
resp = llm_bot.response.choices[0].message.content
try: logger.info(f"llm 返回原数据 {resp}")
resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0]) break
if len(resp_commands) == 0: 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 return
# 进入停车区域
# by_cmd.send_speed_y(15) try:
by_cmd.send_distance_y(25, 180) json_text = re.findall("```json(.*?)```", resp, re.S)
time.sleep(1) if len(json_text) == 0:
# time.sleep(1.25) # 返回的内容不带'''json
car_stop() resp_commands = eval(resp)
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
else: else:
continue resp_commands = json5.loads(json_text[0])
logger.info(f"解析后的动作序列 {resp_commands}")
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)
for action in action_list:
self.add_item(action)
time.sleep(0.1)
time.sleep(0.5) 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 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): def nexec(self):
logger.warning("正在跳過大模型任務") logger.warning("正在跳過大模型任務")
time.sleep(2) time.sleep(2)
pass pass
def after(self): def after(self):
var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"]) 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: while by_cmd.send_angle_claw(90) == -1:
pass pass
time.sleep(2) time.sleep(2)
@@ -1442,7 +1606,7 @@ class kick_ass():
self.target_person = cfg_args['lane_mode']['mode_index'] self.target_person = cfg_args['lane_mode']['mode_index']
# by_cmd.send_angle_claw(15) # 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): def find(self):
ret = filter.find(tlabel.SIGN) ret = filter.find(tlabel.SIGN)
@@ -1457,17 +1621,32 @@ class kick_ass():
calibrate_new(tlabel.SIGN, offset = 8, run = True) calibrate_new(tlabel.SIGN, offset = 8, run = True)
by_cmd.send_angle_claw(15) by_cmd.send_angle_claw(15)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_z(30, 60) # by_cmd.send_position_axis_z(30, 80)
by_cmd.send_position_axis_x(1, 130) # 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: if self.target_person == 1:
target_distance = self.pos_gap1 # target_distance = self.pos_gap1
pass
else: else:
target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10 # 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) 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}") logger.info(f"target distance {target_distance}")
time.sleep(1.5 + (self.target_person - 1) * 0.7 ) time.sleep(1.5 + (self.target_person - 1) * 0.7 )
car_stop() car_stop()
# by_cmd.send_angle_claw_arm(225) # by_cmd.send_angle_claw_arm(225)
@@ -1485,11 +1664,13 @@ class kick_ass():
ret, box = filter.get(tlabel.BASE) ret, box = filter.get(tlabel.BASE)
if ret: if ret:
error = (box[0][2] + box[0][0] - 320) / 2 error = (box[0][2] + box[0][0] - 320) / 2
by_cmd.send_speed_omega(-error * 0.8) by_cmd.send_speed_omega(-error * 0.8)
time.sleep(0.02) time.sleep(0.02)
# by_cmd.send_speed_x(25) # by_cmd.send_speed_x(25)
# time.sleep(4) # time.sleep(4)
pass pass
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):

21
templates/index.html
View File

@@ -109,7 +109,7 @@
</el-col> </el-col>
<el-col :xs="24" :sm="8"> <el-col :xs="24" :sm="8">
<el-button @click="skipTask" type="success" style="width: 100%"> <el-button @click="skipTask" type="success" style="width: 100%">
强制跳转 关闭
</el-button> </el-button>
</el-col> </el-col>
</el-row> </el-row>
@@ -128,12 +128,16 @@
</el-button> --> </el-button> -->
<el-form v-if="showConfigForm && config" class="config-form"> <el-form v-if="showConfigForm && config" class="config-form">
<el-button @click="saveConfig" type="primary" style="margin-top: 20px;">保存配置</el-button>
<el-button v-if="showConfigForm" @click="toggleConfigForm" type="primary" style="margin-bottom: 20px;">
{{ showConfigForm ? '关闭' : '打开' }} 配置
</el-button>
<div v-for="(section, sectionName) in config" :key="sectionName"> <div v-for="(section, sectionName) in config" :key="sectionName">
<h3>{{ sectionName }}</h3> <h3>{{ sectionName }}</h3>
<el-row :gutter="20"> <el-row :gutter="20">
<el-col v-for="(value, key) in section" :key="key" :xs="24" :sm="12" :md="8" :lg="6"> <el-col v-for="(value, key) in section" :key="key" :xs="24" :sm="12" :md="8" :lg="6">
<el-form-item :label="key"> <el-form-item :label="key">
<el-select v-if="sectionName === 'kick_ass'" v-model="config[sectionName][key]"> <el-select v-if="sectionName === 'lane_mode'" v-model="config[sectionName][key]">
<el-option <el-option
v-for="item in target_person_options" v-for="item in target_person_options"
:key="item.value" :key="item.value"
@@ -162,10 +166,7 @@
</el-col> </el-col>
</el-row> </el-row>
</div> </div>
<el-button @click="saveConfig" type="primary" style="margin-top: 20px;">保存配置</el-button>
<el-button v-if="showConfigForm" @click="toggleConfigForm" type="primary" style="margin-bottom: 20px;">
{{ showConfigForm ? '关闭' : '打开' }} 配置
</el-button>
</el-form> </el-form>
<h2 class="section-title">Log Display</h2> <h2 class="section-title">Log Display</h2>
@@ -197,16 +198,16 @@
timer: null, timer: null,
target_person_options: [{ target_person_options: [{
value: 1, value: 1,
label: '模式 1' label: '6_9'
}, { }, {
value: 2, value: 2,
label: '模式 2' label: '7_10_2'
}, { }, {
value: 3, value: 3,
label: '模式 3' label: '7_14_2'
}, { }, {
value: 4, value: 4,
label: '模式 4' label: '7_15_4'
}], }],
}, },
methods: { methods: {

139
templates/index1.html Normal file
View File

@@ -0,0 +1,139 @@
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>btl143</title>
<link rel="stylesheet" href="static/index.css">
<script src="static/vue.js"></script>
<script src="static/index.js"></script>
<script src="static/socket.io.js"></script>
<style>
body {
font-family: Arial, sans-serif;
margin: 0;
padding: 0;
background-color: #f5f7fa;
}
.app-container {
padding: 20px;
max-width: 1200px;
margin: 0 auto;
}
.button-group {
margin-bottom: 20px;
}
.full-screen-background {
background-color: #f0f0f0;
position: fixed;
top: 60px; /* Adjust based on button height and margin */
left: 0;
right: 0;
bottom: 0;
z-index: -1;
}
.counter-display {
padding: 20px;
text-align: center;
position: fixed;
top: 80px; /* Adjust based on your requirements */
left: 50%;
transform: translateX(-50%);
z-index: 1;
}
</style>
</head>
<body>
<div id="app">
<el-container class="app-container">
<el-main>
<h1 class="section-title">btl143 upper</h1>
<el-row :gutter="20" class="button-group">
<el-col :xs="24" :sm="8">
<el-button
@click="toggleTask"
:type="taskActive ? 'success' : 'danger'"
style="width: 100%">
{{ taskActive ? `开启 task ${counter}` : `关闭 task ${counter}` }}
</el-button>
</el-col>
</el-row>
<div class="full-screen-background" @click="incrementCounter">
<div class="counter-display">
</div>
</div>
</el-main>
</el-container>
</div>
<script>
new Vue({
el: '#app',
data: {
taskActive: true,
counter: 1
},
methods: {
toggleTask() {
if (this.taskActive) {
this.socket.emit('operate', {type: 'operate_task', content: 'run'});
}
else {
this.socket.emit('operate', {type: 'operate_task', content: 'stop'});
}
this.taskActive = !this.taskActive;
},
incrementCounter() {
this.counter += 1;
if (this.counter == 5) {
this.counter = 1;
}
this.socket.emit('operate', {type: 'save_target_person', content: this.counter});
},
startServer() {
this.socket.emit('operate', {type: 'operate_server', content: 'run'});
},
stopServer() {
this.socket.emit('operate', {type: 'operate_server', content: 'stop'});
},
restartServer() {
this.socket.emit('operate', {type: 'operate_server', content: 'restart'});
},
startTask() {
this.socket.emit('operate', {type: 'operate_task', content: 'run'});
},
stopTask() {
this.socket.emit('operate', {type: 'operate_task', content: 'stop'});
},
restartTask() {
this.socket.emit('operate', {type: 'operate_task', content: 'restart'});
},
skipTask() {
this.socket.emit('operate', { type: 'skip_task', content: '' });
}
},
mounted() {
this.socket = io('http://' + document.domain + ':5001');
this.socket.on('connect', () => {
console.log('Connected to server');
});
this.socket.on('task_status', (data) => {
if (data.content == 0) {
this.taskActive = true
} else {
this.taskActive = false
}
});
}
});
</script>
</body>
</html>

128
templates/index2.html Normal file
View File

@@ -0,0 +1,128 @@
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>btl143</title>
<link rel="stylesheet" href="static/index.css">
<script src="static/vue.js"></script>
<script src="static/index.js"></script>
<script src="static/socket.io.js"></script>
<style>
body {
font-family: Arial, sans-serif;
margin: 0;
padding: 0;
background-color: #f5f7fa;
}
.app-container {
padding: 20px;
max-width: 1200px;
margin: 0 auto;
}
.button-group {
margin-bottom: 20px;
}
.full-screen-background {
background-color: #f0f0f0;
position: fixed;
top: 60px; /* Adjust based on button height and margin */
left: 0;
right: 0;
bottom: 0;
z-index: -1;
}
.counter-display {
padding: 20px;
text-align: center;
position: fixed;
top: 80px; /* Adjust based on your requirements */
left: 50%;
transform: translateX(-50%);
z-index: 1;
}
</style>
</head>
<body>
<div id="app">
<el-container class="app-container">
<el-main>
<h1 class="section-title">btl143 upper</h1>
<el-row :gutter="20" class="button-group">
<el-col :xs="24" :sm="8">
<el-button
@click="toggleTask"
:type="taskActive ? 'success' : 'danger'"
style="width: 100%">
{{ taskActive ? `开启 task` : `关闭 task` }}
</el-button>
</el-col>
</el-row>
</el-main>
</el-container>
</div>
<script>
new Vue({
el: '#app',
data: {
taskActive: true,
counter: 1
},
methods: {
toggleTask() {
if (this.taskActive) {
this.socket.emit('operate', {type: 'operate_task', content: 'run'});
}
else {
this.socket.emit('operate', {type: 'operate_task', content: 'stop'});
}
this.taskActive = !this.taskActive;
},
startServer() {
this.socket.emit('operate', {type: 'operate_server', content: 'run'});
},
stopServer() {
this.socket.emit('operate', {type: 'operate_server', content: 'stop'});
},
restartServer() {
this.socket.emit('operate', {type: 'operate_server', content: 'restart'});
},
startTask() {
this.socket.emit('operate', {type: 'operate_task', content: 'run'});
},
stopTask() {
this.socket.emit('operate', {type: 'operate_task', content: 'stop'});
},
restartTask() {
this.socket.emit('operate', {type: 'operate_task', content: 'restart'});
},
skipTask() {
this.socket.emit('operate', { type: 'skip_task', content: '' });
}
},
mounted() {
this.socket = io('http://' + document.domain + ':5001');
this.socket.on('connect', () => {
console.log('Connected to server');
});
this.socket.on('task_status', (data) => {
if (data.content == 0) {
this.taskActive = true
} else {
this.taskActive = false
}
});
}
});
</script>
</body>
</html>

241
test/test_action.py Normal file
View File

@@ -0,0 +1,241 @@
import os
import sys
import math
parent_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(parent_dir)
from by_cmd_py import by_cmd_py
import time
import zmq
import numpy as np
context = zmq.Context()
socket = context.socket(zmq.REQ)
socket.connect("tcp://localhost:6666")
def car_stop():
for _ in range(3):
cmd_py_obj.send_speed_x(0)
time.sleep(0.2)
cmd_py_obj.send_speed_y(0)
time.sleep(0.2)
cmd_py_obj.send_speed_omega(0)
class LLM_Action:
def __init__(self,cmd_py_obj):
self.by_cmd = cmd_py_obj
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 __call__(self, item):
try:
return self.action_dict[item.get('action')](item.get('time'))
except:
pass
return False
def beep_seconds(self, _time):
self.by_cmd.send_beep(1)
time.sleep(_time * 0.7)
self.by_cmd.send_beep(0)
return True
def beep_counts(self, _time):
for _ in range(_time):
self.by_cmd.send_beep(1)
time.sleep(0.3)
self.by_cmd.send_beep(0)
time.sleep(0.2)
return True
def light_seconds(self, _time):
self.by_cmd.send_light(1)
time.sleep(_time * 0.7)
self.by_cmd.send_light(0)
return True
def light_counts(self, _time):
for _ in range(_time):
self.by_cmd.send_light(1)
time.sleep(0.3)
self.by_cmd.send_light(0)
time.sleep(0.2)
return True
def beep_light_counts(self, _time):
for _ in range(_time):
self.by_cmd.send_beep(1)
self.by_cmd.send_light(1)
time.sleep(0.3)
self.by_cmd.send_beep(0)
self.by_cmd.send_light(0)
time.sleep(0.2)
return True
def beep_light_seconds(self, _time):
self.by_cmd.send_beep(1)
self.by_cmd.send_light(1)
time.sleep(_time * 0.3)
self.by_cmd.send_beep(0)
self.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
print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
speed_time = int(abs(_time) * 750)
self.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
print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
speed_time = int(abs(_time) * 750)
self.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
print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
speed_time = int(abs(_time) * 750)
self.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
print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
speed_time = int(abs(_time) * 750)
self.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
print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
speed_time_x = int(abs(_dis_x) * 750)
speed_time_y = int(abs(_dis_y) * 750)
self.by_cmd.send_distance_x(10 * direct_x, speed_time_x)
self.by_cmd.send_distance_y(10 * direct_y, speed_time_y)
time.sleep(max(speed_time_x, speed_time_y) / 100) #FIXME 除以 100 是否正确
return True
def go_left_rotate(self, _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")
self.sum_rotate_angle -= _time
speed_time = int(abs(_time) * 4.35)
self.by_cmd.send_angle_omega(50, speed_time)
time.sleep(speed_time / 200 + 0.5)
# time.sleep(speed_time / _time / 2)
return True
def go_right_rotate(self, _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")
self.sum_rotate_angle += _time
speed_time = int(abs(_time) * 4.35)
self.by_cmd.send_angle_omega(-50, speed_time)
time.sleep(speed_time / 200 + 0.5)
# time.sleep(speed_time / _time / 2)
return True
def go_sleep(self, _time):
time.sleep(_time*0.7)
return True
def reset(self):
print(f"开始复位：[当前位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)}]")
# 先复位角度
# if self.sum_rotate_angle > 0:
# self.sum_rotate_angle = self.sum_rotate_angle % 360
# else:
# self.sum_rotate_angle = -(abs(self.sum_rotate_angle) % 360)
# if self.sum_rotate_angle > 0:
# # 采用左转回正
# self.go_left_rotate(self.sum_rotate_angle)
# # speed_time = int(abs(self.sum_rotate_angle) * 7.25)
# # self.by_cmd.send_angle_omega(30, speed_time)
# pass
# else:
# # 采用右转回正
# self.go_right_rotate(abs(self.sum_rotate_angle))
# # speed_time = int(abs(self.sum_rotate_angle) * 7.25)
# # self.by_cmd.send_angle_omega(-30, speed_time)
left_dregee = math.degrees(self.abs_w % (2 * math.pi)) #归一化角度到 0-2pi
if math.sin(self.abs_w) < 0:
print(f"需要左旋 {360.0 - left_dregee} 回正")
self.go_left_rotate(360.0 - left_dregee)
else:
print(f"需要右旋 {left_dregee} 回正")
self.go_right_rotate(left_dregee)
time.sleep(0.1)
self.go_shift(self.abs_x * -1.0, self.abs_y * -1.0 - 0.6) # 左移 0.6m 回到赛道
# # 再回正 x 轴
# if self.front_time > self.back_time:
# # 采用后退回正
# speed_time = self.front_time - self.back_time
# self.by_cmd.send_distance_x(-10, speed_time)
# else:
# speed_time = self.back_time - self.front_time
# self.by_cmd.send_distance_x(10, speed_time)
# time.sleep(speed_time / 100)
# time.sleep(0.1)
# # 最后回正 y 轴
# speed_time = self.left_time - self.right_time
# if speed_time < 0:
# speed_time = 4500 + abs(speed_time)
# else:
# speed_time = 4500 - speed_time
# self.by_cmd.send_distance_y(-10, speed_time / 15 + 100)
# print(speed_time * 1e-3 * 0.9)
# time.sleep(speed_time * 1e-3 * 0.9)
print(f"回正后最终位置： ({self.abs_y:.2f}, {self.abs_x:.2f}), 角度： {math.degrees(self.abs_w % (2 * math.pi))}")
if __name__ == "__main__":
cmd_py_obj = by_cmd_py()
# cmd_py_obj.send_angle_omega(-30, 20 * 7.25)
# time.sleep(20 * 7.25 / 20 / 2)
llm_act = LLM_Action(cmd_py_obj)
action_list = [{"index":0,"action":"go_left_rotate","time":270},{"index":1,"action":"go_back","time":0.2},{"index":2,"action":"go_left","time":0.2},{"index":2,"action":"beep_counts","time":3}]
cmd_py_obj.send_distance_y(10, 450)
time.sleep((450 * 5 / 1000) + 0.5)
# time.sleep(15 * 300 * 1e-3 * 0.7)
# car_stop()
for action in action_list:
llm_act(action)
time.sleep(0.1)
pass
time.sleep(1)
llm_act.reset()
# car_stop()

10
test/test_filter.py
View File

@@ -16,7 +16,7 @@ socket.connect("tcp://localhost:6667")
logger.info("subtask yolo client init") logger.info("subtask yolo client init")
filter = label_filter(socket) filter = label_filter(socket)
filter.switch_camera(2) filter.switch_camera(1)
# find_counts = 0 # find_counts = 0
@@ -25,10 +25,14 @@ filter.switch_camera(2)
# label = tlabel.TPLATFORM # label = tlabel.TPLATFORM
while True: while True:
time.sleep(0.2) time.sleep(0.2)
ret, box = filter.get(tlabel.BASE) ret, box = filter.get(tlabel.HOSPITAL)
if ret: if ret:
width = box[0][2] - box[0][0]
error = (box[0][2] + box[0][0] - 320) / 2 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)
# label = tlabel.HOSPITAL # label = tlabel.HOSPITAL
# ret, box = filter.get(label) # ret, box = filter.get(label)

52
test/test_llm.py
View File

@@ -13,18 +13,48 @@ class LLM:
def __init__(self): def __init__(self):
self.model = 'ernie-3.5' self.model = 'ernie-3.5'
self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。 self.prompt = '''
严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据''' 你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 JSON 结果。请注意，只能使用以下指定的动作，不能创造新的动作：
self.prompt += '''正确的示例如下： 允许的动作及其对应格式如下：
向左移 0.1m, 向左转弯 85 度 [{'func': 'move', 'x': 0, 'y': 0.1},{'func': 'turn','angle': -85}], - 向左移：{"index":N,"action":"go_left","time":T}
向右移 0.2m, 向前 0.1m [{'func': 'move', 'x': 0, 'y': -0.2},{'func': 'move', 'x': 0.1, 'y': 0}], - 向右移：{"index":N,"action":"go_right","time":T}
向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}], - 向前移：{"index":N,"action":"go_front","time":T}
原地左转 38 度 [{'func': 'turn','angle': -38}], - 向后移：{"index":N,"action":"go_back","time":T}
蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}] - 向左转：{"index":N,"action":"go_left_rotate","time":T}
发光或者照亮 5 秒 [{'func': 'light', 'time': 5}] - 向右转：{"index":N,"action":"go_right_rotate","time":T}
向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}], - 蜂鸣器发声：{"index":N,"action":"beep_seconds","time":T}
向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}], - 蜂鸣器发声次数：{"index":N,"action":"beep_counts","time":T}
- 发光或者照亮：{"index":N,"action":"light_seconds","time":T}
- 发光次数或者闪烁次数：{"index":N,"action":"light_counts","time":T}
- 发光并伴随蜂鸣器：{"index":N,"action":"beep_light_counts","time":T}
- 等待{"index":N,"action":"go_sleep","time":T}
示例输入输出如下：
输入：向左移 0.1m, 向左转弯 85 度
输出：[{"index":0,"action":"go_left","time":0.1},{"index":1,"action":"go_left_rotate","time":85}]
输入：向右移 0.2m, 向前 0.1m
输出：[{"index":0,"action":"go_right","time":0.2},{"index":1,"action":"go_front","time":0.1}]
输入：向右转 90 度，向右移 0.1m
输出：[{"index":0,"action":"go_right_rotate","time":90},{"index":1,"action":"go_right","time":0.1}]
输入：原地左转 38 度
输出：[{"index":0,"action":"go_left_rotate","time":38}]
输入：蜂鸣器发声 5 秒
输出：[{"index":0,"action":"beep_seconds","time":5}]
输入：发光或者照亮 5 秒
输出：[{"index":0,"action":"light_seconds","time":5}]
输入：向右走 30cm, 照亮 2s
输出：[{"index":0,"action":"go_right","time":0.3},{"index":1,"action":"light_seconds","time":2}]
输入：向左移 0.2m, 向后 0.1m
输出：[{"index":0,"action":"go_left","time":0.2},{"index":1,"action":"go_back","time":0.1}]
输入：鸣叫 3 声
输出：[{"index":0,"action":"beep_counts","time":3}]
输入：前行零点五米
输出：[{"index":0,"action":"go_front","time":0.5}]
输入：闪烁灯光 1 次并伴有蜂鸣器
输出：[{"index":0,"action":"beep_light_counts","time": 1}]
输入：灯光闪烁 3 次同时蜂鸣器也叫 3 次
输出：[{"index":0,"action":"beep_light_counts","time": 3}]
''' '''
self.prompt += '''请根据上面的示例，解析该任务文本，并返回相应的 JSON 字段。确保 JSON 中包含了键 index action 和 time 以及相应的值'''
self.messages = [] self.messages = []
self.resp = None self.resp = None
self.reset() self.reset()

8
test/test_ocr.py
View File

@@ -17,13 +17,7 @@ while True:
if resp.get('code') == 0: if resp.get('code') == 0:
text = '' text = ''
for item in resp.get('content'): for item in resp.get('content'):
if item['probability']['average'] < 0.90: 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 continue
text += item['words'] text += item['words']
print(text) print(text)

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

239
utils.py
View File

@@ -2,6 +2,7 @@
from enum import Enum from enum import Enum
import numpy as np import numpy as np
import erniebot import erniebot
from openai import OpenAI
from simple_pid import PID from simple_pid import PID
from loguru import logger from loguru import logger
import threading import threading
@@ -284,7 +285,11 @@ class label_filter:
else: else:
target_bool = False target_bool = False
target_box = None 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, :] boxes = results[expect_boxes, :]
# 在此处过滤 # 在此处过滤
if len(boxes) != 0: if len(boxes) != 0:
@@ -372,55 +377,235 @@ class label_filter:
error = (boxes[center_x_index][4] + boxes[center_x_index][2] - self.img_size[0]) / 2 error = (boxes[center_x_index][4] + boxes[center_x_index][2] - self.img_size[0]) / 2
return (True, error) return (True, error)
return (False, 0) return (False, 0)
class LLM_deepseek:
def __init__(self):
self.response = None
self.success_status = threading.Event()
self.error_status = threading.Event()
self.success_status.clear()
self.error_status.clear()
self.chat = ''
self.client = OpenAI(api_key="sk-c2e1073883304143981a9750b97c3518", base_url="https://api.deepseek.com")
self.prompt = '''
你是一个机器人动作规划者，请把我的话翻译成机器人动作规划并生成对应的 JSON 结果。请注意，只能使用以下指定的动作，不能创造新的动作：
允许的动作及其对应格式如下：
[{'properties': {'index': {'title': 'Index', 'type': 'integer'}, 'action': {'title': 'Action', 'type': 'string'}, 'time': {'title': 'Time', 'type': 'number'}}, 'required': ['index', 'action', 'time'], 'title': 'Action', 'type': 'object'}]
我不允许你自我创造出新的 action,action 字段仅仅包括以下内容：
go_right 向右移动
go_left 向左移动
go_front 向前移动
go_back 向后移动
go_left_rotate 向左旋转
go_right_rotate 向右旋转
beep_seconds 蜂鸣器鸣叫的时间
beep_counts 蜂鸣器鸣叫的次数
light_seconds 灯光发光的时间
light_counts 灯光闪烁的次数
beep_light_counts 灯光和蜂鸣器一起闪烁的次数
go_sleep 什么都不做
我的话和你的回复示例如下：
我的话：向左移 0.1m, 向左转弯 85 度
你的回复：[{"index":0,"action":"go_left","time":0.1},{"index":1,"action":"go_left_rotate","time":85}]
我的话：向右移 0.2m, 向前 0.1m
你的回复：[{"index":0,"action":"go_right","time":0.2},{"index":1,"action":"go_front","time":0.1}]
我的话：向右转 90 度，向右移 0.1m
你的回复：[{"index":0,"action":"go_right_rotate","time":90},{"index":1,"action":"go_right","time":0.1}]
我的话：原地左转 38 度
你的回复：[{"index":0,"action":"go_left_rotate","time":38}]
我的话：蜂鸣器发声 5 秒
你的回复：[{"index":0,"action":"beep_seconds","time":5}]
我的话：发光或者照亮 5 秒
你的回复：[{"index":0,"action":"light_seconds","time":5}]
我的话：向右走 30cm，照亮 2s
你的回复：[{"index":0,"action":"go_right","time":0.3},{"index":1,"action":"light_seconds","time":2}]
我的话：向左移 0.2m, 向后 0.1m
你的回复：[{"index":0,"action":"go_left","time":0.2},{"index":1,"action":"go_back","time":0.1}]
我的话：鸣叫 3 声
你的回复：[{"index":0,"action":"beep_counts","time":3}]
我的话：前行零点五米
你的回复：[{"index":0,"action":"go_front","time":0.5}]
我的话：闪烁灯光 1 次并伴有蜂鸣器
你的回复：[{"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):
logger.info("llm 请求远程服务器中 (request_thread)")
try:
self.response = self.client.chat.completions.create(
model="deepseek-chat",
messages=[
{"role": "system", "content": self.prompt},
{"role": "user", "content": '我的话如下：' + self.chat},
],
stream=False,
temperature=0.7
)
logger.info("llm 远程服务器正常返回 (request_thread)")
self.success_status.set()
except:
logger.warning("llm 请求失败或返回异常，先检查网络连接 (request_thread)")
self.error_status.set()
def request(self, _chat):
self.chat = _chat
thread = threading.Thread(target=self.request_thread, daemon=True)
thread.start()
logger.info("llm 开启请求线程")
def get_command_json(self,chat = ''):
# response = self.client.chat.completions.create(
# model="deepseek-chat",
# messages=[
# {"role": "system", "content": self.prompt},
# {"role": "user", "content": '我的话如下：' + chat},
# ],
# stream=False,
# temperature=0.7
# )
logger.info("llm 阻塞等待服务器返回中")
while not self.status: # FIXME 阻塞等待是否合适
pass
logger.info("llm 收到返回")
return self.response.choices[0].message.content
class LLM: class LLM:
def __init__(self): def __init__(self):
self.init_done_flag = False self.init_done_flag = False
erniebot.api_type = "qianfan" erniebot.api_type = "qianfan"
erniebot.ak = "jReawMtWhPu0wrxN9Rp1MzZX" erniebot.ak = "jReawMtWhPu0wrxN9Rp1MzZX"
erniebot.sk = "eowS1BqsNgD2i0C9xNnHUVOSNuAzVTh6" erniebot.sk = "eowS1BqsNgD2i0C9xNnHUVOSNuAzVTh6"
self.model = 'ernie-3.5' self.model = 'ernie-lite'
self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。 # self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。
严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！''' # 严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！'''
self.prompt += '''正确的示例如下： # self.prompt += '''正确的示例如下：
向左移 0.1m, 向左转弯 85 度 [{'func': 'move', 'x': 0, 'y': 0.1},{'func': 'turn','angle': -85}], # 向左移 0.1m, 向左转弯 85 度 [{'func': 'move', 'x': 0, 'y': 0.1},{'func': 'turn','angle': -85}],
向右移 0.2m, 向前 0.1m [{'func': 'move', 'x': 0, 'y': -0.2},{'func': 'move', 'x': 0.1, 'y': 0}], # 向右移 0.2m, 向前 0.1m [{'func': 'move', 'x': 0, 'y': -0.2},{'func': 'move', 'x': 0.1, 'y': 0}],
向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}], # 向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}],
原地左转 38 度 [{'func': 'turn','angle': -38}], # 原地左转 38 度 [{'func': 'turn','angle': -38}],
蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}] # 蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
发光或者照亮 5 秒 [{'func': 'light', 'time': 5}] # 发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}], # 向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}],
向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}], # 向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}],
鸣叫 3 声 [{'func': 'beep', 'time': 3}] # 鸣叫 3 声 [{'func': 'beep', 'time': 3}]
前行零点五米 [{'func': 'move', 'x': 0.5, 'y': 0}] # 前行零点五米 [{'func': 'move', 'x': 0.5, 'y': 0}]
# '''
# self.prompt = '''
# 你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 JSON 结果。请注意，只能使用以下指定的动作，不能创造新的动作：
# 允许的动作及其对应格式如下：
# - 向左移：{"index":N,"action":"go_left","time":T}
# - 向右移：{"index":N,"action":"go_right","time":T}
# - 向前移：{"index":N,"action":"go_front","time":T}
# - 向后移：{"index":N,"action":"go_back","time":T}
# - 向左转：{"index":N,"action":"go_left_rotate","time":T}
# - 向右转：{"index":N,"action":"go_right_rotate","time":T}
# - 蜂鸣器发声：{"index":N,"action":"beep_seconds","time":T}
# - 蜂鸣器发声次数：{"index":N,"action":"beep_counts","time":T}
# - 发光或者照亮：{"index":N,"action":"light_seconds","time":T}
# - 发光次数或者闪烁次数：{"index":N,"action":"light_counts","time":T}
# - 发光并伴随蜂鸣器：{"index":N,"action":"beep_light_counts","time":T}
# - 等待{"index":N,"action":"go_sleep","time":T}
# 示例输入输出如下：
# 输入：向左移 0.1m, 向左转弯 85 度
# 输出：[{"index":0,"action":"go_left","time":0.1},{"index":1,"action":"go_left_rotate","time":85}]
# 输入：向右移 0.2m, 向前 0.1m
# 输出：[{"index":0,"action":"go_right","time":0.2},{"index":1,"action":"go_front","time":0.1}]
# 输入：向右转 90 度，向右移 0.1m
# 输出：[{"index":0,"action":"go_right_rotate","time":90},{"index":1,"action":"go_right","time":0.1}]
# 输入：原地左转 38 度
# 输出：[{"index":0,"action":"go_left_rotate","time":38}]
# 输入：蜂鸣器发声 5 秒
# 输出：[{"index":0,"action":"beep_seconds","time":5}]
# 输入：发光或者照亮 5 秒
# 输出：[{"index":0,"action":"light_seconds","time":5}]
# 输入：向右走 30cm, 照亮 2s
# 输出：[{"index":0,"action":"go_right","time":0.3},{"index":1,"action":"light_seconds","time":2}]
# 输入：向左移 0.2m, 向后 0.1m
# 输出：[{"index":0,"action":"go_left","time":0.2},{"index":1,"action":"go_back","time":0.1}]
# 输入：鸣叫 3 声
# 输出：[{"index":0,"action":"beep_counts","time":3}]
# 输入：前行零点五米
# 输出：[{"index":0,"action":"go_front","time":0.5}]
# 输入：闪烁灯光 1 次并伴有蜂鸣器
# 输出：[{"index":0,"action":"beep_light_counts","time": 1}]
# 输入：灯光闪烁 3 次同时蜂鸣器也叫 3 次
# 输出：[{"index":0,"action":"beep_light_counts","time": 3}]
# '''
# self.prompt += '''请根据上面的示例，解析该任务文本，并返回相应的 JSON 字段。确保 JSON 中包含了键 index action 和 time 以及相应的值。不要附带其他的解释和注释，只需要 JSON 字段。'''
self.prompt = '''
你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 JSON 结果。请注意，只能使用以下指定的动作，不能创造新的动作：
允许的动作及其对应格式如下：
[{'properties': {'index': {'title': 'Index', 'type': 'integer'}, 'action': {'title': 'Action', 'type': 'string'}, 'time': {'title': 'Time', 'type': 'number'}}, 'required': ['index', 'action', 'time'], 'title': 'Action', 'type': 'object'}]
我不允许你自我创造出新的 action,action 字段仅仅包括以下内容：
go_right 向右移动
go_left 向左移动
go_front 向前移动
go_back 向后移动
go_left_rotate 向左旋转
go_right_rotate 向右旋转
beep_seconds 蜂鸣器鸣叫的时间
beep_counts 蜂鸣器鸣叫的次数
light_seconds 灯光发光的时间
light_counts 灯光闪烁的次数
beep_light_counts 灯光和蜂鸣器一起闪烁的次数
go_sleep 什么都不做
我的话和你的回复示例如下：
我的话：向左移 0.1m, 向左转弯 85 度
你的回复：[{"index":0,"action":"go_left","time":0.1},{"index":1,"action":"go_left_rotate","time":85}]
我的话：向右移 0.2m, 向前 0.1m
你的回复：[{"index":0,"action":"go_right","time":0.2},{"index":1,"action":"go_front","time":0.1}]
我的话：向右转 90 度，向右移 0.1m
你的回复：[{"index":0,"action":"go_right_rotate","time":90},{"index":1,"action":"go_right","time":0.1}]
我的话：原地左转 38 度
你的回复：[{"index":0,"action":"go_left_rotate","time":38}]
我的话：蜂鸣器发声 5 秒
你的回复：[{"index":0,"action":"beep_seconds","time":5}]
我的话：发光或者照亮 5 秒
你的回复：[{"index":0,"action":"light_seconds","time":5}]
我的话：向右走 30cm，照亮 2s
你的回复：[{"index":0,"action":"go_right","time":0.3},{"index":1,"action":"light_seconds","time":2}]
我的话：向左移 0.2m, 向后 0.1m
你的回复：[{"index":0,"action":"go_left","time":0.2},{"index":1,"action":"go_back","time":0.1}]
我的话：鸣叫 3 声
你的回复：[{"index":0,"action":"beep_counts","time":3}]
我的话：前行零点五米
你的回复：[{"index":0,"action":"go_front","time":0.5}]
我的话：闪烁灯光 1 次并伴有蜂鸣器
你的回复：[{"index":0,"action":"beep_light_counts","time": 1}]
我的话：灯光闪烁 3 次同时蜂鸣器也叫 3 次
你的回复：[{"index":0,"action":"beep_light_counts","time": 3}]
我的话如下：
''' '''
self.prompt += '''你只需要根据我的示例解析出指令即可，不要给我其他多余的回复;再次强调 你无需给我其他多余的回复 这对我很重要'''
self.messages = [] self.messages = []
self.resp = None self.resp = None
worker = threading.Thread(target=self.reset, daemon=True) worker = threading.Thread(target=self.reset, daemon=True)
worker.start() worker.start()
def reset(self): def reset(self):
self.messages = [self.make_message(self.prompt)] try:
self.resp = erniebot.ChatCompletion.create( self.messages = [self.make_message(self.prompt)]
model=self.model, self.resp = erniebot.ChatCompletion.create(
messages=self.messages, model=self.model,
) messages=self.messages,
self.messages.append(self.resp.to_message()) )
self.init_done_flag = True self.messages.append(self.resp.to_message())
logger.info("LLM init done") self.init_done_flag = True
logger.info("LLM init done")
except:
logger.error("LLM init error")
def make_message(self,content): def make_message(self,content):
return {'role': 'user', 'content': content} return {'role': 'user', 'content': content}
def get_command_json(self,chat): def get_command_json(self,chat):
while self.init_done_flag == False: # 等待初始化 (要是等到调用还没初始化，那就是真寄了) while self.init_done_flag == False: # 等待初始化 (要是等到调用还没初始化，那就是真寄了)
pass pass
chat = '我的话如下：' + chat
self.messages.append(self.make_message(chat)) self.messages.append(self.make_message(chat))
self.resp = erniebot.ChatCompletion.create( self.resp = erniebot.ChatCompletion.create(
model=self.model, model=self.model,
messages=self.messages, messages=self.messages,
) )
self.messages.append(self.resp.to_message()) self.messages.append(self.resp.to_message())
resp = self.resp.get_result().replace(' ', '').replace('\n', '').replace('\t', '') resp = self.resp.get_result().replace(' ', '')
return resp return resp
class CountRecord: class CountRecord: