版本暂存

.gitignore

@@ -196,3 +196,5 @@ pyrightconfig.json
 
 # log path
 log/*
+
+temp/*

action.py

@@ -13,7 +13,7 @@ move = None
 axis = None
 cmd = None
 
-cfg = toml.load('cfg_action.toml')
+cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml')
 
 def import_obj(_bycmd):
     global bycmd

app.py

@@ -0,0 +1,150 @@
+from flask import Flask, render_template
+from flask_socketio import SocketIO
+import toml
+from loguru import logger
+import logging
+from multiprocessing import Process, Queue
+import threading
+import multiprocessing
+import os
+import time
+import subprocess
+import signal
+import importlib
+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/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()
+# 跳过任务 干预任务调度
+skip_task_queue = Queue()
+
+app = Flask(__name__)
+app.jinja_env.variable_start_string = '[('
+app.jinja_env.variable_end_string = ')]'
+app.config['SECRET_KEY'] = 'secret!'
+socketio = SocketIO(app, allow_unsafe_werkzeug=True)
+
+server_process = None
+task_process = None
+
+class WebSocketHandler(logging.Handler):
+    def emit(self, record):
+        log_entry = self.format(record)
+        socketio.emit('log', {'level': record.levelname.lower(), 'content': log_entry})
+
+# 设置日志
+logger.remove()
+handler = WebSocketHandler()
+logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG")
+
+fileOptions_path = '/home/evan/Workplace/project_main'
+fileOptions_list = ['cfg_main.toml', 'cfg_subtask.toml']
+
+@app.route('/')
+def index():
+    return render_template('index.html')
+
+
+@socketio.on('operate')
+def operate_handle(data):
+    global server_process
+    global task_process
+    global processes
+    global time_record
+    if data['type'] == 'save_config':
+        f = open(os.path.join(fileOptions_path,data['file_name']), 'w')
+        ret = toml.dump(data['content'], f)
+        logger.info(f"保存成功 {data['file_name']}")
+        f.close()
+    elif data['type'] == 'operate_server':
+        logger.info(data)
+        if data['content'] == 'run':
+            log_file = "server_processes.log"
+            log = open(log_file, "w")
+            time.sleep(2)
+            # 启动所有脚本
+            for i, env_info in enumerate(server_command):
+                env_path = env_info["path"]
+                script = env_info["script"]
+                env = os.environ.copy() 
+                if i == 0:
+                    process = subprocess.Popen([script], cwd=env_path, env=env, stdout=log, stderr=subprocess.STDOUT)
+                    processes.append(process)
+                    time.sleep(2)
+                
+                process = subprocess.Popen(['python', script], cwd=env_path, env=env, stdout=log, stderr=subprocess.STDOUT)
+                processes.append(process)
+            logger.info("开启 server")
+        elif data['content'] == 'stop':
+            for process in processes:
+                logger.error(process.pid)
+                os.kill(process.pid, signal.SIGINT)
+            logger.info("关闭 server")
+        elif data['content'] == 'restart':
+            
+            logger.info("重启 server")
+    elif data['type'] == 'operate_task':
+        # 任务函数
+        if data['content'] == 'run':
+            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.start()
+            logger.info("开启 task")
+        elif data['content'] == 'stop':
+            task_process.terminate()
+            logger.info(f"任务结束 用时{time.perf_counter() - time_record}s")
+            logger.info("关闭  task")
+        elif data['content'] == 'restart':
+            if task_process != None:
+                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:
+            with open("server_processes.log", 'r') as file:
+                content = file.read()
+        except:
+            pass
+        socketio.emit('server_log', {'type': 'server_log', 'content': content})
+    elif data['type'] == 'skip_task':
+        logger.info(data)
+        skip_task_queue.put(1)
+
+@socketio.on('connect')
+def test_connect():
+    logger.info('Client connected')
+    socketio.emit('config_data', {'type': 'fileOptions', 'content': fileOptions_list})
+    config_data = {}
+    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})
+def thread_function():
+    global queue
+    while True:
+        try:
+            log = queue.get()
+            socketio.emit('log', log)
+        except multiprocessing.Queue.Empty:
+            pass
+
+    
+if __name__ == '__main__':
+    thread1 = threading.Thread(target=thread_function, daemon = True)
+    thread1.start()
+    socketio.run(app, host='0.0.0.0', port=5001, allow_unsafe_werkzeug=True)
+    if server_process != None:
+        server_process.terminate()
+    if task_process != None:
+        task_process.terminate()

cfg_main.toml

@@ -3,29 +3,27 @@ logger_filename = "log/file_{time}.log"
 logger_format = "{time} {level} {message}"
 
 [task]
-Subtask_enable = true # 子任务总使能 调试用！
-GetBlock_enable = true # 人员施救使能
-PutBlock_enable = true # 紧急转移使能
-GetBBall_enable = true # 整装上阵使能
-UpTower_enable = true  # 通信抢修使能
-GetRBall_enable = true # 高空排险使能
-PutBBall_enable = true # 派发物资使能
-PutHanoi_enable = true # 物资盘点使能
-MoveArea_enable = true # 应急避险使能
-KickAss_enable = true  # 扫黑除暴使能
+Subtask_enable = true
+GetBlock_enable = true
+PutBlock_enable = true
+GetBBall_enable = true
+UpTower_enable = true
+GetRBall_enable = true
+PutBBall_enable = true
+PutHanoi_enable = true
+MoveArea_enable = true
+KickAss_enable = true
 
 [find_counts]
-GetBlock_counts = 5 # 人员施救计数
-PutBlock_counts = 5  # 紧急转移计数
-GetBBall_counts = 1  # 整装上阵计数
-UpTower_counts = 3   # 通信抢修计数
-GetRBall_counts = 10 # 高空排险计数
-
-
-PutBBall_counts = 5     # 派发物资计数
-PutHanoi1_counts = 10   # 物资盘点计数
-PutHanoi2_counts = 2    # 物资盘点计数
-PutHanoi3_counts = 2    # 物资盘点计数
-MoveArea1_counts = 6    # 应急避险计数
-MoveArea2_counts = 1700 # 应急避险第二阶段计数
-KickAss_counts = 10     # 扫黑除暴计数
+GetBlock_counts = 5
+PutBlock_counts = 5
+GetBBall_counts = 1
+UpTower_counts = 3
+GetRBall_counts = 10
+PutBBall_counts = 15
+PutHanoi1_counts = 10
+PutHanoi2_counts = 5
+PutHanoi3_counts = 2
+MoveArea1_counts = 6
+MoveArea2_counts = 1700
+KickAss_counts = 10

cfg_main.toml.bak

@@ -0,0 +1,29 @@
+[debug]
+logger_filename = "log/file_{time}.log"
+logger_format = "{time} {level} {message}"
+
+[task]
+Subtask_enable = false
+GetBlock_enable = false
+PutBlock_enable = false
+GetBBall_enable = false
+UpTower_enable = false
+GetRBall_enable = false
+PutBBall_enable = false
+PutHanoi_enable = false
+MoveArea_enable = true
+KickAss_enable = true
+
+[find_counts]
+GetBlock_counts = 8
+PutBlock_counts = 5
+GetBBall_counts = 3
+UpTower_counts = 3
+GetRBall_counts = 10
+PutBBall_counts = 10
+PutHanoi1_counts = 10
+PutHanoi2_counts = 2
+PutHanoi3_counts = 2
+MoveArea1_counts = 6
+MoveArea2_counts = 1700
+KickAss_counts = 10

cfg_subtask.toml

@@ -1,92 +1,61 @@
-################################################
 [get_block]
-# pid 参数值
-pid_kp = 1.0
+pid_kp = 1
 pid_ki = 0
 pid_kd = 0
+first_block = "blue"
 
-# *第一个抓取的方块
-first_block = "red"
-# first_block = "blue"
-
-################################################
 [put_block]
-# pid 参数值
 pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
 
-################################################
 [get_bball]
-# pid 参数值
 pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
 
-################################################
 [up_tower]
-# pid 参数值
 pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
 
-################################################
 [get_rball]
-# pid 参数值
-pid_kp = 0.8
+pid_kp = 0.5
 pid_ki = 0
 pid_kd = 0
 
-################################################
 [put_bball]
-# pid 参数值
-pid_kp = 0.6
+pid_kp = 0.7
 pid_ki = 0
 pid_kd = 0
 
-################################################
 [put_hanoi1]
-# pid 参数值
-pid_kp = 1.0
+pid_kp = 1
 pid_ki = 0
 pid_kd = 0
 
-################################################
 [put_hanoi2]
-# pid 参数值
-pid_kp = 1.0
+pid_kp = 0.7
 pid_ki = 0
 pid_kd = 0
-
-# 距离标定值
-pos_gap = 160 # 标定值，两个放置位置的标定距离 
-# first_target = "lp"
+pos_gap = 160
 first_target = "mp"
-# first_target = "sp"
-################################################
+
 [put_hanoi3]
-# pid 参数值
 pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
 
-################################################
 [move_area]
-# pid 参数值
 pid_kp = 1.4
 pid_ki = 0
 pid_kd = 0
+llm_enable = true
 
-llm_enable = true # 大模型机器人
-
-################################################
 [kick_ass]
-# pid 参数值
 pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
-
-pos_gap1 = 150    # 目标牌和第一个 person 之间的距离
-pos_gap2 = 80     # person 之间的距离
-target_person = 3 # 击打的人 - 最靠近标识牌的为 1
-################################################
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 2

main.py

@@ -1,23 +1,24 @@
 import toml
-import queue
 import threading
+import queue
 from loguru import logger
 import subtask as sb
 import majtask as mj
 from by_cmd_py import by_cmd_py
 import time
 import action as act
+# 读取配置
+cfg_main = toml.load('cfg_main.toml')
+cfg_subtask = toml.load('cfg_subtask.toml')
+
+# logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
+running = True
 
 cmd_py_obj = by_cmd_py()
 sb.import_obj(cmd_py_obj)
 act.import_obj(cmd_py_obj)
 
-# 读取配置
-cfg_main = toml.load('cfg_main.toml')
-
 # 配置日志输出
-logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
-
 act.axis.camera(0)
 act.axis.x2(140)
 act.axis.storage(20)
@@ -27,21 +28,20 @@ act.axis.exec()
 # 向任务队列添加任务
 task_queue = queue.Queue()
 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_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.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_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.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'])) 
     # 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.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_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], 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_hanoi3, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_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.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_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.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_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.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.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable'])) 
 # 将任务队列传入调度模块中
 task_queuem_t = sb.task_queuem(task_queue)
 
@@ -54,13 +54,11 @@ def worker_thread():
 worker = threading.Thread(target=worker_thread, daemon=True)  
 worker.start()  
 
+
 # 创建主任务
 main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
-
-# 主线程仅在子线程搜索 (SEARCHING) 和 空闲 (IDLE) 状态下进行操作
-# while task_queuem_t.busy is True:
 try:
-    while True:
+    while running:
         if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
             pass
         else:
@@ -74,5 +72,9 @@ except KeyboardInterrupt:
         time.sleep(0.1)
         cmd_py_obj.send_speed_omega(0)
         time.sleep(0.1)
-
+for _ in range(3):
+    cmd_py_obj.send_speed_x(0)
+    time.sleep(0.1)
+    cmd_py_obj.send_speed_omega(0)
+    time.sleep(0.1)
 logger.info("Main thread exit")

main_upper.py

@@ -0,0 +1,104 @@
+import toml
+import threading
+import queue
+from loguru import logger
+import subtask as sb
+import majtask as mj
+from by_cmd_py import by_cmd_py
+import time
+import action as act
+import logging
+import signal
+
+running = True
+def main_func(_queue, _skip_queue):
+    if _queue != None:
+        # 日志重定向
+        class Handler(logging.Handler):
+            def emit(self, record):
+                log_entry = self.format(record)
+                _queue.put({'level': record.levelname.lower(), 'content': log_entry})
+        logger.remove()
+        handler = Handler()
+        logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG")
+        
+    def signal_handler(sig, frame):
+        global running
+        running = False
+    signal.signal(signal.SIGTERM, signal_handler)
+
+
+    cmd_py_obj = by_cmd_py(_queue)
+    sb.import_obj(cmd_py_obj)
+    act.import_obj(cmd_py_obj)
+
+    # 读取配置
+    cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml')
+    logger.info(cfg_main)
+    cfg_subtask = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml')
+
+    # 配置日志输出
+    logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
+
+    act.axis.camera(0)
+    act.axis.x2(140)
+    act.axis.storage(20)
+    act.axis.scoop(25)
+    act.axis.exec()
+
+    logger.info(cfg_main)
+    
+    # 向任务队列添加任务
+    task_queue = queue.Queue()
+    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_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.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) 
+        # # 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.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_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_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_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_queuem_t = sb.task_queuem(task_queue)
+    # 创建任务队列的工作线程
+    def worker_thread():
+        while task_queuem_t.exec(_skip_queue) is True:
+            pass
+
+    # 启动工作线程
+    worker = threading.Thread(target=worker_thread, daemon=True)  
+    worker.start()  
+
+
+    # 创建主任务
+    main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
+    try:
+        while running:
+            if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
+                pass
+            else:
+                main_task_t.run()
+                pass
+    except KeyboardInterrupt:
+        logger.info("Interrupt received, stopping...")
+        # 停车 
+        for _ in range(3):
+            cmd_py_obj.send_speed_x(0)
+            time.sleep(0.1)
+            cmd_py_obj.send_speed_omega(0)
+            time.sleep(0.1)
+    for _ in range(3):
+        cmd_py_obj.send_speed_x(0)
+        time.sleep(0.1)
+        cmd_py_obj.send_speed_omega(0)
+        time.sleep(0.1)
+    logger.info("Main thread exit")
+if __name__ == '__main__':
+    main_func(None)

subtask.py

@@ -1,7 +1,6 @@
 '''
 待办事项：
-- 医院第二个方块动作适配
-- find 超时
+- 第一二個方塊自動識別，hanoi2 識別到物塊即停車，不需手動輸入第一個物塊（修改 fliter 使其能一次請求過濾多個標籤)
 '''
 from enum import Enum
 from loguru import logger
@@ -15,28 +14,56 @@ import zmq
 import time
 import variable as var
 import action as act
-
-context = zmq.Context()
-socket = context.socket(zmq.REQ)
-socket.connect("tcp://localhost:6667")
-logger.info("subtask yolo client init")
-
-cfg = toml.load('cfg_subtask.toml') # 加载任务配置
-logger.info("load subtask config")
+import re
+import threading
+import ctypes
+cfg = None
 
 by_cmd = None
 filter = None
 llm_bot = None
+
+# 目标检测 socket 客户端
+context = None
+socket = None
+
+context1 = None
+ocr_socket = None
+
 '''
 description: main.py 里执行 引入全局变量
 param {*} _by_cmd 控制器对象
 return {*}
 '''
 def import_obj(_by_cmd):
+
     global by_cmd
     global filter
     global llm_bot
+
+    global context
+    global socket
+
+    global context1
+    global ocr_socket
+
+    global cfg
+    
+    cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
     by_cmd = _by_cmd
+
+    # 目标检测 socket 客户端
+    context = zmq.Context()
+    socket = context.socket(zmq.REQ)
+    socket.connect("tcp://localhost:6667")
+    logger.info("subtask yolo client init")
+
+    # ocr socket 客户端
+    context1 = zmq.Context()
+    ocr_socket = context1.socket(zmq.REQ)
+    ocr_socket.connect("tcp://localhost:6668")
+    logger.info("subtask ocr client init")
+
     filter = label_filter(socket)
     if cfg['move_area']['llm_enable']:
         llm_bot = LLM()
@@ -205,12 +232,14 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
 
 # 任务类
 class task:  
-    def __init__(self, task_template, find_counts=10, enable=True):  
+    def __init__(self, name, task_template, find_counts=10, enable=True):  
         self.enable = enable
         self.task_t = task_template()
 
         self.counts = 0
-        self.find_counts = find_counts
+        self.find_counts = int(find_counts)
+        self.name = name
+
     def init(self):
         if hasattr(self.task_t, 'init') and callable(getattr(self.task_t, 'init', None)):
             self.task_t.init()
@@ -218,12 +247,7 @@ class task:
             logger.warning("[Task ]# 该任务没有 init 方法")
     def find(self):
         if hasattr(self.task_t, 'find') and callable(getattr(self.task_t, 'find', None)):
-            # 检查该任务执行标志
-            while True:
-                ret = self.task_t.find()
-                self.counts += ret
-                if self.counts >= self.find_counts:
-                    break
+            return self.task_t.find()
         else:
             logger.warning("[Task ]# 该任务没有 find 方法")
     def exec(self):  
@@ -242,6 +266,7 @@ class task:
                 logger.warning("[Task ]# 该任务没有 nexec 方法")
     def after(self):
         if hasattr(self.task_t, 'after') and callable(getattr(self.task_t, 'after', None)):
+            logger.info(f"[Task ]# {self.name} 正在执行 after")
             self.task_t.after()
             logger.debug(f"[Task ]# Task completed")  
         else:
@@ -262,7 +287,9 @@ class task_queuem(task):
         self.status = task_queuem_status.IDEL
         self.busy = True
         logger.info(f"[TaskM]# Task num {self.queue.qsize()}")
-    def exec(self):
+        # exec 线程
+        self.exec_thread = None
+    def exec(self, skip_queue):
         # 如果空闲状态则将下一个队列任务取出
         if self.status is task_queuem_status.IDEL:
             if self.queue.qsize() == 0:
@@ -281,14 +308,42 @@ class task_queuem(task):
             logger.info(f"[TaskM]# ---------------------->>>>")
         # 阻塞搜索任务标志位
         elif self.status is task_queuem_status.SEARCHING:
-            logger.info(f"[TaskM]# Start searching task target")
-            self.task_now.find()
-            self.status = task_queuem_status.EXECUTING
+            logger.info(f"[{self.task_now.name}]# Start searching task target")
+            while True:
+                if not skip_queue.empty():
+                    _ = skip_queue.get()
+                    logger.error(f"{self.task_now.name} 任务在 find 中已经被手动跳过")
+                    self.status = task_queuem_status.IDEL   # 空动作不需要阻塞巡线，直接置位
+                    self.task_now.after()                   # 执行任务后处理
+                    self.queue.task_done()                  # 弹出已执行的任务
+                    return True
+                ret = self.task_now.find()
+                self.task_now.counts += ret
+                if self.task_now.counts >= self.task_now.find_counts:
+                    self.status = task_queuem_status.EXECUTING
+                    break
         # 执行任务函数
         elif self.status is task_queuem_status.EXECUTING:
             if self.task_now.enable is True:
                 logger.info(f"[TaskM]# Start execute task function")
-                self.task_now.exec()                    # 执行当前任务函数
+                self.exec_thread = threading.Thread(target=self.task_now.exec)
+                # 启动线程
+                self.exec_thread.start()
+                while True:
+                    if not self.exec_thread.is_alive():
+                        break
+                    else:
+                        if not skip_queue.empty():
+                            car_stop()
+                            thread_id = self.exec_thread.ident
+                            res = ctypes.pythonapi.PyThreadState_SetAsyncExc(thread_id, ctypes.py_object(SystemExit))
+                            _ = skip_queue.get()
+                            logger.error(f"{self.task_now.name} 任务在 exec 中已经被手动跳过")
+                            self.status = task_queuem_status.IDEL   # 空动作不需要阻塞巡线，直接置位
+                            self.task_now.after()                   # 执行任务后处理
+                            self.queue.task_done()                  # 弹出已执行的任务
+                            return True
+                # self.task_now.exec()                    # 执行当前任务函数
                 self.status = task_queuem_status.IDEL   # 执行完成后为退出巡线阻塞
                 self.task_now.after()                   # 执行任务后处理
                 self.queue.task_done()                  # 弹出已执行的任务
@@ -306,26 +361,41 @@ class task_queuem(task):
 class get_block1():
     def init(self):
         var.task_speed = 0
-        logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
         act.cmd.camera(0)
         act.cmd.z2(20, 60, 0)
         filter.switch_camera(1)
-        if cfg['get_block']['first_block'] == "blue":
-            self.target_label = tlabel.BBLOCK
-            self.another_label = tlabel.RBLOCK
-        else:
-            self.target_label = tlabel.RBLOCK
-            self.another_label = tlabel.BBLOCK
-            cfg['get_block']['first_block'] = "red"
+        # if cfg['get_block']['first_block'] == "blue":
+        #     self.target_label = tlabel.BBLOCK
+        #     self.another_label = tlabel.RBLOCK
+        # else:
+        #     self.target_label = tlabel.RBLOCK
+        #     self.another_label = tlabel.BBLOCK
+        #     cfg['get_block']['first_block'] = "red"
+        self.target_label = [tlabel.RBLOCK, tlabel.BBLOCK]
+        self.target_counts = [0, 0]
     def find(self):
         # 目标检测红/蓝方块
-        ret = filter.find(self.target_label)
-        if ret > 0:
+        # ret = filter.find(self.target_label)
+        # if ret > 0:
+        #     return True
+        # return False
+        ret = filter.find_mult(self.target_label)
+        self.target_counts[0] += ret[0]
+        self.target_counts[1] += ret[1]
+        if any(ret):
             return True
         return False
+
+
     def exec(self):
         car_stop()
-        calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5)
+        if self.target_counts[0] > self.target_counts[1]:
+            var.first_block = tlabel.RBLOCK
+            var.second_block = tlabel.BBLOCK
+        else:
+            var.first_block = tlabel.BBLOCK
+            var.second_block = tlabel.RBLOCK
+        calibrate_new(var.first_block, offset = 15, run = True, run_speed = 5)
         logger.info("抓取块")
 
         by_cmd.send_position_axis_z(30, 60)
@@ -363,29 +433,28 @@ class get_block1():
 
 class get_block2():
     def init(self):
-        logger.info(f"人员施救 2 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
         while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
         filter.switch_camera(1)
-        if cfg['get_block']['first_block'] == "red":
-            self.target_label = tlabel.BBLOCK
-            self.another_label = tlabel.RBLOCK
-        else:
-            self.target_label = tlabel.RBLOCK
-            self.another_label = tlabel.BBLOCK
+        # if cfg['get_block']['first_block'] == "red":
+        #     self.target_label = tlabel.BBLOCK
+        #     self.another_label = tlabel.RBLOCK
+        # else:
+        #     self.target_label = tlabel.RBLOCK
+        #     self.another_label = tlabel.BBLOCK
     def find(self):
         # 目标检测红/蓝方块
-        ret = filter.find(self.target_label)
+        ret = filter.find(var.second_block)
         if ret > 0:
             return True
         return False
     def exec(self):
         car_stop()
-        calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5)
+        calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5)
         logger.info("抓取块")
         time.sleep(0.5)
         by_cmd.send_angle_claw_arm(220)
-        by_cmd.send_angle_claw(53)
+        by_cmd.send_angle_claw(63)
         by_cmd.send_position_axis_z(30, 60)
         time.sleep(1)
         by_cmd.send_position_axis_x(1, 20)
@@ -407,7 +476,7 @@ class get_block2():
     def after(self):
         var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
         # 任务检查间隔
-        time.sleep(9)
+        time.sleep(7)
 
 
 # 紧急转移
@@ -423,7 +492,7 @@ class put_block():
         ret, box = filter.get(tlabel.HOSPITAL)
         if ret > 0:
             width = box[0][2] - box[0][0]
-            if width > 145:
+            if width > 130:
                 return True
         return False
     def exec(self):
@@ -441,19 +510,27 @@ class put_block():
 
         # 放置第二個塊
         by_cmd.send_angle_storage(20)
-        by_cmd.send_distance_x(-10, 110)
-        time.sleep(1)
-        by_cmd.send_position_axis_x(1, 150)
-        by_cmd.send_position_axis_z(30, 150)
-        time.sleep(2)
-        by_cmd.send_angle_claw_arm(126)
+        by_cmd.send_position_axis_x(1, 130)
+        by_cmd.send_position_axis_z(30, 120)
+        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(15,100)
+        by_cmd.send_position_axis_z(30,70)
         time.sleep(1)
-        by_cmd.send_position_axis_x(1, 0)
+        by_cmd.send_angle_claw(25)
+        by_cmd.send_distance_x(-10, 110)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 110)
+        time.sleep(1)
+        by_cmd.send_angle_claw_arm(220)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 0)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 40)
         time.sleep(1.5)
-        by_cmd.send_angle_storage(0)
+        by_cmd.send_angle_claw(45)
         pass
     def nexec(self):
         pass
@@ -520,11 +597,11 @@ class get_bball():
             time.sleep(1)
             by_cmd.send_distance_axis_z(30, 20)
             time.sleep(1)
-            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_position_axis_x(1, 30)
             time.sleep(0.5)
             by_cmd.send_distance_axis_z(30, -40)
             time.sleep(0.5)
-            by_cmd.send_angle_claw_arm(70)
+            by_cmd.send_angle_claw_arm(80)
             time.sleep(0.5)
             by_cmd.send_angle_claw(54)
             time.sleep(1)
@@ -573,11 +650,10 @@ class up_tower():
         by_cmd.send_distance_y(-10, 50)
         time.sleep(3)
         by_cmd.send_angle_zhuan(10)
-        time.sleep(10)
+        time.sleep(11)
         by_cmd.send_distance_y(10, 60)
         time.sleep(1)
         by_cmd.send_angle_zhuan(0)
-        time.sleep(0.5)
         # while True:
         #     pass
     def nexec(self):
@@ -623,8 +699,9 @@ class get_rball():
         # by_cmd.send_angle_scoop(15)
         time.sleep(0.5)
         by_cmd.send_position_axis_z(30, 170)
-        time.sleep(3.5)
+        time.sleep(3)
         by_cmd.send_angle_scoop(7)
+        time.sleep(0.5)
         by_cmd.send_distance_y(15, 70)
         time.sleep(1)
         by_cmd.send_angle_omega(-55,30)
@@ -661,16 +738,17 @@ class put_bball():
         by_cmd.send_distance_x(10, 20)
         # 向左运动
         by_cmd.send_distance_y(-10, 35)
+        by_cmd.send_angle_storage(15)
         time.sleep(1)
 
         by_cmd.send_angle_storage(55)
         logger.info("把球放篮筐里")
 
-        time.sleep(2)
-        by_cmd.send_distance_y(10, 55)
         time.sleep(1)
-
+        by_cmd.send_distance_y(10, 55)
         by_cmd.send_angle_storage(20)
+        time.sleep(1)
+        car_stop()
         pass
     def nexec(self):
         pass
@@ -756,16 +834,16 @@ 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(-55,194)
             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(55,194)
             time.sleep(2)
             while (by_cmd.send_angle_camera(0) == -1):
                 by_cmd.send_angle_camera(0)
@@ -789,14 +867,17 @@ class put_hanoi2():
         logger.info("物资盘点 2 初始化")
         if utils.direction == tlabel.RMARK:
             self.offset = 22
+            self.platform_offset = -29
         else:
             self.offset = 10
+            self.platform_offset = -39
     def find(self):
-        ret, box = filter.get(self.target_label)
+        # 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.offset
-            if abs(error) < 40:
+            error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset
+            if error > 0:
                 return True
         return False
     def exec(self): 
@@ -1110,6 +1191,7 @@ class move_area2():
         time.sleep(delay_time)
         by_cmd.send_beep(0)
     def sub_move(self, x, y):
+        # FIXME 如果同時有 xy，是否會造成 delay 不足
         self.delta_x += x
         self.delta_y += y
         
@@ -1125,7 +1207,8 @@ class move_area2():
                 by_cmd.send_distance_y(-15, delay_time)
             else:
                 by_cmd.send_distance_y(15, delay_time)
-        time.sleep(delay_time / 400 * 1.5)
+        time.sleep(delay_time / 500)
+        car_stop()
         pass
     def sub_turn(self, angle):
         self.delta_omage += angle
@@ -1161,8 +1244,8 @@ class move_area2():
                 return
             # 进入停车区域
             # by_cmd.send_speed_y(15)
-            by_cmd.send_distance_y(15, 300)
-            time.sleep(2)
+            by_cmd.send_distance_y(25, 180)
+            time.sleep(1)
             # time.sleep(1.25)
             car_stop()
             logger.info(resp_commands)
@@ -1197,11 +1280,11 @@ class move_area2():
         time.sleep(delay_time / 300 * 1.5)
         if self.delta_y > 0:
             # 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了
-            delay_time = 300 - (self.delta_y * 500)
+            delay_time = 180 - (self.delta_y * 500)
         else:
-            delay_time = 300 + (abs(self.delta_y) * 500)
+            delay_time = 180 + (abs(self.delta_y) * 500)
         # 离开停车区域
-        by_cmd.send_distance_y(-15, delay_time)
+        by_cmd.send_distance_y(-25, delay_time)
 
         time.sleep(delay_time * 5e-3)
         

templates/index.html

@@ -0,0 +1,277 @@
+<!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="https://unpkg.com/element-ui/lib/theme-chalk/index.css">
+    <script src="https://cdn.jsdelivr.net/npm/vue@2.6.14/dist/vue.js"></script>
+    <script src="https://unpkg.com/element-ui/lib/index.js"></script>
+    <script src="https://cdnjs.cloudflare.com/ajax/libs/socket.io/4.0.1/socket.io.js"></script> -->
+
+    <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;
+        }
+        .section-title {
+            margin-top: 30px;
+            margin-bottom: 20px;
+            color: #303133;
+            border-bottom: 1px solid #dcdfe6;
+            padding-bottom: 10px;
+        }
+        .button-group {
+            margin-bottom: 20px;
+        }
+        .config-form {
+            background-color: white;
+            padding: 20px;
+            border-radius: 4px;
+            box-shadow: 0 2px 12px 0 rgba(0,0,0,.1);
+        }
+        .log-container {
+            height: 400px;
+            overflow-y: auto;
+            background-color: white;
+            border-radius: 4px;
+            padding: 10px;
+            box-shadow: 0 2px 12px 0 rgba(0,0,0,.1);
+        }
+        .log-container1 {
+            height: 400px;
+            overflow-y: auto;
+            background-color: white;
+            border-radius: 4px;
+            padding: 10px;
+            box-shadow: 0 2px 12px 0 rgba(0,0,0,.1);
+        }
+        .log-entry {
+            margin-bottom: 5px;
+            font-family: monospace;
+        }
+        .log-info { color: #409EFF; }
+        .log-error { color: #F56C6C; }
+        .log-warning { color: #E6A23C; }
+        .log-debug { color: #909399; }
+    </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="startServer" type="success" style="width: 100%">开启 server</el-button>
+                    </el-col>
+                    <el-col :xs="24" :sm="8">
+                        <el-button @click="stopServer" type="danger" style="width: 100%">关闭 server</el-button>
+                    </el-col>
+                    <el-col :xs="24" :sm="8">
+                        <el-button @click="restartServer" type="warning" style="width: 100%">重启 server</el-button>
+                    </el-col>
+                </el-row>
+                
+                <el-row :gutter="20" class="button-group">
+                    <el-col :xs="24" :sm="8">
+                        <el-button @click="startTask" type="success" style="width: 100%">开启 task</el-button>
+                    </el-col>
+                    <el-col :xs="24" :sm="8">
+                        <el-button @click="stopTask" type="danger" style="width: 100%">关闭 task</el-button>
+                    </el-col>
+                    <el-col :xs="24" :sm="8">
+                        <el-button @click="restartTask" type="warning" style="width: 100%">重启 task</el-button>
+                    </el-col>
+                </el-row>
+
+                <el-row :gutter="20" class="button-group">
+                    <el-col :xs="24" :sm="8">
+                        <el-button @click="showServerLog" type="success" style="width: 100%">
+                            {{ showServerLogFlag ? '关闭' : '打开' }}  server 日志
+                        </el-button>
+                    </el-col>
+                    <el-col :xs="24" :sm="8">
+                        
+                    </el-col>
+                    <el-col :xs="24" :sm="8">
+                        <el-button @click="skipTask" type="success" style="width: 100%">
+                            强制跳转
+                        </el-button>
+                    </el-col>
+                </el-row>
+                
+                <el-select v-model="selectedFile" placeholder="" @change="loadConfig" style="width: 100%; margin-bottom: 20px;">
+                    <el-option
+                        v-for="item in fileOptions"
+                        :key="item"
+                        :label="item"
+                        :value="item">
+                    </el-option>
+                </el-select>
+                
+                <el-button v-if="showConfigForm" @click="toggleConfigForm" type="primary" style="margin-bottom: 20px;">
+                    {{ showConfigForm ? '关闭' : '打开' }} 配置
+                </el-button>
+                
+                <el-form v-if="showConfigForm && config" class="config-form">
+                    <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-switch
+                                        v-if="typeof value === 'boolean'"
+                                        v-model="config[sectionName][key]">
+                                    </el-switch>
+                                    <el-input-number
+                                        v-else-if="typeof value === 'number'"
+                                        v-model="config[sectionName][key]"
+                                        :controls="true"
+                                        :min="Number.MIN_SAFE_INTEGER"
+                                        :max="Number.MAX_SAFE_INTEGER"
+                                        :step="1"
+                                        style="width: 100%;">
+                                    </el-input-number>
+                                    <el-input
+                                        v-else
+                                        v-model="config[sectionName][key]">
+                                    </el-input>
+                                </el-form-item>
+                            </el-col>
+                        </el-row>
+                    </div>
+                    <el-button @click="saveConfig" type="primary" style="margin-top: 20px;">保存配置</el-button>
+                </el-form>
+
+                <h2 class="section-title">Log Display</h2>
+
+                <div v-if="showServerLogFlag" class="log-container1" v-html="formatServerLog(ServerLog)"></div>
+
+
+                <div class="log-container">
+                    <div v-for="logEntry in logs" :key="logEntry.id" :class="['log-entry', `log-${logEntry.level}`]">
+                        [{{ logEntry.level.toUpperCase() }}] {{ logEntry.content }}
+                    </div>
+                </div>
+            </el-main>
+        </el-container>
+    </div>
+
+    <script>
+        new Vue({
+            el: '#app',
+            data: {
+                logs: [],
+                selectedFile: null,
+                showConfigForm: false,
+                fileOptions: [],
+                config: null,
+                files: {},
+                ServerLog: "",
+                showServerLogFlag: false,
+                timer: null
+            },
+            methods: {
+                loadConfig() {
+                    this.config = this.files[this.selectedFile];
+                    this.showConfigForm = true;
+                },
+                toggleConfigForm() {
+                    this.showConfigForm = !this.showConfigForm;
+                },
+                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'});
+                },
+                saveConfig() {
+                    console.log(this.config);
+                    this.socket.emit('operate', {type: 'save_config', file_name: this.selectedFile, content: this.config});
+                },
+                addLog(log) {
+                    this.logs.push({
+                        level: log.level,
+                        content: log.content
+                    });
+                    this.$nextTick(() => {
+                        const container = document.querySelector('.log-container');
+                        container.scrollTop = container.scrollHeight;
+                        if (this.logs.length > 100) {
+                            this.logs = this.logs.slice(-100);
+                        }
+                    });
+                },
+                showServerLog() {
+                    if (this.showServerLogFlag == false)
+                    {
+                        this.timer = setInterval(() => {
+                            this.socket.emit('operate', { type: 'show_server_log', content: '' });
+                        }, 2000);
+                    }
+                    else
+                    {
+                        clearInterval(this.timer);
+                        this.timer = null;
+                    }
+                    this.showServerLogFlag = !this.showServerLogFlag;
+                },
+                formatServerLog(log) {
+                    // 将换行符转换为 <br> 标签
+                    return log.replace(/\n/g, '<br>');
+                },
+                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('log', (data) => {
+                    this.addLog(data);
+                });
+                this.socket.on('config_data', (data) => {
+                    if (data.type == 'config_data') {
+                        this.files = data.content;
+                    } else if (data.type == 'fileOptions') {
+                        this.fileOptions = data.content;
+                    }
+                });
+                this.socket.on('server_log', (data) => {
+                    this.ServerLog = "";
+                    this.ServerLog = data.content;
+                });
+                
+            }
+        });
+    </script>
+</body>
+</html>

test/test_capture.py

@@ -0,0 +1,14 @@
+import cv2
+
+
+cap = cv2.VideoCapture(20)
+cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M','J','P','G'))
+cap.set(cv2.CAP_PROP_FRAME_WIDTH, 960)
+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 540)
+
+while True:
+    ret, frame = cap.read()
+    if ret:
+        cv2.imshow("frame",frame)
+    cv2.waitKey(1)
+cv2.destroyAllWindows()

test/test_filter.py

@@ -1,18 +1,14 @@
-
-import sys
 import os
+import sys
 parent_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
 sys.path.append(parent_dir)
-
 from utils import label_filter
 from loguru import logger
 from utils import tlabel
 import zmq
 import time
-from by_cmd_py import by_cmd_py
-import time
-from utils import CountRecord
-by_cmd = by_cmd_py()
+
+
 
 context = zmq.Context()
 socket = context.socket(zmq.REQ)
@@ -22,209 +18,22 @@ logger.info("subtask yolo client init")
 filter = label_filter(socket)
 filter.switch_camera(1)
 
-def car_stop():
-    for _ in range(3):
-        by_cmd.send_speed_x(0)
-        time.sleep(0.2)
-        by_cmd.send_speed_omega(0)
-# 蓝色球使用
-def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
-    not_found_counts = 0
-    ret, error = filter.aim_right(label)
-    while not ret:
-        not_found_counts += 1
-        if not_found_counts >= 20:
-            not_found_counts = 0
-            error = -320 # error > 0 front run
-            logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找")
-            break
-        ret, error = filter.aim_right(label)
-    
-    error += offset
-    if abs(error) > 10 and run:
-        if error > 0:
-            by_cmd.send_speed_x(-run_speed)
-        else:
-            by_cmd.send_speed_x(run_speed)
-        pass
-    # 停的位置已经很接近目标，可以直接使用 distance 校准
-    else:
-        # logger.info("停车时误差就小于")
-        # error = error * 3
-        # if error > 0:
-        #     by_cmd.send_distance_x(-10, int(error))
-        # else:
-        #     by_cmd.send_distance_x(10, int(-error))
-        return
-    while True:
-        ret, error = filter.aim_right(label)
-        while not ret:
-            ret, error = filter.aim_right(label)
-        error += offset
-        if ret:
-            if abs(error) <= 8:
-                car_stop()
-                logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
-
-                ret, error = filter.aim_right(label)
-                while not ret:
-                    ret, error = filter.aim_right(label)
-                error += offset
-                logger.info(f"calibrate_right_new:停车后的误差是{error}")
-                if abs(error) > 8:
-                    logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
-                    error = error * 3
-                    if error > 0:
-                        by_cmd.send_distance_x(-10, int(error))
-                    else:
-                        by_cmd.send_distance_x(10, int(-error))
-                
-                break
-'''
-description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车
-如果停车后 error > 8 则使用 distance 校准 
-这个方法仅用于在视野里能找到的情况下进行校准，并不能实现边走边寻找 然后再校准
-param {*} label
-param {*} offset
-param {*} run
-param {*} run_speed
-return {*}
-'''     
-def calibrate_new(label, offset, run = True, run_speed = 3.5):
-    not_found_counts = 0
-    ret, box = filter.get(label)
-    while not ret:
-        not_found_counts += 1
-        if not_found_counts >= 20:
-            not_found_counts = 0
-            error = -320 # error > 0 front run
-            logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找")
-        ret, box = filter.get(label)
-    
-    error = (box[0][2] + box[0][0] - 320) / 2 + offset
-    if abs(error) > 10 and run:
-        if error > 0:
-            by_cmd.send_speed_x(-run_speed)
-        else:
-            by_cmd.send_speed_x(run_speed)
-    # 停的位置已经很接近目标，可以直接使用 distance 校准
-    else:
-        if abs(error) > 8:
-            logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准")
-            # error = error # 3
-            if error > 0:
-                by_cmd.send_distance_x(-10, int(error))
-            else:
-                by_cmd.send_distance_x(10, int(-error))
-            return
-        logger.info(f"calibrate_new:停车后误差{error}小于 8  不校准")
-        return 
-    while True:
-        ret, box = filter.get(label)
-        while not ret:
-            ret, box = filter.get(label)
-        
-        error = (box[0][2] + box[0][0] - 320) / 2 + offset
-        if ret:
-            if abs(error) <= 10: # 5
-                car_stop()
-                logger.info("calibrate_new:行进时 误差小于 10 直接停车")
-
-                ret, box = filter.get(label)
-                while not ret:
-                    ret, box = filter.get(label)
-
-                error = (box[0][2] + box[0][0] - 320) / 2 + offset
-                logger.info(f"calibrate_new:停车后的误差是{error}")
-                if abs(error) > 8:
-                    logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准")
-                    error = error * 3
-                    logger.error(f"error * 3{error}")
-                    if error > 0:
-                        by_cmd.send_distance_x(-10, int(error))
-                    else:
-                        by_cmd.send_distance_x(10, int(-error))
-                
-                break
-def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
-    # run_direc == 1 向前
-    if run_direc == 1:
-        by_cmd.send_speed_x(run_speed)
-    else:
-        by_cmd.send_speed_x(-run_speed)
-
-    while True:
-        ret, box = filter.get(label)
-        while not ret:
-            ret, box = filter.get(label)
-        error = (box[0][2] + box[0][0] - 320) / 2 + offset
-        if ret:
-            logger.info(f"当前误差:{error}, box[{box[0][2]},{box[0][0]}]")
-            # 校准速度越大 停车的条件越宽泛
-            if abs(error) <= 20:
-                car_stop()
-                logger.info("explore_calibrate_new:行进时 误差小于 10 直接停车")
-                
-                error_sum = 0
-                for _ in range(3):
-                    ret, box = filter.get(label)
-                    while not ret:
-                        ret, box = filter.get(label)
-                    error = (box[0][2] + box[0][0] - 320) / 2 + offset
-                    error_sum += error
-                error_sum /= 3
-                # logger.info(f"停车后像素误差:{error}")
-                logger.info(f"停车后像素误差:{error_sum}")
-                # if abs(error) > 8:
-                logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
-                error_sum = error_sum * 3
-                logger.error(f"error * 3 {error}")
-                if error > 0:
-                    by_cmd.send_distance_x(-10, int(error))
-                else:
-                    by_cmd.send_distance_x(10, int(-error))
-                
-                break
-
-offset = 10
-# by_cmd.send_angle_claw_arm(217)
-# while True:
-#     pass
-# while (by_cmd.send_angle_camera(180) == -1):
-#     by_cmd.send_angle_camera(180)
-while (by_cmd.send_angle_camera(180) == -1):
-    by_cmd.send_angle_camera(180)
-# by_cmd.send_speed_x(15)
 
 find_counts = 0
-label = tlabel.HOSPITAL
-record = CountRecord(5)
+offset = 22
+label = [tlabel.LPILLER, tlabel.MPILLER, tlabel.SPILLER]
 while True:
     time.sleep(0.2)
+    ret, box = filter.get(tlabel.TPLATFORM)
+    if ret:
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        logger.error(error)
+
+    # label = tlabel.HOSPITAL
+    # ret, box = filter.get(label)
+    # while not ret:
+    #     ret, box = filter.get(label)
+    # width = box[0][2] - box[0][0]
+    # logger.info(width)
     
 
-    # ret = filter.find(label)
-    # ret = filter.find(label)
-    # if ret > 0:
-    #     find_counts += 1
-    #     if record(label):
-    #         car_stop()
-    # if find_counts >= 5:
-    #     car_stop()
-    ret, box = filter.get(label)
-    while not ret:
-        ret, box = filter.get(label)
-    width = box[0][2] - box[0][0]
-    logger.info(width)
-    # error = (box[0][2] + box[0][0] - 320) / 2 + offset
-    # explore_calibrate_new(tlabel.LPILLER, offset = 10, run_direc = 1, run_speed = 5)
-    # calibrate_new(label, offset = 15, run = True)
-    # car_stop()
-    # time.sleep(0.5)
-    # for _ in range(3):
-    #     calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4)
-    #     logger.info("抓蓝色球")
-    #     time.sleep(5)
-    # logger.info("抓取块")
-    # time.sleep(0.1)
-

test/test_ocr.py

@@ -0,0 +1,32 @@
+import zmq
+
+
+context = zmq.Context()
+socket = context.socket(zmq.REQ)
+socket.connect("tcp://localhost:6668")
+
+filter_w = (213, 540)
+filter_h = (120, 360)
+
+
+while True:
+    a = input("")
+    socket.send_string("")
+    resp = socket.recv_pyobj()
+    print(resp)
+    if resp.get('code') == 0:
+        text = ''
+        for item in resp.get('content'):
+            if item['probability']['average'] < 0.90:
+                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)
+
+
+    

utils.py

@@ -190,6 +190,27 @@ class label_filter:
                     return True 
         return False
     '''
+    description: 判断传入的多目标标签是否存在，存在返回 True
+    param {*} self
+    param {*} tlabel
+    return {[bool]}
+    '''    
+    def find_mult(self, tlabel):
+        response = self.get_resp()
+        find_result = []
+        if response['code'] == 0:
+            ret, results = self.filter_box(response['data'])
+            if ret:
+                for label in tlabel:
+                    expect_boxes = (results[:, 0] == label.value)
+                    boxes = results[expect_boxes, :]
+                    if len(boxes) != 0:
+                        find_result.append(True)
+                    else:
+                        find_result.append(False)
+                return find_result
+        return [False for _ in range(len(tlabel))]
+    '''
     description: 根据传入的标签，寻找画面中最左侧的并返回 error
     param {*} self
     param {*} tlabel
@@ -260,8 +281,10 @@ class LLM:
                         发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
                         向右走 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}],
+                        鸣叫 3 声 [{'func': 'beep', 'time': 3}]
+                        前行零点五米 [{'func': 'move', 'x': 0.5, 'y': 0}]
                     '''
-        self.prompt += '''你无需回复我'''
+        self.prompt += '''你只需要根据我的示例解析出指令即可，不要给我其他多余的回复;再次强调 你无需给我其他多余的回复 这对我很重要'''
         self.messages = []
         self.resp = None
         worker = threading.Thread(target=self.reset, daemon=True)  
@@ -287,7 +310,7 @@ class LLM:
         )
         self.messages.append(self.resp.to_message())
         resp = self.resp.get_result().replace(' ', '').replace('\n', '').replace('\t', '')
-        return resp[7:-3]
+        return resp
 
 class CountRecord:
     def __init__(self, stop_count=2) -> None:

variable.py

@@ -10,3 +10,10 @@ pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0}
 
 # 转向 pid 对象
 pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=50)
+
+llm_text = ''
+
+skip_llm_task_flag = False
+
+first_block = None
+second_block = None