国赛出发前版本

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

.gitignore

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

action.py

@@ -1,6 +1,8 @@
 import threading
 import time
 import zmq
+import queue
+import toml
 import numpy as np
 from loguru import logger
 from simple_pid import PID
@@ -8,13 +10,25 @@ from utils import PidWrap
 
 bycmd = None
 move = None
+axis = None
+cmd = None
 
-def import_obj(_bycmd):
+action_run_flag = None
+
+cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml')
+
+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()
 
 class lane_cls:
     def __init__(self, _speed, _time, _pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0}) -> None:
@@ -88,12 +102,163 @@ class move_cls:
 
 # TODO 新运动指令类，指令的发送和完成查询功能，发送时开启新线程
 
-#  TODO 轴控制指令类，增加指令后加入动作队列，非阻塞
 
+class cmd_cls():
+    # FIXME 延时计算时没有关联轴速度
+    def __init__(self) -> None:
+        # 根据设备上电复位后实际位置设置
+        self.__axis_x_pos = 160
+        self.__axis_z_pos = 100
+        # self.__axis_claw_pos = 0
+        # self.__axis_claw_arm_pos = 0
+        # self.__axis_storage_pos = 0
+        pass
+    def z(self, _speed, _dis, _time_via = -1):
+        # 设置绝对位置
+        self.__axis_z_pos += _dis
+        # 如果未指定延时时间，则按照相对位移计算延时时间
+        if _time_via == -1:
+            time_via = abs(_dis) * cfg["axis"]["axis_z_time_via_alpha"]
+        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 and action_run_flag.isSet():
+            pass  
+        time.sleep(time_via)
+    def z2(self, _speed, _pos, _time_via = -1):
+        # 计算绝对位置
+        _dis = self.__axis_z_pos - _pos
+        self.__axis_z_pos = _pos
+        # 如果未指定延时时间，则按照相对位移计算延时时间
+        if _time_via == -1:
+            time_via = abs(_dis) * cfg["axis"]["axis_z_time_via_alpha"]
+        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 and action_run_flag.isSet():
+            pass  
+        time.sleep(time_via)
+    def x(self, _speed, _dis, _time_via = -1):
+        # 设置绝对位置
+        self.__axis_x_pos += _dis
+        # 如果未指定延时时间，则按照相对位移计算延时时间
+        if _time_via == -1:
+            time_via = abs(_dis) * cfg["axis"]["axis_z_time_via_alpha"]
+        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 and action_run_flag.isSet():
+            pass  
+        time.sleep(time_via)
+    def x2(self, _speed, _pos, _time_via = -1):
+        # 计算绝对位置
+        _dis = self.__axis_x_pos - _pos
+        self.__axis_x_pos = _pos
+        # 如果未指定延时时间，则按照相对位移计算延时时间
+        if _time_via == -1:
+            time_via = abs(_dis) * cfg["axis"]["axis_x_time_via_alpha"]
+        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 and action_run_flag.isSet():
+            pass  
+        time.sleep(time_via)
+    def camera(self, angle):
+        while bycmd.send_angle_camera(angle) == -1 and action_run_flag.isSet():
+            pass
+    def claw(self, angle):
+        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 and action_run_flag.isSet():
+            pass
+        time.sleep(1)
+    def scoop(self, angle):
+        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 and action_run_flag.isSet():
+            pass
+        time.sleep(0.5)
+    def zhuan(self, angle):
+        while bycmd.send_angle_zhuan(angle) == -1 and action_run_flag.isSet():
+            pass
+        time.sleep(0.5)
+    def wait(self, _time):
+        time.sleep(_time)
+
+# TODO 增加任务队列中非阻塞控制
 class axis_cls():
-    def axis_z(self, _distance):
+    def __init__(self) -> None:
+        self.axis_queue = queue.Queue()
+        self.busy = False
         pass
-    def axis_z2(self, _position):
+    def z(self, _distance, _time_via = -1):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.z(20, _distance, _time_via))
         pass
-    def exec(self):
+    def z2(self, _position, _time_via = -1):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.z2(20, _position, _time_via))
+        pass
+    def x(self, _distance, _time_via = -1):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.x(1, _distance, _time_via))
+        pass
+    def x2(self, _position, _time_via = -1):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.x2(1, _position, _time_via))
+        pass
+    def camera(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.camera(_angle))
+        pass
+    def claw(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.claw(_angle))
+        pass
+    def claw_arm(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.claw_arm(_angle))
+        pass
+    def scoop(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.scoop(_angle))
+        pass
+    def storage(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.storage(_angle))
+        pass
+    def wait(self, _time):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.wait(_time))
+    def pop(self):
+        self.busy = True
+        while self.axis_queue.qsize() > 0:
+            logger.info(f"axis cmd {self.axis_queue.qsize()}")
+            self.axis_queue.get()()
+            self.axis_queue.task_done()
+            time.sleep(0.005)
+        self.busy = False
+        pass
+    def exec(self, _block:bool = True):
+        while self.busy is True:
+            pass
+        if _block is True:
+            self.pop()
+        else:
+            thread = threading.Thread(target=self.pop)
+            thread.start()
         pass

app.py

@@ -0,0 +1,216 @@
+from flask import Flask, render_template, request
+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 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/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
+# FIXME 在列表里存所有的 task_process  
+task_process = None
+task_run_flag = threading.Event()
+
+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_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('/run')
+def run():
+    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):
+    global server_process
+    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)
+        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':
+            task_run_flag.set()
+            # 开启 task 进程前先关闭所有历史进程
+            if task_process != None:
+                task_process.terminate()
+
+            time_record = time.perf_counter()
+            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_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':
+            pass
+    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)
+    # 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():
+    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})
+    socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag.isSet())})
+def thread_function():
+    global queue
+    while True:
+        try:
+            log = queue.get()
+            socketio.emit('log', log)
+        except multiprocessing.Queue.Empty:
+            pass
+
+    
+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)
+    # 启动所有脚本
+    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)
+
+    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()
+    for process in processes:
+        logger.error(process.pid)
+        os.kill(process.pid, signal.SIGINT)

cfg_action.toml

@@ -0,0 +1,4 @@
+[axis]
+# 轴运动延时系数
+axis_z_time_via_alpha = 0.05
+axis_x_time_via_alpha = 0.01

cfg_args.toml

@@ -0,0 +1,14 @@
+[lane_mode]
+mode_index = 1
+
+[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

cfg_main.toml

@@ -1,30 +1,19 @@
 [debug]
 logger_filename = "log/file_{time}.log"
-logger_format = "{time} {level} {message}"
+logger_format = "[{level}] {file}:{line} <{time}> {message}"
+
 
-[task]
-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 = 10 # 人员施救计数
-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 = 12
+GetBBall_counts = 5
+UpTower_counts = 3
+GetRBall_counts = 10
+PutBBall_counts = 15
+PutHanoi1_counts = 7
+PutHanoi2_counts = 2
+PutHanoi3_counts = 2
+MoveArea1_counts = 6
+MoveArea2_counts = 10
+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,27 +1,61 @@
 [get_block]
-# first_block = "red"
+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]
-pos_gap = 160 # 标定值，两个放置位置的标定距离 
-pos_lp = 2    # 1\2\3 数字越小越靠近红色置物区
-pos_mp = 1
+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]
-llm_enable = false # 大模型机器人
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = true
 
 [kick_ass]
-pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
-pos_gap2 = 80 # person 之间的距离
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
 target_person = 1

cfg_subtask.toml.6_9.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.1
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 0.8
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.6
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 2.0
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 1.3
+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.7126.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.3
+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.3
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 1.3
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.4
+pid_ki = 0
+pid_kd = 0
+llm_enable = false
+
+[kick_ass]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

cfg_subtask.toml.7131.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.3
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 0.8
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.6
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 2.5
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 1.3
+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.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.bak.7102

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 2.0
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 2.0
+pid_ki = 0
+pid_kd = 0.1
+
+[get_bball]
+pid_kp = 2.0
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 2.8
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 2.0
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 2.4
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1.2
+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 = 2.0
+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.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

@@ -1,40 +1,47 @@
 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)
+act.axis.scoop(25)
+act.axis.exec()
 
 # 向任务队列添加任务
 task_queue = queue.Queue()
-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'])) 
-
+# 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)
 
@@ -46,22 +53,12 @@ def worker_thread():
 # 启动工作线程
 worker = threading.Thread(target=worker_thread, daemon=True)  
 worker.start()  
-# if (cmd_py_obj.send_angle_camera(180) == -1):
-#     cmd_py_obj.send_angle_camera(180)
-cmd_py_obj.send_position_axis_x(1, 140)
-time.sleep(0.5)
-cmd_py_obj.send_angle_storage(20)
-time.sleep(0.5)
-cmd_py_obj.send_angle_scoop(25)
-time.sleep(2)
+
 
 # 创建主任务
 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:
@@ -75,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,116 @@
+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(_run_flag,_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):
+        _run_flag.clear()
+    signal.signal(signal.SIGTERM, signal_handler)
+
+
+    cmd_py_obj = by_cmd_py(_queue)
+    sb.import_obj(cmd_py_obj, _skip_queue)
+    # act.import_obj(_run_flag, cmd_py_obj)
+
+    # 读取配置
+    cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml')
+    cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.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")
+    logger.info(cfg_args)
+
+    cmd_py_obj.send_angle_camera(0)
+    cmd_py_obj.send_position_axis_x(1, 140)
+    cmd_py_obj.send_angle_storage(20)
+    cmd_py_obj.send_angle_scoop(25)
+    # cmd_py_obj.send_angle_claw_arm(225)
+    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)
+    
+    # 向任务队列添加任务
+    task_queue = queue.Queue()
+    if cfg_args['task']['Subtask_enable'] is True:
+        task_queue.put(sb.task("人员施救第一块", sb.get_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_args['task']['GetBlock_enable'])) 
+        task_queue.put(sb.task("人员施救第二块", sb.get_block2, cfg_main['find_counts']['GetBlock_counts'], cfg_args['task']['GetBlock_enable'])) 
+        task_queue.put(sb.task("紧急转移", sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_args['task']['GetBlock_enable'])) 
+        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.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'])) 
+        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_args['task']['MoveArea_enable'])) 
+        task_queue.put(sb.task("应急避险二阶段", sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_args['task']['MoveArea_enable'])) 
+        task_queue.put(sb.task("扫黑除暴", sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_args['task']['KickAss_enable'])) 
+    # 将任务队列传入调度模块中
+    task_queuem_t = sb.task_queuem(task_queue)
+    # 创建任务队列的工作线程
+    def worker_thread():
+        while task_queuem_t.exec(_skip_queue) and _run_flag.isSet():
+            pass
+
+    # 启动工作线程
+    worker = threading.Thread(target=worker_thread, daemon=True)  
+    worker.start()  
+
+
+    # 创建主任务
+    main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
+    try:
+        while _run_flag.isSet():
+            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__':
+    from multiprocessing import Queue
+    temp = Queue()
+    main_func(None, temp)

majtask.py

@@ -1,15 +1,21 @@
 
 import zmq
 import time
+import numpy as np
 from loguru import logger
 import utils
-from utils import PidWrap
+import variable as var
     
 class main_task():
     def __init__(self,by_cmd):
+        # lane infer server
         self.context = zmq.Context()
         self.socket = self.context.socket(zmq.REQ)
         self.socket.connect("tcp://localhost:6666")
+        # lane infer server1
+        # self.context1 = zmq.Context()
+        # self.socket1 = self.context.socket(zmq.REQ)
+        # self.socket1.connect("tcp://localhost:6669")
 
         # 赛道回归相关
         self.x = 0
@@ -21,16 +27,18 @@ class main_task():
         self.by_cmd = by_cmd
 
         # 转向 pid
-        # self.pid1 = PidWrap(0.5, 0, 0, output_limits=55) # 1.2
-        self.pid1 = PidWrap(1.2, 0, 0, output_limits=50) # 1.2
-        self.pid1.set_target(0)
+        # var.pid_turning = PidWrap(0.5, 0, 0, output_limits=55) # 1.2
+        # pid 参数在
+        var.pid_turning.set_target(0)
 
     def parse_data(self,data):
         if data.get('code') == 0:
-            self.x += data.get('data')[0]
-            self.y += data.get('data')[1]
-            self.error_counts += 1
-            
+            ck_val = data.get('data')
+            # logger.debug(ck_val)
+            if isinstance(ck_val, np.ndarray):  
+                self.x += ck_val[0]
+                self.y += ck_val[1]
+                self.error_counts += 1
         else:
             pass
 
@@ -52,24 +60,25 @@ class main_task():
             self.x = self.x / 3
             self.y = self.y / 3
             self.lane_error = self.x - 160
-            utils.lane_error = self.lane_error # 赋全局变量
+            var.lane_error = self.lane_error # 赋全局变量
             self.error_counts = 0
             self.x = 0
             self.y = 0
             error_abs = abs(self.lane_error)
 
             if error_abs > 50:
-                speed = 15
+                # speed = 11
+                speed = 13
             elif error_abs > 45:
                 speed = 15
             elif error_abs > 35:
-                speed = 15
+                speed = 17
             elif error_abs > 25:
-                speed = 15
-            elif error_abs > 15:
-                speed = 15
-            else:
                 speed = 18
+            elif error_abs > 15:
+                speed = 19
+            else:
+                speed = 21
 
             # lane_model initial
             # if error_abs > 50:
@@ -85,16 +94,20 @@ class main_task():
             # else:
             #     speed = 18
 
-            if utils.task_speed != 0:
-                speed = utils.task_speed
+            if var.task_speed != 0:
+                speed = var.task_speed
             self.by_cmd.send_speed_x(speed)
-            # pid_out = self.pid1.get(self.lane_error*0.65)
-            pid_out = self.pid1.get(self.lane_error)
+            # pid_out = var.pid_turning.get(self.lane_error*0.65)
+            pid_out = var.pid_turning.get(self.lane_error)
             # pid_out = -pid_out
             # logger.debug(f"err={self.lane_error}, pwm out={pid_out}")
             self.by_cmd.send_speed_omega(pid_out)
-            
+        # if var.switch_lane_model:
+        #     self.socket1.send_string("")
+        #     resp = self.socket1.recv_pyobj()
+        # else:
+        #     self.socket.send_string("")
+        #     resp = self.socket.recv_pyobj()
         self.socket.send_string("")
         resp = self.socket.recv_pyobj()
-        # logger.info(resp)
         self.parse_data(resp)

templates/csdn.html

@@ -0,0 +1,112 @@
+
+<!DOCTYPE html>
+<html lang="en">
+<head>
+    <meta charset="UTF-8">
+    <meta name="viewport" content="width=device-width, initial-scale=1.0">
+    <title>第十九届全国大学生智能汽车竞赛 百度智慧交通创意组 “风雨同舟”比赛规则_全国大学生智能汽车竞赛 智慧交通创意-CSDN博客</title>
+    <script src="/static/vue.js"></script>
+    <script src="static/socket.io.js"></script>
+    <style>
+        body, html {
+            margin: 0;
+            padding: 0;
+            height: 100%;
+            overflow: hidden;
+        }
+        #app {
+            width: 100vw;
+            height: 100vh;
+            overflow: hidden;
+            position: relative;
+        }
+        .background-container {
+            width: 100%;
+            height: 100%;
+            background-size: cover;
+            background-position: center;
+            transition: background-position 0.1s ease-out;
+        }
+        .floating-buttons {
+            position: fixed;
+            bottom: 20px;
+            left: 0;
+            right: 0;
+            display: flex;
+            justify-content: center;
+            gap: 20px;
+            z-index: 10;
+        }
+        .floating-button {
+            width: 160px;
+            height: 40px;
+            border-radius: 20px;
+            background-color: #fc5531;
+            border: none;
+            color: white;
+            font-size: 16px;
+            cursor: pointer;
+            transition: background-color 0.3s;
+        }
+        .floating-button:hover {
+            background-color: #fc5531;
+        }
+    </style>
+</head>
+<body>
+    <div id="app">
+        <div class="background-container" 
+             :style="backgroundStyle" 
+             @mousemove="handleMouseMove" 
+             ref="container">
+        </div>
+        <div class="floating-buttons">
+            <button class="floating-button" @click="handleButton1Click">打开 CSDN APP</button>
+            <button class="floating-button" @click="handleButton2Click">小程序看全文</button>
+        </div>
+    </div>
+
+    <script>
+        new Vue({
+            el: '#app',
+            data: {
+                imageSrc: '/static/csdn.png',  // 使用大尺寸占位图片
+                backgroundPosition: '0%'
+            },
+            computed: {
+                backgroundStyle() {
+                    return {
+                        backgroundImage: `url(${this.imageSrc})`,
+                        backgroundPosition: `center ${this.backgroundPosition}`
+                    };
+                }
+            },
+            methods: {
+                handleMouseMove(event) {
+                    const containerRect = this.$refs.container.getBoundingClientRect();
+                    const mouseY = event.clientY - containerRect.top;
+                    const scrollPercent = (mouseY / containerRect.height) * 100;
+                    this.backgroundPosition = `${scrollPercent}%`;
+                },
+                handleButton1Click() {
+                    // alert('按钮 1 被点击了');
+                },
+                handleButton2Click() {
+                    this.socket.emit('operate', { type: 'skip_task', content: '' });
+                },
+                onImageLoad() {
+                    console.log('背景图片加载完成');
+                    // 这里可以添加图片加载完成后的额外操作
+                }
+            },
+            mounted() {
+                const img = new Image();
+                img.onload = this.onImageLoad;
+                img.src = this.imageSrc;
+                this.socket = io('http://' + document.domain + ':5001');
+
+            }
+        });
+    </script>
+</body>
+</html>

templates/index.html

@@ -0,0 +1,302 @@
+<!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">
+                    <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 === 'lane_mode'" v-model="config[sectionName][key]">
+                                        <el-option
+                                          v-for="item in target_person_options"
+                                          :key="item.value"
+                                          :label="item.label"
+                                          :value="item.value">
+                                        </el-option>
+                                    </el-select>
+                                    <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-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,
+                target_person_options: [{
+                  value: 1,
+                  label: '6_9'
+                }, {
+                  value: 2,
+                  label: '7_10_2'
+                }, {
+                  value: 3,
+                  label: '7_14_2'
+                }, {
+                  value: 4,
+                  label: '7_15_4'
+                }],
+            },
+            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>

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

@@ -0,0 +1,49 @@
+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
+import signal
+
+context = zmq.Context()
+socket = context.socket(zmq.REQ)
+socket.connect("tcp://localhost:6667")
+logger.info("subtask yolo client init")
+
+filter = label_filter(socket)
+filter.switch_camera(2)
+
+
+
+cmd_py_obj = by_cmd_py()
+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)
+def signal_handler(sig, frame):
+    car_stop()
+
+offset = 0
+signal.signal(signal.SIGTERM, signal_handler)
+cmd_py_obj.send_speed_x(15)
+while True:
+
+    time.sleep(0.02)
+    ret, box = filter.get(tlabel.BASE)
+    # if ret:
+        # 宽度大于 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
+        print(error)
+        cmd_py_obj.send_speed_omega(-error * 0.8)
+car_stop()

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,15 +1,15 @@
-
-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
 
+
+
 context = zmq.Context()
 socket = context.socket(zmq.REQ)
 socket.connect("tcp://localhost:6667")
@@ -17,35 +17,28 @@ logger.info("subtask yolo client init")
 
 filter = label_filter(socket)
 filter.switch_camera(1)
-offset = 12
+
+
+# find_counts = 0
+# offset = 19
+# label = [tlabel.LPILLER, tlabel.MPILLER, tlabel.SPILLER]
+# label = tlabel.TPLATFORM
 while True:
     time.sleep(0.2)
-    # ret1 = filter.get_mult(tlabel.RMARK)
-    # ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK])
-    # if ret:
-    #     # print(error)
-    #     if abs(error) < 40:
-    #         logger.info('yes')
-
-    label = tlabel.RBLOCK
-    ret, box = filter.get(label)
-    while not ret:
-        # 注意这里一定要保证摄像头内有该目标 否则会无限循环
-        ret, box = filter.get(label)
-    error = (box[0][2] + box[0][0] - 320) / 2 + offset
-    # ret, box = filter.get(tlabel.BBLOCK)
-    
+    ret, box = filter.get(tlabel.HOSPITAL)
     if ret:
-        print(error)
-    else:
-        pass
+        width = box[0][2] - box[0][0]
+        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)
+    # while not ret:
+    #     ret, box = filter.get(label)
+    # width = box[0][2] - box[0][0]
+    # logger.info(width)
     
 
-    # if ret1:
-    #     print(error1)
-    #     if abs(error1) < 30:
-    #         print("you")
-    # elif ret2:
-    #     print(error2)
-    #     if abs(error2) < 30:
-    #         print("zuo")

test/test_llm.py

@@ -13,16 +13,48 @@ class LLM:
     def __init__(self):
         
         self.model = 'ernie-3.5'
-        self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。
-                    严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！'''
-        self.prompt += '''正确的示例如下：
-                        向左移 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}],
-                        向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}],
-                        原地左转 38 度 [{'func': 'turn','angle': -38}],
-                        蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
-                        发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
+        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 以及相应的值'''
         self.messages = []
         self.resp = None
         self.reset()
@@ -47,6 +79,8 @@ if __name__ == "__main__":
     lmm_bot = LLM()
     while True:
         chat = input("输入：")
-        print(lmm_bot.get_command_json(chat))
+        resp = lmm_bot.get_command_json(chat).replace(' ', '').replace('\n', '').replace('\t', '')
+
+        print(eval(resp[7:-3]))
 
 

test/test_ocr.py

@@ -0,0 +1,26 @@
+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.80:
+                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,7 +2,10 @@
 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
 
 # 巡线误差
 lane_error = 0
@@ -104,6 +107,53 @@ class label_filter:
             if len(results) > 0:
                 return True, np.array(results)
         return False, None
+    # '''
+    # description: 对模型推理推理结果使用 threshold 和其他条件过滤 默认阈值为 0.5
+    # param {*} self
+    # param {*} data get_resp 返回的数据
+    # return {bool,array}
+    # '''    
+    # def filter_box_custom(self, data, ymax_range):  
+    #     if len(data) > 0:  
+    #         expect_boxes = (data[:, 1] > self.threshold) & (data[:, 0] > -1)  
+    #         np_boxes = data[expect_boxes, :]  
+    #         results = [  
+    #             [  
+    #                 item[0],  # 'label':  
+    #                 item[1],  # 'score':  
+    #                 item[2],  # 'xmin':  
+    #                 item[3],  # 'ymin':  
+    #                 item[4],  # 'xmax':  
+    #                 item[5],  # 'ymax':  
+    #                 not (ymax_range[0] < item[3] < ymax_range[1]), # 如果 ymin 处在范围内则返回 False，认为该目标不符合要求
+    #                 not (ymax_range[0] < item[5] < ymax_range[1]) # 如果 ymax 处在范围内则返回 False，认为该目标不符合要求
+    #             ]  
+    #             for item in np_boxes  
+    #         ]  
+    #         if len(results) > 0:  
+    #             return True, np.array(results)  
+    #     return False, None
+    
+    #原来的函数
+    def filter_box_custom(self,data):
+        if len(data) > 0:
+            expect_boxes = (data[:, 1] > self.threshold) & (data[:, 0] > -1)
+            np_boxes = data[expect_boxes, :]
+            results = [
+                [
+                    item[0], # 'label': 
+                    item[1], # 'score': 
+                    item[2], # 'xmin':
+                    item[3], # 'ymin':
+                    item[4], # 'xmax':
+                    item[5]  # 'ymax':
+                ]
+                for item in np_boxes
+                if item[5] < 180
+            ]
+            if len(results) > 0:
+                return True, np.array(results)
+        return False, None
     '''
     description: 根据传入的标签过滤，返回该标签的个数、box
     param {*} self
@@ -188,6 +238,95 @@ class label_filter:
                     return True 
         return False
     '''
+    description: 查询两个目标 只有 target_label 返回 box
+    param {*} self
+    param {*} tlabel
+    return {[bool]}
+    ''' 
+    def get_two(self, target_label, label):
+        response = self.get_resp()
+        if response['code'] == 0:
+            ret, results = self.filter_box_custom(response['data'])
+            if ret:
+                expect_boxes = (results[:, 0] == target_label.value)
+                boxes = results[expect_boxes, :]
+                if len(boxes) != 0:
+                    target_bool = True
+                    target_box = boxes[:, 2:]
+                else:
+                    target_bool = False
+                    target_box = None
+                expect_boxes = (results[:, 0] == label.value)
+                boxes = results[expect_boxes, :]
+                if len(boxes) != 0:
+                    label_bool = True
+                else:
+                    label_bool = False
+                return (target_bool, label_bool, target_box)
+        return (False, False, None)
+    
+    '''
+    description: 查询两个目标 只有 target_label 返回 box
+    param {*} self
+    param {*} tlabel
+    return {[bool]}
+    ''' 
+    def get_two_hanoi(self, target_label, label, flipv):
+        response = self.get_resp()
+        if response['code'] == 0:
+            # FIXME 直接在外部过滤，不在 fliter 内过滤
+            ret, results = self.filter_box(response['data'])
+            if ret:
+                expect_boxes = (results[:, 0] == target_label.value)
+                boxes = results[expect_boxes, :]
+                if len(boxes) != 0:
+                    target_bool = True
+                    target_box = boxes[:, 2:]
+                else:
+                    target_bool = False
+                    target_box = None
+                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:
+                    # 如果垂直翻转 (走右侧) 且 *ymin* 小于 60（走右侧）
+                    if flipv:
+                        label_bool = all(box[3] > 60  for box in boxes)
+                    # 如果不垂直翻转 (走左侧) 且 *ymax* 大于 180（走左侧）
+                    else:
+                        label_bool = all(box[5] < 180  for box in boxes)
+                    # label_bool = True
+                else:
+                    label_bool = False
+                return (target_bool, label_bool, target_box)
+        return (False, False, None)
+
+    '''
+    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
@@ -238,45 +377,236 @@ 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.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。
-                    严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！'''
-        self.prompt += '''正确的示例如下：
-                        向左移 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}],
-                        向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}],
-                        原地左转 38 度 [{'func': 'turn','angle': -38}],
-                        蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
-                        发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
+        self.model = 'ernie-lite'
+        # self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。
+        #             严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！'''
+        # self.prompt += '''正确的示例如下：
+        #                 向左移 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}],
+        #                 向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}],
+        #                 原地左转 38 度 [{'func': 'turn','angle': -38}],
+        #                 蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
+        #                 发光或者照亮 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 = '''
+        #                 你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 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
-        self.reset()
+        worker = threading.Thread(target=self.reset, daemon=True)  
+        worker.start()
     def reset(self):
-        self.messages = [self.make_message(self.prompt)]
-        self.resp = erniebot.ChatCompletion.create(
-                        model=self.model,
-                        messages=self.messages,
-                    )
-        self.messages.append(self.resp.to_message())
+        try:
+            self.messages = [self.make_message(self.prompt)]
+            self.resp = erniebot.ChatCompletion.create(
+                            model=self.model,
+                            messages=self.messages,
+                        )
+            self.messages.append(self.resp.to_message())
+            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())
-        return self.resp.get_result()
+        resp = self.resp.get_result().replace(' ', '')
+        return resp
 
 class CountRecord:
     def __init__(self, stop_count=2) -> None:
@@ -313,6 +643,7 @@ class PidWrap:
         self.pid_t.Kp = kp
         self.pid_t.Ki = ki
         self.pid_t.Kd = kd
+        logger.info(f"[PID]# 更新 PID 参数：Kp({kp:.2f}) Ki({ki:.2f}) Kd({kd:.2f})")
     def get(self, val_in):
         return self.pid_t(val_in)
 

variable.py

@@ -0,0 +1,22 @@
+from utils import PidWrap
+
+# 巡线误差
+lane_error = 0
+# 进入任务时可以通过修改 task_speed 控制巡线速度
+task_speed = 0
+
+# pid 参数
+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=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 = ''
+
+skip_llm_task_flag = False
+
+first_block = None
+second_block = None
+
+switch_lane_model = False