project_main/main_upper.py

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)