pref: 修改子任务为类

main.py

@@ -8,19 +8,19 @@ import subtask as sb
 cfg_main = toml.load('cfg_main.toml')
 
 # 配置日志输出
-logger.add(cfg_main['debug'].get('logger_filename'), format=cfg_main['debug'].get('logger_format'), level="INFO")
+logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
 
-task_queue = queue.Queue()  
+task_queue = queue.Queue()
 
-task_queue.put(sb.task(sb.get_block, cfg_main['task'].get('GetBlock_enable'))) 
+task_queue.put(sb.task(sb.get_block.exec, sb.get_block.find, cfg_main['task']['GetBlock_enable'])) 
-task_queue.put(sb.task(sb.put_block, cfg_main['task'].get('PutBlock_enable'))) 
+task_queue.put(sb.task(sb.put_block.exec, sb.put_block.find, cfg_main['task']['PutBlock_enable'])) 
-task_queue.put(sb.task(sb.get_bball, cfg_main['task'].get('GetBBall_enable'))) 
+task_queue.put(sb.task(sb.get_bball.exec, sb.get_bball.find, cfg_main['task']['GetBBall_enable'])) 
-task_queue.put(sb.task(sb.up_tower, cfg_main['task'].get('UpTower_enable'))) 
+task_queue.put(sb.task(sb.up_tower.exec, sb.up_tower.find, cfg_main['task']['UpTower_enable'])) 
-task_queue.put(sb.task(sb.get_rball, cfg_main['task'].get('GetRBall_enable'))) 
+task_queue.put(sb.task(sb.get_rball.exec, sb.get_rball.find, cfg_main['task']['GetRBall_enable'])) 
-task_queue.put(sb.task(sb.put_bball, cfg_main['task'].get('PutBBall_enable'))) 
+task_queue.put(sb.task(sb.put_bball.exec, sb.put_bball.find, cfg_main['task']['PutBBall_enable'])) 
-task_queue.put(sb.task(sb.put_hanoi, cfg_main['task'].get('PutHanoi_enable'))) 
+task_queue.put(sb.task(sb.put_hanoi.exec, sb.put_hanoi.find, cfg_main['task']['PutHanoi_enable'])) 
-task_queue.put(sb.task(sb.move_area, cfg_main['task'].get('MoveArea_enable'))) 
+task_queue.put(sb.task(sb.move_area.exec, sb.move_area.find, cfg_main['task']['MoveArea_enable'])) 
-task_queue.put(sb.task(sb.kick_ass, cfg_main['task'].get('KickAss_enable'))) 
+task_queue.put(sb.task(sb.kick_ass.exec, sb.kick_ass.find, cfg_main['task']['KickAss_enable'])) 
 
 task_queuem_t = sb.task_queuem(task_queue)
 
@@ -37,6 +37,7 @@ while task_queuem_t.busy is True:
         pass
     else:
         # 模拟执行回归任务
-        logger.info("***** sim huigui task *****")
+        # logger.info("***** sim huigui task *****")
+        pass
 
 logger.info("Main thread exit")

mtask.py

@@ -0,0 +1,66 @@
+from simple_pid import PID
+
+class PidWrap:
+    def __init__(self, kp, ki, kd, setpoint=0, output_limits=1):
+        self.pid_t = PID(kp, ki, kd, setpoint, output_limits=(0-output_limits, output_limits))
+    def set_target(self, target):
+        self.pid_t.setpoint = target
+    def set(self, kp, ki, kd):
+        self.pid_t.kp = kp
+        self.pid_t.ki = ki
+        self.pid_t.kd = kd
+    def get(self, val_in):
+        return self.pid_t(val_in)
+    
+class main_task():
+    def __init__(self,socket):
+        self.lane_socket = socket
+
+        # 赛道回归相关
+        self.x = 0
+        self.y = 0
+        self.error_counts = 0
+        self.lane_error = 0
+
+        # 车控制对象初始化
+        # self.by_cmd = by_cmd_py()
+
+        # 转向 pid
+        self.pid1 = PidWrap(0.7, 0, 0,output_limits=40)
+        self.pid1.set_target(0)
+
+    def parse_data(self,data):
+        if data.get('code') == 0:
+            if data.get('type') == 'infer':
+                self.x += data.get('data')[0][0]
+                self.y += data.get('data')[0][1]
+                self.error_counts += 1
+            
+        else:
+            pass
+    def run(self):
+        try:
+            data = self.queen.get_nowait()
+            self.parse_data(data)
+        except:
+            pass
+        # 运行巡线任务
+        self.lane_task()
+            
+    def lane_task(self):
+        # TODO 巡航参数从配置文件中读取
+        if self.error_counts > 2:
+            self.x = self.x / 3
+            self.y = self.y / 3
+            self.lane_error = self.x - 160
+            self.error_counts = 0
+            self.x = 0
+            self.y = 0
+            if self.lane_error > 30:
+                self.pid1.set(0.7, 0, 0)
+            else:
+                self.pid1.set(0.5, 0, 0)
+            # self.by_cmd.send_speed_x(7)
+            pid_out = self.pid1.get(self.lane_error)
+            # self.by_cmd.send_speed_omega(pid_out)
+        self.lane_socket.send_string("infer")

subtask.py

@@ -4,22 +4,23 @@ import time
 
 # 任务类
 class task:  
-    def __init__(self, task_operation, enable=True):  
+    def __init__(self, func_exec, func_find, enable=True):  
         self.enable = enable
-        self.task_operation = task_operation  
+        self.func_exec = func_exec
-    def check(self):
+        self.func_find = func_find  
-        # 检查该任务是否需要执行
+    def find(self):
+        # 检查该任执行标志
         # TODO 完善该接口，是否需要单独为每种 task 编写一个函数，还是设置一个通用的过滤器（从 detection 模块过滤结果）
-        ret = True
+        while self.func_find() is False:
-        return ret
+            pass
-    def execute(self):  
+    def exec(self):  
         # 根据标志位确定是否执行该任务
         if self.enable is True:
-            logger.info(f"[Task]#Executing task \"{self.task_operation.__name__}\"")  
+            logger.debug(f"[Task ]# Executing task")  
-            self.task_operation()  
+            self.func_exec()  
-            logger.info(f"[Task]#Task \"{self.task_operation.__name__}\" completed.")  
+            logger.debug(f"[Task ]# Task completed")  
         else:
-            logger.warning(f"[Task]#Skip task \"{self.task_operation.__name__}\"")
+            logger.warning(f"[Task ]# Skip task")
 
 # 任务队列状态类
 class task_queuem_status(Enum):
@@ -35,13 +36,13 @@ class task_queuem(task):
         self.queue = queue
         self.status = task_queuem_status.IDEL
         self.busy = True
-        logger.info(f"[TaskM]#Task num {self.queue.qsize()}")
+        logger.info(f"[TaskM]# Task num {self.queue.qsize()}")
     def exec(self):
         # 如果空闲状态则将下一个队列任务取出
         if self.status is task_queuem_status.IDEL:
             if self.queue.qsize() == 0:
                 self.busy = False
-                logger.info(f"[TaskM]#Task queue empty, exit")
+                logger.info(f"[TaskM]# Task queue empty, exit")
                 return False
             self.task_now = self.queue.get()
 
@@ -51,58 +52,85 @@ class task_queuem(task):
             else:
                 self.status = task_queuem_status.EXECUTING
 
-            logger.info(f"[TaskM]#Start process task \"{self.task_now.task_operation.__name__}\" >>>>")
+            logger.info(f"[TaskM]# ---------------------->>>>")
         # 阻塞搜索任务标志位
         elif self.status is task_queuem_status.SEARCHING:
-            logger.info(f"[TaskM]#Start searching task target")
+            logger.info(f"[TaskM]# Start searching task target")
-            while self.task_now.check() is False: # TODO 增加超时处理
+            self.task_now.find()
-                break
             self.status = task_queuem_status.EXECUTING
         # 执行任务函数
         elif self.status is task_queuem_status.EXECUTING:
-            logger.info(f"[TaskM]#Start execute task function")
+            logger.info(f"[TaskM]# Start execute task function")
-            self.task_now.execute()                 # 执行当前任务函数
+            self.task_now.exec()                 # 执行当前任务函数
             self.queue.task_done()                  # 弹出已执行的任务
             self.status = task_queuem_status.IDEL   #
-            logger.info(f"[TaskM]#finish process task {self.task_now.task_operation.__name__} <<<<")
+            logger.info(f"[TaskM]# <<<<----------------------")
         return True
 
 
 
 
 # 人员施救
-def get_block():
+class get_block():
-    pass
+    def find():
+        return True
+    def exec():
+        pass
+
 
 # 紧急转移
-def put_block():
+class put_block():
-    pass
+    def find():
+        return False
+    def exec():
+        pass
 
 # 整装上阵
-def get_bball():
+class get_bball():
-    pass
+    def find():
+        return False
+    def exec():
+        pass
 
 # 通信抢修
-def up_tower():
+class up_tower():
-    pass
+    def find():
+        return False
+    def exec():
+        pass
 
 # 高空排险
-def get_rball():
+class get_rball():
-    pass
+    def find():
+        return False
+    def exec():
+        pass
 
 # 派发物资
-def put_bball():
+class put_bball():
-    pass
+    def find():
+        return False
+    def exec():
+        pass
 
 # 物资盘点
-def put_hanoi():
+class put_hanoi():
-    pass
+    def find():
+        return False
+    def exec():
+        pass
 
 # 应急避险
-def move_area():
+class move_area():
-    pass
+    def find():
+        return False
+    def exec():
+        pass
 
 # 扫黑除暴
-def kick_ass():
+class kick_ass():
-    pass
+    def find():
+        return False
+    def exec():
+        pass