pref: 注册时直接传入任务类

feat: 分任务设置检测计数值

subtask.py

@@ -3,24 +3,52 @@ from loguru import logger
 from utils import label_filter
 from utils import tlabel
 import toml
+import zmq
 import time
 
+context = zmq.Context()
+socket = context.socket(zmq.REQ)
+socket.connect("tcp://localhost:6667")
+logger.info("socket init")
+
+
+by_cmd = None
+filter = None
+def import_obj(_by_cmd):
+    global by_cmd
+    global filter
+    by_cmd = _by_cmd
+    filter = label_filter(socket)
+
+
+
 # 任务类
 class task:  
-    def __init__(self, func_exec, func_find, enable=True):  
+    def __init__(self, task_template, find_counts=10, enable=True):  
         self.enable = enable
-        self.func_exec = func_exec
-        self.func_find = func_find  
+        self.task_t = task_template()
+
+        self.counts = 0
+        self.find_counts = find_counts
+    def init(self):
+        self.task_t.init()
     def find(self):
         # 检查该任执行标志
-        # TODO 完善该接口，是否需要单独为每种 task 编写一个函数，还是设置一个通用的过滤器（从 detection 模块过滤结果）
-        while self.func_find() is False:
-            pass
+        while True:
+            # if self.func_find():
+            if self.task_t.find():
+                self.counts += 1
+            if self.counts >= self.find_counts:
+                break
+        # while self.func_find() is False:
+        #     pass
+
     def exec(self):  
         # 根据标志位确定是否执行该任务
         if self.enable is True:
             logger.debug(f"[Task ]# Executing task")  
-            self.func_exec()  
+            # self.func_exec()  
+            self.task_t.exec()
             logger.debug(f"[Task ]# Task completed")  
         else:
             logger.warning(f"[Task ]# Skip task")
@@ -31,7 +59,7 @@ class task_queuem_status(Enum):
     SEARCHING = 1
     EXECUTING = 2
 
-# 任务队列类 非EXECUTEING 时均执行 huigui，注意互斥操作
+# 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作
 class task_queuem(task):
     # task_now = task(None, False)
     def __init__(self, queue):
@@ -52,9 +80,10 @@ class task_queuem(task):
             # 如果当前任务没有使能，则直接转入执行状态，由任务执行函数打印未执行信息
             if self.task_now.enable is True:
                 self.status = task_queuem_status.SEARCHING
+                # 如果使能该任务则执行该任务的初始化动作
+                self.task_now.init()
             else:
                 self.status = task_queuem_status.EXECUTING
-
             logger.info(f"[TaskM]# ---------------------->>>>")
         # 阻塞搜索任务标志位
         elif self.status is task_queuem_status.SEARCHING:
@@ -75,143 +104,367 @@ class task_queuem(task):
 
 # 人员施救
 class get_block():
-    def find():
+    def init(self):
+        logger.info("人员施救初始化")
+    def find(self):
         # 目标检测红/蓝方块
-        filter = label_filter(None)
         ret1, list1 = filter.get(tlabel.RBLOCK)
-        ret2, list2 = filter.get(tlabel.BBLOCK)
-
-        if (ret1 > 0) or (ret2 > 0):
-            logger.info("[TASK1]# find label")
+        if ret1 > 0:
+            logger.info("[抓方块]# find label")
             return True
         else:
             return False
-    def exec():
+    def exec(self):
+        for _ in range(3):
+            by_cmd.send_speed_x(7)
+            by_cmd.send_speed_omega(0)
+            time.sleep(0.1)
+        logger.info("abcd")
         cfg = toml.load('cfg_subtask.toml') # 加载任务配置
+        while True:
+            # logger.info("等待进入准确区域")
+            ret, error = filter.aim_near(tlabel.RBLOCK)
+            while not ret:
+                ret, error = filter.aim_near(tlabel.RBLOCK)
+            # logger.info(error)
+            if abs(error) < 5:
+                for _ in range(3):
+                    by_cmd.send_speed_x(0)
+                    time.sleep(0.2)
+                    by_cmd.send_speed_omega(0)
+                break 
+        ret, error = filter.aim_near(tlabel.RBLOCK)
+        while not ret:
+            ret, error = filter.aim_near(tlabel.RBLOCK)
+        time.sleep(1)
+        logger.error(error)
+        if abs(error) > 5:
+            logger.info("校准中")
+            if error > 0:
+                by_cmd.send_distance_x(-10, int(error*3))
+            else:
+                by_cmd.send_distance_x(10, int(-error*3))
+                logger.error(error)
+            time.sleep(1)
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        time.sleep(2)
+        
+        by_cmd.send_position_axis_z(10, 150)
+        time.sleep(5)
+        by_cmd.send_angle_claw_arm(127)
+        time.sleep(1)
+        by_cmd.send_position_axis_x(4, 140)
+        time.sleep(4)
+        by_cmd.send_angle_claw_arm(220)
+        by_cmd.send_angle_claw(90)
+        time.sleep(1)
+        by_cmd.send_distance_axis_z(10, -70)
+        time.sleep(3)
+        by_cmd.send_angle_claw(27)
+        by_cmd.send_distance_axis_z(10, 10)
+        time.sleep(2)
+        by_cmd.send_distance_axis_x(4, -100)
+        time.sleep(1)
+        by_cmd.send_distance_axis_z(10, -40)
+        time.sleep(3)
+        by_cmd.send_angle_claw(35)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(10, 150)
+        time.sleep(3)
+        by_cmd.send_position_axis_x(2, 140)
+        #  抓取第二个块后 收爪 
+        time.sleep(3)
+        by_cmd.send_position_axis_x(4, 0)
+    def nexec(self):
         pass
 
 
 # 紧急转移
 class put_block():
-    def find():
+    def init(self):
+        logger.info("紧急转移初始化")
+    def find(self):
         # 目标检测医院
-        filter = label_filter(None)
         ret1, list1 = filter.get(tlabel.HOSPITAL)
         if ret1 > 0:
             return True
         else:
             return False
-    def exec():
+    def exec(self):
         cfg = toml.load('cfg_subtask.toml') # 加载任务配置
+        logger.info("找到医院")
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        
+        by_cmd.send_position_axis_z(10, 150)
+        time.sleep(3)
+        # TODO 切换爪子方向
+        by_cmd.send_position_axis_x(2, 140)
+        time.sleep(2)
+        by_cmd.send_position_axis_z(10, 170)
         pass
 
 # 整装上阵
 class get_bball():
-    def find():
+    def init(self):
+        by_cmd.send_position_axis_x(2, 140)
+        logger.info("整装上阵初始化")
+        time.sleep(0.5)
+        if (by_cmd.send_angle_camera(90) == -1):
+            by_cmd.send_angle_camera(90)
+    def find(self):
         # 目标检测黄球
-        filter = label_filter(None)
         ret1, list1 = filter.get(tlabel.YBALL)
         if ret1 > 0:
             return True
         else:
             return False
-    def exec():
+    def exec(self):
+        logger.info("找到黄色球")
+        for _ in range(3):
+            by_cmd.send_speed_x(7)
+            by_cmd.send_speed_omega(0)
+            time.sleep(0.1)
+        while True:
+            # logger.info("等待进入准确区域")
+            ret, error = filter.aim_near(tlabel.YBALL)
+            while not ret:
+                ret, error = filter.aim_near(tlabel.YBALL)
+            # logger.info(error)
+            if abs(error) < 5:
+                for _ in range(3):
+                    by_cmd.send_speed_x(0)
+                    time.sleep(0.2)
+                    by_cmd.send_speed_omega(0)
+                break 
+        ret, error = filter.aim_near(tlabel.YBALL)
+        while not ret:
+            ret, error = filter.aim_near(tlabel.YBALL)
+        time.sleep(1)
+        logger.error(error)
+        if abs(error) > 5:
+            logger.info("校准中")
+            if error > 0:
+                by_cmd.send_distance_x(-10, int(error*3))
+            else:
+                by_cmd.send_distance_x(10, int(-error*3))
+                logger.error(error)
+            time.sleep(1)
+        if (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+        by_cmd.send_position_axis_z(20, 160)
+        time.sleep(2)
+        by_cmd.send_position_axis_x(2, 70)
+        time.sleep(2)
+        by_cmd.send_angle_claw(90)
+        time.sleep(0.2)
+        by_cmd.send_position_axis_x(2, 0)
+        time.sleep(2)
+        by_cmd.send_angle_claw(27)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(20, 180)
+        time.sleep(1)
+        by_cmd.send_position_axis_x(4, 45)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(20, 140)
+        time.sleep(3)
+        by_cmd.send_position_axis_x(2, 140)
+        time.sleep(2)
+        by_cmd.send_angle_claw(90)
         pass
 
 # 通信抢修
 class up_tower():
-    def find():
+    def init(self):
+        logger.info("通信抢修初始化")
+    def find(self):
         # 目标检测通信塔
-        filter = label_filter(None)
-        ret1, list1 = filter.get(tlabel.YBALL)
+        ret1, list1 = filter.get(tlabel.TOWER)
         if ret1 > 0:
             return True
         else:
             return False
-    def exec():
-        pass
+    def exec(self):
+        logger.info("找到塔")
+        for _ in range(3):
+            by_cmd.send_speed_x(7)
+            by_cmd.send_speed_omega(0)
+            time.sleep(0.1)
+        
+
+        while True:
+            # logger.info("等待进入准确区域")
+            ret, error = filter.aim_near(tlabel.TOWER)
+            while not ret:
+                ret, error = filter.aim_near(tlabel.TOWER)
+            # logger.info(error)
+            if abs(error) < 5:
+                for _ in range(3):
+                    by_cmd.send_speed_x(0)
+                    time.sleep(0.2)
+                    by_cmd.send_speed_omega(0)
+                break 
+        ret, error = filter.aim_near(tlabel.TOWER)
+        while not ret:
+            ret, error = filter.aim_near(tlabel.TOWER)
+        time.sleep(1)
+        logger.error(error)
+        if abs(error) > 5:
+            logger.info("校准中")
+            if error > 0:
+                by_cmd.send_distance_x(-10, int(error*3))
+            else:
+                by_cmd.send_distance_x(10, int(-error*3))
+                logger.error(error)
+            time.sleep(1)
 
 # 高空排险
 class get_rball():
-    def find():
+    def init(self):
+        logger.info("高空排险初始化")
+        if (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+    def find(self):
         # 目标检测红球
-        filter = label_filter(None)
         ret1, list1 = filter.get(tlabel.RBALL)
         if ret1 > 0:
             return True
         else:
             return False
-    def exec():
+    def exec(self):
+        logger.info("找到红球")
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        time.sleep(1)
         pass
 
 # 派发物资
 class put_bball():
-    def find():
+    def init(self):
+        logger.info("派发物资初始化")
+        if (by_cmd.send_angle_camera(90) == -1):
+            by_cmd.send_angle_camera(90)
+    def find(self):
         # 目标检测通信塔
-        filter = label_filter(None)
         ret1, list1 = filter.get(tlabel.BASKET)
         if ret1 > 0:
             return True
         else:
             return False
-    def exec():
+    def exec(self):
+        logger.info("找到篮筐")
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        time.sleep(1)
+        
         pass
-
+direction = tlabel.RMARK
+direction_left = 0
+direction_right = 0
 # 物资盘点
 class put_hanoi1():
-    def find():
+    def init(self):
+        logger.info("物资盘点 1 初始化")
+        socket.send_string("2")
+        socket.recv()
+    def find(self):
+        global direction
+        global direction_left
+        global direction_right
         # 目标检测左右转向标识
-        filter = label_filter(None)
-        ret1, list1 = filter.get(tlabel.MARKL)
-        ret2, list2 = filter.get(tlabel.MARKR)
-
-        if (ret1 > 0) or (ret2 > 0):
+        # TODO 框的大小判断距离
+        ret1, list1 = filter.get(tlabel.RMARK)
+        ret2, list2 = filter.get(tlabel.LMARK)
+        if ret1:
+            logger.info("向右拐")
+            direction_right += 1
             return True
+        elif ret2:
+            logger.info("向左拐")
+            direction_left += 1
+            return True
+        return False
+    def exec(self):
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        time.sleep(0.2)
+        # if direction == tlabel.RMARK:
+        if direction_right > direction_left:
+            by_cmd.send_angle_omega(-20,500)
         else:
-            return False
-    def exec():
+            by_cmd.send_angle_omega(20,500)
+            time.sleep(0.2)
+            if (by_cmd.send_angle_camera(180) == -1):
+                by_cmd.send_angle_camera(180)
+        time.sleep(2)
+        socket.send_string("1")
+        socket.recv()
         pass
 
 class put_hanoi2():
-    def find():
+    def init(self):
+        logger.info("物资盘点 2 初始化")
+    def find(self):
         # 目标检测左右转向标识
-        filter = label_filter(None)
-        ret1, list1 = filter.get(tlabel.LPILLAR)
+        ret1, list1 = filter.get(tlabel.LPILLER)
 
         if ret1 > 0:
             return True
         else:
             return False
-    def exec():
+    def exec(self):
+        logger.info("找到最大块")
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        time.sleep(1)
         pass
 
 # 应急避险
 class move_area():
-    def find():
+    def init(self):
+        logger.info("应急避险初始化")
+        if (by_cmd.send_angle_camera(180) == -1):
+            by_cmd.send_angle_camera(180)
+    def find(self):
         # 目标检测标志牌
         # TODO 如何确保在都检测标志牌的情况下，和下一个任务进行区分
-        filter = label_filter(None)
         ret1, list1 = filter.get(tlabel.SIGN)
 
         if ret1 > 0:
             return True
         else:
             return False
-    def exec():
+    def exec(self):
+        logger.info("找到标示牌")
         pass
 
 # 扫黑除暴
 class kick_ass():
-    def find():
+    def init(self):
+        logger.info("扫黑除暴初始化")
+    def find(self):
         # 目标检测标志牌
         # TODO 如何确保在都检测标志牌的情况下，和上一个任务进行区分
-        filter = label_filter(None)
         ret1, list1 = filter.get(tlabel.SIGN)
 
         if ret1 > 0:
             return True
         else:
             return False
-    def exec():
+    def exec(self):
+        logger.info("找到标示牌")
         pass