国赛出发前版本

fix: 补充 action 列表检查
pref: 修改 BBALL offset 值 (未同步到其他备份 subtask.py 中)
套用8822参数

action.py

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

app.py

@@ -1,4 +1,4 @@
-from flask import Flask, render_template
+from flask import Flask, render_template, request
 from flask_socketio import SocketIO
 import toml
 from loguru import logger
@@ -10,19 +10,21 @@ import os
 import time
 import subprocess
 import signal
-import importlib
+import base64
+import json
 from main_upper import main_func
 server_command = [
     {"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_server1.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 = []
 
 time_record = None
 
+
+
 # 日志队列
 queue = Queue()
 # 跳过任务 干预任务调度
@@ -35,7 +37,9 @@ app.config['SECRET_KEY'] = 'secret!'
 socketio = SocketIO(app, allow_unsafe_werkzeug=True)
 
 server_process = None
+# FIXME 在列表里存所有的 task_process  
 task_process = None
+task_run_flag = threading.Event()
 
 class WebSocketHandler(logging.Handler):
     def emit(self, record):
@@ -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_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('/')
 def index():
     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')
 def operate_handle(data):
@@ -64,6 +90,7 @@ def operate_handle(data):
     global task_process
     global processes
     global time_record
+    global task_run_flag
     if data['type'] == 'save_config':
         f = open(os.path.join(fileOptions_path,data['file_name']), 'w')
         ret = toml.dump(data['content'], f)
@@ -99,21 +126,23 @@ def operate_handle(data):
     elif data['type'] == 'operate_task':
         # 任务函数
         if data['content'] == 'run':
+            task_run_flag.set()
+            # 开启 task 进程前先关闭所有历史进程
             if task_process != None:
                 task_process.terminate()
+
             time_record = time.perf_counter()
-            task_process = Process(target=main_func, args=(queue,skip_task_queue))
+            task_process = Process(target=main_func, args=(task_run_flag,queue,skip_task_queue))
             task_process.start()
             logger.info("开启 task")
         elif data['content'] == 'stop':
-            task_process.terminate()
+            task_run_flag.clear()
+            if task_process != None:
+                task_process.terminate()
             logger.info(f"任务结束 用时{time.perf_counter() - time_record}s")
             logger.info("关闭  task")
         elif data['content'] == 'restart':
-            if task_process != None:
+            pass
-                task_process.terminate()
-            task_process = Process(target=main_func, args=(queue,skip_task_queue))
-            task_process.start()
     elif data['type'] == 'show_server_log':
         content = ''
         try:
@@ -125,6 +154,12 @@ def operate_handle(data):
     elif data['type'] == 'skip_task':
         logger.info(data)
         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')
 def test_connect():
@@ -134,6 +169,7 @@ def test_connect():
     for item in fileOptions_list:
         config_data[item] = toml.load(os.path.join(fileOptions_path,item))
     socketio.emit('config_data', {'type': 'config_data', 'content': config_data})
+    socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag.isSet())})
 def thread_function():
     global queue
     while True:
@@ -146,6 +182,12 @@ def thread_function():
     
 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 = open(log_file, "w")
     time.sleep(2)

cfg_args.toml

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

cfg_main.toml

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

cfg_subtask.toml

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

cfg_subtask.toml.6_9.bak

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

cfg_subtask.toml.7154.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.2
+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
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 2
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 2.5
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = false
+
+[kick_ass]
+pid_kp = 0.8
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

cfg_subtask.toml.8822.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.5
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[get_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 1.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

cfg_subtask.toml.891.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.5
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[get_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 1.8
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 1.5
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = true
+
+[kick_ass]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

cfg_subtask.toml.raw.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.0
+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.0
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.5
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.5
+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 = 0.8
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

main.py

@@ -27,21 +27,21 @@ act.axis.exec()
 
 # 向任务队列添加任务
 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)
 

main_upper.py

@@ -6,12 +6,12 @@ import subtask as sb
 import majtask as mj
 from by_cmd_py import by_cmd_py
 import time
-import action as act
+# import action as act
 import logging
 import signal
 
-running = True
+# running = True
-def main_func(_queue, _skip_queue):
+def main_func(_run_flag,_queue, _skip_queue):
     if _queue != None:
         # 日志重定向
         class Handler(logging.Handler):
@@ -23,14 +23,13 @@ def main_func(_queue, _skip_queue):
         logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG")
         
     def signal_handler(sig, frame):
-        global running
+        _run_flag.clear()
-        running = False
     signal.signal(signal.SIGTERM, signal_handler)
 
 
     cmd_py_obj = by_cmd_py(_queue)
     sb.import_obj(cmd_py_obj, _skip_queue)
-    act.import_obj(cmd_py_obj)
+    # act.import_obj(_run_flag, cmd_py_obj)
 
     # 读取配置
     cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml')
@@ -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.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)
     
@@ -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'])) 
         # # 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.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_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'])) 
@@ -71,7 +79,7 @@ def main_func(_queue, _skip_queue):
     task_queuem_t = sb.task_queuem(task_queue)
     # 创建任务队列的工作线程
     def worker_thread():
-        while task_queuem_t.exec(_skip_queue) is True:
+        while task_queuem_t.exec(_skip_queue) and _run_flag.isSet():
             pass
 
     # 启动工作线程
@@ -82,7 +90,7 @@ def main_func(_queue, _skip_queue):
     # 创建主任务
     main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
     try:
-        while running:
+        while _run_flag.isSet():
             if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
                 pass
             else:

majtask.py

@@ -67,6 +67,7 @@ class main_task():
             error_abs = abs(self.lane_error)
 
             if error_abs > 50:
+                # speed = 11
                 speed = 13
             elif error_abs > 45:
                 speed = 15

subtask.py

@@ -2,19 +2,23 @@ from enum import Enum
 from loguru import logger
 from utils import label_filter
 from utils import tlabel
-from utils import LLM
+# from utils import LLM
+from utils import LLM_deepseek
 from utils import CountRecord
 import utils
 import toml
 import zmq
 import time
 import variable as var
-import action as act
 import re
-import threading
+import math
-import ctypes
+import json
+import json5
+# import threading
+# import ctypes
 cfg = None
 cfg_args = None
+cfg_move_area = None
 by_cmd = None
 filter = None
 llm_bot = None
@@ -47,9 +51,15 @@ def import_obj(_by_cmd, skip_queue):
 
     global cfg
     global cfg_args
+    global cfg_move_area
     global global_skip_queue
     cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
     cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.toml')
+    try:
+        with open('/home/evan/Workplace/project_main/cfg_move_area.json', 'r') as f:
+            cfg_move_area = json.load(f)
+    except:
+        cfg_move_area = None
     by_cmd = _by_cmd
     global_skip_queue = skip_queue
 
@@ -60,14 +70,14 @@ def import_obj(_by_cmd, skip_queue):
     logger.info("subtask yolo client init")
 
     # 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)
     if cfg['move_area']['llm_enable']:
-        llm_bot = LLM()
+        llm_bot = LLM_deepseek()
 def car_stop():
     for _ in range(3):
         by_cmd.send_speed_x(0)
@@ -105,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
             ret, error = filter.aim_right(label)
         error += offset
         # if ret:
-        if abs(error) <= 8:
+        if abs(error) <= 10:
             car_stop()
             logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
             ret, error = filter.aim_right(label)
@@ -222,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 car_stop()
                 logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
                 
-                ret, box = filter.get(label)
+                # 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"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -277,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
                 car_stop()
                 logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
                 
-                ret, box = filter.get(target_label)
+                # 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}")
                 break
     return True
@@ -415,8 +425,7 @@ class task_queuem(task):
 class get_block1():
     def init(self):
         var.task_speed = 15
-        act.cmd.camera(0)
+        by_cmd.send_angle_camera(0)
-        act.cmd.z2(20, 60, 0)
         filter.switch_camera(1)
         # if cfg['get_block']['first_block'] == "blue":
         #     self.target_label = tlabel.BBLOCK
@@ -452,26 +461,26 @@ class get_block1():
         calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
         logger.info("抓取块")
 
-        by_cmd.send_position_axis_z(30, 60)
+        by_cmd.send_position_axis_z(30, 80)
-        time.sleep(1)
+        time.sleep(1.5)
         by_cmd.send_angle_claw(63)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(1)
         by_cmd.send_angle_claw(25)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(30, 90)
+        by_cmd.send_position_axis_z(30, 110)
         time.sleep(0.5)
 
         by_cmd.send_angle_claw_arm(175)
         time.sleep(0.1)
         by_cmd.send_position_axis_x(1, 100)
         time.sleep(1)
-        by_cmd.send_position_axis_z(30, 70)
+        by_cmd.send_position_axis_z(30, 100)
         time.sleep(0.5)
         by_cmd.send_angle_claw(63)
         time.sleep(0.5)
 
-        by_cmd.send_position_axis_z(30, 130)
+        by_cmd.send_position_axis_z(30, 150)
         time.sleep(1)
         by_cmd.send_position_axis_x(1, 140)
         by_cmd.send_angle_claw_arm(225)
@@ -546,7 +555,7 @@ class put_block():
         ret, box = filter.get(tlabel.HOSPITAL)
         if ret > 0:
             width = box[0][2] - box[0][0]
-            if width > 130:
+            if width > 125: #FIXME maybe 125 batter
                 return True
         return False
     def exec(self):
@@ -557,7 +566,7 @@ class put_block():
         by_cmd.send_distance_x(10, 100)
         by_cmd.send_position_axis_z(30, 0)
         time.sleep(0.5)
-        by_cmd.send_position_axis_x(1, 20)
+        by_cmd.send_position_axis_x(1, 50) # 20
         time.sleep(1)
         by_cmd.send_angle_claw(63)
         time.sleep(1)
@@ -565,24 +574,24 @@ class put_block():
         # 放置第二個塊
         by_cmd.send_angle_storage(20)
         by_cmd.send_position_axis_x(1, 110)
-        by_cmd.send_position_axis_z(30, 120)
+        by_cmd.send_position_axis_z(30, 140)
         time.sleep(1.5)
         by_cmd.send_angle_claw_arm(180)
         by_cmd.send_angle_claw(85)
         # by_cmd.send_angle_storage(0)
         time.sleep(1)
-        by_cmd.send_position_axis_z(30,70)
+        by_cmd.send_position_axis_z(30,100)
         time.sleep(1)
         by_cmd.send_angle_claw(25)
         by_cmd.send_distance_x(-10, 110)
         time.sleep(1)
-        by_cmd.send_position_axis_z(30, 110)
+        by_cmd.send_position_axis_z(30, 130)
         time.sleep(1)
         by_cmd.send_angle_claw_arm(225)
         time.sleep(1)
         by_cmd.send_position_axis_z(30, 0)
         time.sleep(0.5)
-        by_cmd.send_position_axis_x(1, 20)
+        by_cmd.send_position_axis_x(1, 50)
         time.sleep(1.5)
         by_cmd.send_angle_claw(45)
         time.sleep(1)
@@ -592,7 +601,7 @@ class put_block():
     def after(self):
         var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
         # 下一动作预备位置
-        while by_cmd.send_position_axis_z(30, 130) == -1:
+        while by_cmd.send_position_axis_z(30, 150) == -1:
             pass
         time.sleep(1)
         while by_cmd.send_position_axis_x(1, 0) == -1:
@@ -640,7 +649,8 @@ class get_bball():
         car_stop()
         time.sleep(0.5)
         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("抓蓝色球")
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(45)
@@ -655,13 +665,13 @@ class get_bball():
             time.sleep(0.5)
             by_cmd.send_distance_axis_z(30, -40)
             time.sleep(0.5)
-            by_cmd.send_angle_claw_arm(80)
+            by_cmd.send_angle_claw_arm(90)
             time.sleep(0.5)
             by_cmd.send_angle_claw(54)
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(45)
             time.sleep(1)
-            by_cmd.send_position_axis_z(30, 135)
+            # by_cmd.send_position_axis_z(30, 155)
             # 继续向前走
             # by_cmd.send_speed_x(4)
         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"])
         # 下一动作预备位置
         by_cmd.send_angle_claw(30)
-        time.sleep(0.5)
+        # time.sleep(0.5)
         while by_cmd.send_position_axis_z(30, 0) == -1:
             pass
-        time.sleep(1)
+        time.sleep(0.5)
         # # 任务检查间隔
         # time.sleep(1)
 
@@ -699,20 +709,26 @@ class up_tower():
         calibrate_new(tlabel.TOWER, offset = 20, run = True)
         time.sleep(1)
         # calibrate(tlabel.TOWER, 27, False, 6)
-        by_cmd.send_distance_x(-10, 120)
+        by_cmd.send_distance_x(-10, 100) # used to be 120
         time.sleep(1)
         # 上古參數
-        # by_cmd.send_distance_y(-10, 50)
+        # by_cmd.send_distance_y(-10, 50) # 80
         # 6_9 模型參數
-        by_cmd.send_distance_y(-10, 40)
+        # by_cmd.send_distance_y(-10, 40)
         # 7_12_3 模型參數
         # by_cmd.send_distance_y(-10, 50)
-        # time.sleep(1)
+        # time.sleep(2)
-        car_stop()
+        # car_stop()
         # FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定
         # time.sleep(3)
         # by_cmd.send_speed_y(-10)
         # time.sleep(0.15)
+        # 8822
+        by_cmd.send_distance_y(-10, 50)
+        time.sleep(0.3)
+        # 891
+        # by_cmd.send_distance_y(-10, 35)
+        # time.sleep(0.3)
         
         by_cmd.send_angle_zhuan(10)
         time.sleep(12)
@@ -758,21 +774,31 @@ class get_rball():
         # 靠近塔
         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 參數
-        by_cmd.send_distance_y(-15, 35)
+        # by_cmd.send_distance_y(-15, 35)
-        time.sleep(2)
+        # time.sleep(2)
         # 7_12_3 參數
         # by_cmd.send_distance_y(-15, 45)
         # 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)
         time.sleep(1)
         logger.info("抓红球")
-        # by_cmd.send_angle_scoop(15)
+        # by_cmd.send_angle_scoop(12)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(30, 200)
+        time.sleep(3)
+        by_cmd.send_angle_scoop(12)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(30, 170)
-        time.sleep(2.5)
         by_cmd.send_angle_scoop(7)
         time.sleep(0.5)
         by_cmd.send_speed_y(15)
@@ -818,7 +844,7 @@ class put_bball():
         by_cmd.send_angle_storage(50)
         logger.info("把球放篮筐里")
 
-        time.sleep(1)
+        time.sleep(2)
         # by_cmd.send_distance_y(10, 55)
         by_cmd.send_angle_storage(20)
         # time.sleep(1)
@@ -861,7 +887,7 @@ class put_hanoi1():
             by_cmd.send_speed_omega(0)
         time.sleep(0.2)
         
-        by_cmd.send_position_axis_z(30, 130)
+        by_cmd.send_position_axis_z(30, 150)
 
         # 校准牌子
         if utils.direction_right > utils.direction_left:
@@ -1003,7 +1029,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓大平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(63)
             time.sleep(2)
@@ -1015,7 +1041,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         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_angle_claw(63)
             time.sleep(2)
@@ -1063,7 +1089,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(55)
             time.sleep(2)
@@ -1075,7 +1101,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         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_angle_claw(55)
             time.sleep(2)
@@ -1087,14 +1113,14 @@ class put_hanoi2():
             time.sleep(1)
             pass
         # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
-        ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 120)
+            by_cmd.send_position_axis_z(30, 150)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1106,7 +1132,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 120)
+            by_cmd.send_position_axis_z(30, 150)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1124,7 +1150,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
@@ -1136,7 +1162,7 @@ class put_hanoi2():
             time.sleep(2)
             pass
         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_angle_claw(50)
             time.sleep(2)
@@ -1148,14 +1174,14 @@ class put_hanoi2():
             time.sleep(2)
             pass
         # ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
-        ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 190) # 170
+            by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1171,7 +1197,7 @@ class put_hanoi2():
             # car_stop()
             pass
         else:
-            by_cmd.send_position_axis_z(30, 190)
+            by_cmd.send_position_axis_z(30, 210)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1184,7 +1210,7 @@ class put_hanoi2():
         # while True:
         #     pass
         by_cmd.send_speed_x(12)
-        time.sleep(1.5)
+        time.sleep(1.2)
     def nexec(self):
         pass
     def after(self):
@@ -1194,11 +1220,12 @@ class put_hanoi2():
         else:
             var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) 
         # time.sleep(2)
+        var.task_speed = 13
         pass
 
 class put_hanoi3():
     def init(self):
-        while by_cmd.send_position_axis_z(30, 130) == -1:
+        while by_cmd.send_position_axis_z(30, 150) == -1:
             pass
         time.sleep(3)
         logger.info("完成任务，爪回左侧")
@@ -1216,14 +1243,16 @@ class put_hanoi3():
         time.sleep(1)
         return True
     def exec(self):
-        while by_cmd.send_position_axis_z(30, 100) == -1:
+        while by_cmd.send_position_axis_z(30, 120) == -1:
             pass
         pass
     def nexec(self):
         pass
     def after(self):
+        by_cmd.send_angle_storage(20)
         by_cmd.send_position_axis_x(1, 150)
-        var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
+        var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"] - 0.2, cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
+        # FIXME 此处 -0.2 在 `2e6ce3e1f7d326d6ce8110855e2339ebc03ab2da` 前没有
 
 # 应急避险 第一阶段 找目标牌
 class move_area1():
@@ -1231,6 +1260,7 @@ class move_area1():
         logger.info("应急避险第一阶段初始化")
         while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
+        filter.switch_camera(1)
     def find(self):
         ret = filter.find(tlabel.SIGN)
         if ret:
@@ -1242,7 +1272,7 @@ class move_area1():
         car_stop()
         time.sleep(1)
         # calibrate_new(tlabel.SIGN, offset = 8, run = True)
-        calibrate_new(tlabel.SIGN, offset = -30, run = True, run_speed = 5)
+        calibrate_new(tlabel.SIGN, offset = -1, run = True, run_speed = 5)
         time.sleep(1)
 
         by_cmd.send_position_axis_x(1, 50)
@@ -1250,33 +1280,42 @@ class move_area1():
 
         # filter_w = (148, 560)
         # 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
                 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
 
@@ -1287,146 +1326,271 @@ class move_area1():
         # 任务检查间隔
         by_cmd.send_position_axis_x(1, 150)
         # time.sleep(1)
-        by_cmd.send_angle_claw_arm(225)
+        # by_cmd.send_angle_claw_arm(225)
 
         pass
 
 # 应急避险 第二阶段 找停车区域
 class move_area2():
+    def __init__(self):
+        self.action_dict = {
+            'beep_seconds': self.beep_seconds,
+            'beep_counts': self.beep_counts,
+            'light_seconds': self.light_seconds,
+            'light_counts': self.light_counts,
+            'beep_light_counts': self.beep_light_counts,
+            'beep_light_seconds': self.beep_light_seconds,
+            'go_front': self.go_front,
+            'go_back': self.go_back,
+            'go_left': self.go_left,
+            'go_right': self.go_right,
+            'go_left_rotate': self.go_left_rotate,
+            'go_right_rotate': self.go_right_rotate,
+            'go_sleep': self.go_sleep
+        }
+        self.front_time = 0
+        self.back_time = 0
+        self.left_time = 0
+        self.right_time = 0
+        self.sum_rotate_angle = 0
+        self.abs_x = 0 # 为了和程序指令适配，其中 x y 方向互换
+        self.abs_y = 0
+        self.abs_w = 0
+        pass
     def init(self):
         logger.info("应急避险第二阶段初始化")
-        self.offset = 15
+        self.offset = 60
         self.delta_x = 0
         self.delta_y = 0
         self.delta_omage = 0
     def find(self):
-        # time.sleep(0.001)
+        if var.skip_llm_task_flag and cfg_move_area == None:
-        if var.skip_llm_task_flag:
             return 5000
         ret, box = filter.get(tlabel.SHELTER)
         if ret:
             error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
-            if abs(error) < 20:
+            # 增加了一个宽度过滤
+            if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180:
                 return 5000
         return False
-    def sub_light(self, delay_time):
+    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)
-        time.sleep(delay_time)
+        time.sleep(_time * 0.7)
         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:
-            # 右转
+            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):
         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
         logger.info("开始寻找停车区域")
         car_stop()
-        calibrate_new(tlabel.SHELTER, offset = 15, run = True)
+        calibrate_new(tlabel.SHELTER, offset = 30, run = True)
         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
-            # 进入停车区域
+
-            # 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:
-                    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)
-        except:
+                self.reset()
+            except:
+                logger.warning("任务解析失败并退出，文心一言真是废物 (毋庸置疑)")
+                pass
+            
+        else:
+            # 无需调用大模型 直接开始执行传入的参数
+            try:
+                by_cmd.send_distance_y(10, 450)
+                time.sleep((450 * 5 / 1000) + 0.5)
+                for action in cfg_move_area:
+                    self.add_item(action)
+                    time.sleep(0.1)
+                time.sleep(0.5)
+                self.reset()
+            except:
+                pass
             pass
-        time.sleep(1)
-        # 回到原位
-        
-        delay_time = int(abs(self.delta_omage) * 400 / 90)
-        if int(abs(self.delta_omage)) == 360:
-            delay_time = 0
-        if self.delta_omage < 0:
-            # 左转
-            by_cmd.send_angle_omega(-55, delay_time)
-        else:
-            # 右转
-            by_cmd.send_angle_omega(55, delay_time)
-        time.sleep(delay_time / 300 * 1.5)
-        if self.delta_y > 0:
-            # 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了
-            delay_time = 180 - (self.delta_y * 500)
-        else:
-            delay_time = 180 + (abs(self.delta_y) * 500)
-        # 离开停车区域
-        by_cmd.send_distance_y(-25, delay_time)
-
-        time.sleep(delay_time * 5e-3)
-        
-        car_stop()
-
-        # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值
-        # by_cmd.send_distance_y(-15, 300)
-        pass
     def nexec(self):
         logger.warning("正在跳過大模型任務")
         time.sleep(2)
         pass
     def after(self):
         var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
-        by_cmd.send_position_axis_z(30, 0)
+        # by_cmd.send_position_axis_z(30, 0)
+        by_cmd.send_position_axis_z(30, 120)
         while by_cmd.send_angle_claw(90) == -1:
             pass
         time.sleep(2)
@@ -1439,10 +1603,10 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
-        by_cmd.send_position_axis_x(1, 160)
+        # by_cmd.send_position_axis_x(1, 160)
 
     def find(self):
         ret = filter.find(tlabel.SIGN)
@@ -1457,17 +1621,32 @@ class kick_ass():
         calibrate_new(tlabel.SIGN, offset = 8, run = True)
         by_cmd.send_angle_claw(15)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(30, 60)
+        # by_cmd.send_position_axis_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:
-            target_distance = self.pos_gap1
+            # target_distance = self.pos_gap1
+            pass
         else:
-            target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
+            # target_distance = self.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()
         
         # by_cmd.send_angle_claw_arm(225)
@@ -1477,9 +1656,21 @@ class kick_ass():
         by_cmd.send_position_axis_x(1, 120)
         time.sleep(1)
         logger.debug("結束任務，前進四")
+
+        filter.switch_camera(2)
+        for _ in range(3):
+            by_cmd.send_speed_x(15)
+        while True:
+            ret, box = filter.get(tlabel.BASE)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+                by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.02)
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass
+        
+       
     def nexec(self):
         pass
     def after(self):

subtask_69.py

@@ -118,7 +118,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
             logger.info(f"calibrate_right_new:停车后的误差是{error}")
             if abs(error) > 8:
                 logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
-                error = error * 3
+                error = error * 1.5
                 if error > 0:
                     by_cmd.send_distance_x(-10, int(error))
                 else:
@@ -241,6 +241,49 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 break
     return True
 
+# 对准应知道是左还是右，右侧需在过滤器中进行翻转
+# flipv 为垂直翻转标志，转右侧开启
+def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5):
+    stop_error = 0
+    error_record = CountRecord(10)
+    if run_direc == 1:
+        by_cmd.send_speed_x(run_speed)
+    else:
+        by_cmd.send_speed_x(-run_speed)
+        
+    if target_label == tlabel.TPLATFORM:
+        stop_error = 8
+    else:
+        stop_error = 15
+    while True:
+        ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
+        while not ret1:
+            # 如果找不到目标且跳过任务队列非空 (即指令跳过)
+            if not global_skip_queue.empty():
+                _ = global_skip_queue.get()
+                logger.error("跳过 hanoi_calibrate")
+                return False
+            # 如果找不到目标且发现错误目标 (上次放置任务失败)
+            if ret2:
+                # 如果连续计数超过阈值，则直接返回
+                if error_record(ret2):
+                    return False
+            ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret1:
+            # 校准速度越大 停车的条件越宽泛 20 15
+            if abs(error) <= stop_error:
+                car_stop()
+                logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
+                
+                ret, box = filter.get(target_label)
+                while not ret:
+                    ret, box = filter.get(target_label)
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"停车后像素误差:{error}")
+                break
+    return True
 # 任务类
 class task:  
     def __init__(self, name, task_template, find_counts=10, enable=True):  
@@ -372,8 +415,8 @@ class task_queuem(task):
 class get_block1():
     def init(self):
         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)
         # if cfg['get_block']['first_block'] == "blue":
         #     self.target_label = tlabel.BBLOCK
@@ -396,8 +439,7 @@ class get_block1():
         if any(ret):
             return True
         return False
-
+    
-
     def exec(self):
         car_stop()
         if self.target_counts[0] > self.target_counts[1]:
@@ -406,7 +448,7 @@ class get_block1():
         else:
             var.first_block = tlabel.BBLOCK
             var.second_block = tlabel.RBLOCK
-        calibrate_new(var.first_block, offset = 15, run = True, run_speed = 5)
+        calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
         logger.info("抓取块")
 
         by_cmd.send_position_axis_z(30, 60)
@@ -421,20 +463,21 @@ class get_block1():
 
         by_cmd.send_angle_claw_arm(175)
         time.sleep(0.1)
-        by_cmd.send_position_axis_x(1, 120)
+        by_cmd.send_position_axis_x(1, 100)
         time.sleep(1)
         by_cmd.send_position_axis_z(30, 70)
-        time.sleep(0.1)
+        time.sleep(0.5)
         by_cmd.send_angle_claw(63)
         time.sleep(0.5)
 
         by_cmd.send_position_axis_z(30, 130)
         time.sleep(1)
         by_cmd.send_position_axis_x(1, 140)
-        by_cmd.send_angle_claw_arm(220)
+        by_cmd.send_angle_claw_arm(225)
         time.sleep(0.5)
-        by_cmd.send_angle_storage(55)
+        # by_cmd.send_angle_storage(55)
-        time.sleep(1)
+        # time.sleep(1)
+        by_cmd.send_position_axis_z(30, 60)
 
         pass
     def nexec(self):
@@ -463,13 +506,12 @@ class get_block2():
         return False
     def exec(self):
         car_stop()
-        calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5)
+        calibrate_new(var.second_block, offset = 16, run = True, run_speed = 5)
         logger.info("抓取块")
         time.sleep(0.5)
-        by_cmd.send_angle_claw_arm(220)
+        by_cmd.send_angle_claw_arm(225)
         by_cmd.send_angle_claw(63)
-        by_cmd.send_position_axis_z(30, 60)
+        time.sleep(0.1)
-        time.sleep(1)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(1)
         by_cmd.send_angle_claw(25)
@@ -521,7 +563,7 @@ class put_block():
 
         # 放置第二個塊
         by_cmd.send_angle_storage(20)
-        by_cmd.send_position_axis_x(1, 130)
+        by_cmd.send_position_axis_x(1, 110)
         by_cmd.send_position_axis_z(30, 120)
         time.sleep(1.5)
         by_cmd.send_angle_claw_arm(180)
@@ -535,7 +577,7 @@ class put_block():
         time.sleep(1)
         by_cmd.send_position_axis_z(30, 110)
         time.sleep(1)
-        by_cmd.send_angle_claw_arm(220)
+        by_cmd.send_angle_claw_arm(225)
         time.sleep(1)
         by_cmd.send_position_axis_z(30, 0)
         time.sleep(0.5)
@@ -554,7 +596,7 @@ class put_block():
         time.sleep(1)
         while by_cmd.send_position_axis_x(1, 0) == -1:
             pass
-        while by_cmd.send_angle_claw_arm(36) == -1:
+        while by_cmd.send_angle_claw_arm(45) == -1:
             pass
         # 任务检查间隔
         # time.sleep(2)
@@ -569,7 +611,7 @@ class get_bball():
         # time.sleep(0.5)
         # by_cmd.send_position_axis_x(1, 0)
         # time.sleep(2)
-        # by_cmd.send_angle_claw_arm(36)
+        # by_cmd.send_angle_claw_arm(45)
         
         while (by_cmd.send_angle_storage(0) == -1):
             by_cmd.send_angle_storage(0)
@@ -582,13 +624,14 @@ class get_bball():
         self.record = CountRecord(5)
     def find(self):
         # 目标检测蓝球
-        ret = filter.find(tlabel.BBALL)
+        # ret = filter.find(tlabel.BBALL)
+        ret = filter.find_mult([tlabel.BBALL, tlabel.YBALL])
         # ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
         # TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关
         # if ret:
         #     if self.record(tlabel.BBALL):
         #         return True
-        if ret:
+        if ret[0] or ret[1]:
             return True
         return False
     def exec(self):
@@ -596,10 +639,10 @@ class get_bball():
         car_stop()
         time.sleep(0.5)
         for _ in range(3):
-            calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5)
+            calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
             logger.info("抓蓝色球")
             time.sleep(0.5)
-            by_cmd.send_angle_claw_arm(36)
+            by_cmd.send_angle_claw_arm(45)
             by_cmd.send_angle_claw(54)
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1.2)
@@ -615,7 +658,7 @@ class get_bball():
             time.sleep(0.5)
             by_cmd.send_angle_claw(54)
             time.sleep(0.5)
-            by_cmd.send_angle_claw_arm(36)
+            by_cmd.send_angle_claw_arm(45)
             time.sleep(1)
             by_cmd.send_position_axis_z(30, 135)
             # 继续向前走
@@ -727,8 +770,8 @@ class get_rball():
         logger.info("抓红球")
         # by_cmd.send_angle_scoop(15)
         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)
         time.sleep(0.5)
         by_cmd.send_speed_y(15)
@@ -834,30 +877,40 @@ class put_hanoi1():
             logger.info("应该向左转")
 
 
-        # # 校准 omega
+        # 校准 omega
-        # if error > 0:
+        for _ in range(10):
-        #     by_cmd.send_angle_omega(-20,abs(var.lane_error))
+            ret, box = filter.get(utils.direction)
-        # else:
+            if ret:
-        #     by_cmd.send_angle_omega(20,abs(var.lane_error))
+                error = (box[0][2] + box[0][0] - 320) / 2
-        # time.sleep(0.5)
+            by_cmd.send_speed_omega(-error * 0.8)
-        # car_stop()
+            time.sleep(0.2)
-        # time.sleep(0.5)
-        # by_cmd.send_distance_x(10, 200)
-
-        # by_cmd.send_distance_x(10, 180)
-        by_cmd.send_distance_x(10, 250)
-        time.sleep(1)
         car_stop()
+
+
+        # 前进
+        # by_cmd.send_distance_x(10, 200)
+        # by_cmd.send_distance_x(10, 180)
+        # by_cmd.send_distance_x(10, 180)
+        # time.sleep(1.5)
+        # car_stop()
+        
+        while True:
+            by_cmd.send_speed_x(8.5)
+            ret, box = filter.get(utils.direction)
+            if ret:
+                if abs(box[0][2] - box[0][0]) > 41:
+                    car_stop()
+                    break
         
         # 根据方向初始化执行器位置
         if utils.direction is tlabel.RMARK:
             # FIXME 右侧的爪子会被 storage 挡住
             by_cmd.send_position_axis_x(1, 0)
-            by_cmd.send_angle_claw_arm(36)
+            by_cmd.send_angle_claw_arm(45)
             by_cmd.send_angle_storage(0)
         else:
             by_cmd.send_position_axis_x(1, 150)
-            by_cmd.send_angle_claw_arm(220)
+            by_cmd.send_angle_claw_arm(225)
             by_cmd.send_angle_storage(55)
         time.sleep(1)
 
@@ -866,16 +919,20 @@ class put_hanoi1():
         if utils.direction_right > utils.direction_left:
             utils.direction = tlabel.RMARK
             # by_cmd.send_angle_omega(-25,430)
-            by_cmd.send_angle_omega(-45,238)
             # by_cmd.send_angle_omega(-55,194)
+            # by_cmd.send_angle_omega(-45,238)
+            # by_cmd.send_angle_omega(-45,252)
+            by_cmd.send_angle_omega(-45,260)
             time.sleep(2)
             while (by_cmd.send_angle_camera(90) == -1):
                 by_cmd.send_angle_camera(90)
         else:
             utils.direction = tlabel.LMARK
             # by_cmd.send_angle_omega(25,430)
-            by_cmd.send_angle_omega(45,238)
             # by_cmd.send_angle_omega(55,194)
+            # by_cmd.send_angle_omega(45,238)
+            # by_cmd.send_angle_omega(45,252)
+            by_cmd.send_angle_omega(45,260)
             time.sleep(2)
             while (by_cmd.send_angle_camera(0) == -1):
                 by_cmd.send_angle_camera(0)
@@ -884,11 +941,17 @@ class put_hanoi1():
     def nexec(self):
         pass
     def after(self):
-        var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
+        # var.switch_lane_model = True
+        if utils.direction == tlabel.RMARK:
+            var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.4, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
+        else:
+            var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
         pass
+        # time.sleep(1.5)
 
 class put_hanoi2():
     def __init__(self):
+        
         if cfg['put_hanoi2']['first_target'] == "lp":
             self.target_label = tlabel.LPILLER
         elif cfg['put_hanoi2']['first_target'] == "mp":
@@ -897,20 +960,25 @@ class put_hanoi2():
             self.target_label = tlabel.SPILLER
     def init(self):
         logger.info("物资盘点 2 初始化")
+        var.task_speed = 8.5
         if utils.direction == tlabel.RMARK:
             # 15
-            self.offset = 19
+            self.offset = 14
             # self.platform_offset = -25
-            self.platform_offset = -10
+            # self.platform_offset = -19
-        else:
-            self.offset = 10
-            #self.platform_offset = -30
             self.platform_offset = -15
+        else:
+            self.offset = 14
+            # self.platform_offset = -30
+            # self.platform_offset = -19
+            self.platform_offset = -15
+        # 延时，防止过早看到 tplatform（虽然此现象相当少见且诡异）
+        time.sleep(1.5)
+
     def find(self):
         # ret, box = filter.get(self.target_label)
         ret, box = filter.get(tlabel.TPLATFORM)
         if ret:
-            var.task_speed = 8.5
             error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset
             if error > 0:
                 return True
@@ -935,12 +1003,12 @@ class put_hanoi2():
         logger.info("抓大平台")
         if utils.direction is tlabel.RMARK:
             by_cmd.send_position_axis_z(30, 10)
-            by_cmd.send_position_axis_x(1, 130)
+            by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(63)
             time.sleep(2)
             by_cmd.send_angle_claw(40)
             time.sleep(0.5)
-            by_cmd.send_distance_axis_z(30, 20)
+            by_cmd.send_distance_axis_z(30, 30)
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
             time.sleep(1)
@@ -952,7 +1020,7 @@ class put_hanoi2():
             time.sleep(2)
             by_cmd.send_angle_claw(40)
             time.sleep(0.5)
-            by_cmd.send_distance_axis_z(30, 20)
+            by_cmd.send_distance_axis_z(30, 30)
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
@@ -964,15 +1032,15 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("放大平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_x(1, 130)
+            by_cmd.send_position_axis_x(1, 150)
             time.sleep(1.5)
             by_cmd.send_distance_axis_z(30, -20)
             time.sleep(1)
             by_cmd.send_angle_claw(81)
             time.sleep(0.5)
-            by_cmd.send_angle_claw(63)
-            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
             time.sleep(1)
             pass
         else:
@@ -982,9 +1050,9 @@ class put_hanoi2():
             time.sleep(1)
             by_cmd.send_angle_claw(81)
             time.sleep(0.5)
-            by_cmd.send_angle_claw(63)
-            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
             time.sleep(1)
 
         ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed =  5)
@@ -995,8 +1063,8 @@ class put_hanoi2():
         logger.info("抓中平台")
         if utils.direction is tlabel.RMARK:
             by_cmd.send_position_axis_z(30, 10)
-            by_cmd.send_position_axis_x(1, 130)
+            by_cmd.send_position_axis_x(1, 150)
-            by_cmd.send_angle_claw(63)
+            by_cmd.send_angle_claw(55)
             time.sleep(2)
             by_cmd.send_angle_claw(35)
             time.sleep(0.5)
@@ -1008,7 +1076,7 @@ class put_hanoi2():
         else:
             by_cmd.send_position_axis_z(30, 10)
             by_cmd.send_position_axis_x(1, 40)
-            by_cmd.send_angle_claw(63)
+            by_cmd.send_angle_claw(55)
             time.sleep(2)
             by_cmd.send_angle_claw(35)
             time.sleep(0.5)
@@ -1017,32 +1085,33 @@ class put_hanoi2():
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
             pass
-        ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
-            logger.error("跳过物资盘点 2 exec")
+            logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 100)
+            by_cmd.send_position_axis_z(30, 120)
             time.sleep(2)
-            by_cmd.send_position_axis_x(1, 130)
+            by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
             by_cmd.send_distance_axis_z(30, -20)
             time.sleep(0.5)
-            by_cmd.send_angle_claw(65)
+            by_cmd.send_angle_claw(55)
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 100)
+            by_cmd.send_position_axis_z(30, 120)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
             by_cmd.send_distance_axis_z(30, -20)
             time.sleep(0.5)
-            by_cmd.send_angle_claw(65)
+            by_cmd.send_angle_claw(55)
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
@@ -1055,15 +1124,15 @@ class put_hanoi2():
         logger.info("抓小平台")
         if utils.direction is tlabel.RMARK:
             by_cmd.send_position_axis_z(30, 10)
-            by_cmd.send_position_axis_x(1, 130)
+            by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
             by_cmd.send_angle_claw(27)
             time.sleep(1)
             by_cmd.send_distance_axis_z(30, 10)
             time.sleep(0.5)
-            by_cmd.send_position_axis_x(1, 10)
+            by_cmd.send_position_axis_x(1, 0)
-            time.sleep(1)
+            time.sleep(2)
             pass
         else:
             by_cmd.send_position_axis_z(30, 10)
@@ -1074,19 +1143,20 @@ class put_hanoi2():
             time.sleep(1)
             by_cmd.send_distance_axis_z(30, 10)
             time.sleep(0.5)
-            by_cmd.send_position_axis_x(1, 160)
+            by_cmd.send_position_axis_x(1, 170)
-            time.sleep(1)
+            time.sleep(2)
             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)
         if not ret:
-            logger.error("跳过物资盘点 2 exec")
+            logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 170)
+            by_cmd.send_position_axis_z(30, 190) # 170
             time.sleep(1.5)
-            by_cmd.send_position_axis_x(1, 130)
+            by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
             by_cmd.send_distance_axis_z(30, -20)
             time.sleep(0.5)
@@ -1094,12 +1164,13 @@ class put_hanoi2():
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
             time.sleep(1)
-            by_cmd.send_speed_y(15)
+
-            time.sleep(0.1)
+            # by_cmd.send_speed_y(10)
-            car_stop()
+            # time.sleep(0.12)
+            # car_stop()
             pass
         else:
-            by_cmd.send_position_axis_z(30, 170)
+            by_cmd.send_position_axis_z(30, 190)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1111,11 +1182,18 @@ class put_hanoi2():
             time.sleep(1.5)
         # while True:
         #     pass
-        
+        by_cmd.send_speed_x(12)
+        time.sleep(1.5)
+        var.task_speed = 10
     def nexec(self):
         pass
     def after(self):
-        var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
+        # var.switch_lane_model = False
+        if utils.direction is tlabel.RMARK:
+            var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
+        else:
+            var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) 
+        # time.sleep(2)
         pass
 
 class put_hanoi3():
@@ -1130,10 +1208,10 @@ class put_hanoi3():
         while by_cmd.send_position_axis_x(1, 0) == -1:
             pass
         time.sleep(1)
-        # while by_cmd.send_angle_claw_arm(220) == -1:
+        # while by_cmd.send_angle_claw_arm(225) == -1:
-        #     pass
-        # while by_cmd.send_angle_claw(90) == -1:
         #     pass
+        while by_cmd.send_angle_claw(85) == -1:
+            pass
     def find(self):
         time.sleep(1)
         return True
@@ -1144,6 +1222,7 @@ class put_hanoi3():
     def nexec(self):
         pass
     def after(self):
+        var.task_speed = 0
         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"])
 
@@ -1209,7 +1288,7 @@ class move_area1():
         # 任务检查间隔
         by_cmd.send_position_axis_x(1, 150)
         # time.sleep(1)
-        by_cmd.send_angle_claw_arm(220)
+        by_cmd.send_angle_claw_arm(225)
 
         pass
 
@@ -1361,7 +1440,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)
@@ -1387,17 +1466,28 @@ class kick_ass():
         else:
             target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
         by_cmd.send_distance_x(10, target_distance)
+        
         logger.info(f"target distance {target_distance}")
         time.sleep(1.5 + (self.target_person - 1) * 0.7 )
         car_stop()
         
-        # by_cmd.send_angle_claw_arm(220)
+        # by_cmd.send_angle_claw_arm(225)
         # time.sleep(0.5)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(3)
         by_cmd.send_position_axis_x(1, 120)
         time.sleep(1)
         logger.debug("結束任務，前進四")
+
+        filter.switch_camera(2)
+        for _ in range(3):
+            by_cmd.send_speed_x(15)
+        while True:
+            ret, box = filter.get(tlabel.BASE)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+            by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.02)
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass

subtask_7123.py

@@ -1367,7 +1367,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)

subtask_7126.py

@@ -1360,7 +1360,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)

templates/index.html

@@ -109,7 +109,7 @@
                     </el-col>
                     <el-col :xs="24" :sm="8">
                         <el-button @click="skipTask" type="success" style="width: 100%">
-                            强制跳转
+                            关闭
                         </el-button>
                     </el-col>
                 </el-row>
@@ -128,12 +128,16 @@
                 </el-button> -->
                 
                 <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">
                         <h3>{{ sectionName }}</h3>
                         <el-row :gutter="20">
                             <el-col v-for="(value, key) in section" :key="key" :xs="24" :sm="12" :md="8" :lg="6">
                                 <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
                                           v-for="item in target_person_options"
                                           :key="item.value"
@@ -162,10 +166,7 @@
                             </el-col>
                         </el-row>
                     </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>
 
                 <h2 class="section-title">Log Display</h2>
@@ -197,16 +198,16 @@
                 timer: null,
                 target_person_options: [{
                   value: 1,
-                  label: '模式 1'
+                  label: '6_9'
                 }, {
                   value: 2,
-                  label: '模式 2'
+                  label: '7_10_2'
                 }, {
                   value: 3,
-                  label: '模式 3'
+                  label: '7_14_2'
                 }, {
                   value: 4,
-                  label: '模式 4'
+                  label: '7_15_4'
                 }],
             },
             methods: {

templates/index1.html

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

templates/index2.html

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

test/test_action.py

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

test/test_cali.py

@@ -34,15 +34,16 @@ def signal_handler(sig, frame):
 
 offset = 0
 signal.signal(signal.SIGTERM, signal_handler)
+cmd_py_obj.send_speed_x(15)
 while True:
-    time.sleep(0.2)
+
-    ret, box = filter.get(tlabel.LMARK)
+    time.sleep(0.02)
-    if ret:
+    ret, box = filter.get(tlabel.BASE)
-        # 宽度大于 41 停车
-        print(f"width: {box[0][2] - box[0][0]} height: {box[0][3] - box[0][1]}")
     # if ret:
-    #     error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        # 宽度大于 41 停车
-    #     print(error)
+        # print(f"width: {box[0][2] - box[0][0]} height: {box[0][3] - box[0][1]}")
-        
+    if ret:
-    #     cmd_py_obj.send_speed_omega(-error * 0.8)
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        print(error)
+        cmd_py_obj.send_speed_omega(-error * 0.8)
 car_stop()

test/test_filter.py

@@ -19,16 +19,20 @@ filter = label_filter(socket)
 filter.switch_camera(1)
 
 
-find_counts = 0
+# find_counts = 0
-offset = 19
+# offset = 19
-label = [tlabel.LPILLER, tlabel.MPILLER, tlabel.SPILLER]
+# label = [tlabel.LPILLER, tlabel.MPILLER, tlabel.SPILLER]
-label = tlabel.TPLATFORM
+# label = tlabel.TPLATFORM
 while True:
     time.sleep(0.2)
-    ret, box = filter.get(tlabel.TPLATFORM)
+    ret, box = filter.get(tlabel.HOSPITAL)
     if ret:
-        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        width = box[0][2] - box[0][0]
-        logger.error(error)
+        error = (box[0][2] + box[0][0] - 320) / 2
+        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
     # ret, box = filter.get(label)

test/test_llm.py

@@ -13,18 +13,48 @@ class LLM:
     def __init__(self):
         
         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.resp = None
         self.reset()

test/test_ocr.py

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

test/test_ocr_camera.py

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

utils.py

@@ -2,6 +2,7 @@
 from enum import Enum
 import numpy as np
 import erniebot
+from openai import OpenAI
 from simple_pid import PID
 from loguru import logger
 import threading
@@ -284,7 +285,11 @@ class label_filter:
                 else:
                     target_bool = False
                     target_box = None
-                expect_boxes = (results[:, 0] == label.value)
+                if len(label) == 1:
+                    expect_boxes = (results[:, 0] == label[0].value)
+                elif len(label) == 2:
+                    expect_boxes = np.logical_or(results[:, 0] == label[0].value, results[:, 0] == label[1].value)
+
                 boxes = results[expect_boxes, :]
                 # 在此处过滤
                 if len(boxes) != 0:
@@ -372,55 +377,235 @@ class label_filter:
                 error = (boxes[center_x_index][4] + boxes[center_x_index][2] - self.img_size[0]) / 2
                 return (True, error)
         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:
     def __init__(self):
         self.init_done_flag = False
         erniebot.api_type = "qianfan"
         erniebot.ak = "jReawMtWhPu0wrxN9Rp1MzZX"
         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.resp = None
         worker = threading.Thread(target=self.reset, daemon=True)  
         worker.start()
     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):
         return {'role': 'user', 'content': content}
     def get_command_json(self,chat):
         while self.init_done_flag == False: # 等待初始化 (要是等到调用还没初始化，那就是真寄了)
             pass
+        chat = '我的话如下：' + chat
         self.messages.append(self.make_message(chat))
         self.resp = erniebot.ChatCompletion.create(
                         model=self.model,
                         messages=self.messages,
         )
         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
 
 class CountRecord:

variable.py

@@ -9,7 +9,8 @@ task_speed = 0
 pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0} # 1.2
 
 # 转向 pid 对象
-pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=60) # FIXME 6_9 模型为 50
+# pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=70) # FIXME 6_9 模型为 60
+pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=60) # FIXME 6_9 模型为 60
 
 llm_text = ''