70 lines
2.9 KiB
Python
70 lines
2.9 KiB
Python
import toml
|
|
import queue
|
|
import threading
|
|
from loguru import logger
|
|
import subtask as sb
|
|
import majtask as mj
|
|
from by_cmd_py import by_cmd_py
|
|
import time
|
|
|
|
cmd_py_obj = by_cmd_py()
|
|
sb.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")
|
|
|
|
# 向任务队列添加任务
|
|
task_queue = queue.Queue()
|
|
task_queue.put(sb.task(sb.get_block, 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'] and cfg_main['task']['PutBlock_enable']))
|
|
task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable']))
|
|
task_queue.put(sb.task(sb.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'], False)) # 无论是否进行任务，检测标识并转向都是必须进行的
|
|
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.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable']))
|
|
task_queue.put(sb.task(sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable']))
|
|
task_queue.put(sb.task(sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable']))
|
|
|
|
# 将任务队列传入调度模块中
|
|
task_queuem_t = sb.task_queuem(task_queue)
|
|
|
|
# 创建任务队列的工作线程
|
|
def worker_thread():
|
|
while task_queuem_t.exec() is True:
|
|
pass
|
|
|
|
# 启动工作线程
|
|
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)
|
|
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:
|
|
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)
|
|
|
|
logger.info("Main thread exit")
|