feat: 增加部分任务

cfg_main.toml

@@ -5,22 +5,23 @@ logger_format = "{time} {level} {message}"
 [task]
 GetBlock_enable = false # 人员施救使能
 PutBlock_enable = false # 紧急转移使能
-GetBBall_enable = true # 整装上阵使能
-UpTower_enable = true  # 通信抢修使能
-GetRBall_enable = true # 高空排险使能
-PutBBall_enable = true # 派发物资使能
-PutHanoi_enable = true # 物资盘点使能
-MoveArea_enable = true # 应急避险使能
+GetBBall_enable = false # 整装上阵使能
+UpTower_enable = false  # 通信抢修使能
+GetRBall_enable = false # 高空排险使能
+PutBBall_enable = false # 派发物资使能
+PutHanoi_enable = false # 物资盘点使能
+MoveArea_enable = false # 应急避险使能
 KickAss_enable = true  # 扫黑除暴使能
 
 [find_counts]
-GetBlock_counts = 5 # 人员施救使能
-PutBlock_counts = 5 # 紧急转移使能
-GetBBall_counts = 5 # 整装上阵使能
-UpTower_counts = 5  # 通信抢修使能
-GetRBall_counts = 5 # 高空排险使能
-PutBBall_counts = 5 # 派发物资使能
-PutHanoi1_counts = 20 # 物资盘点使能
-PutHanoi2_counts = 5 # 物资盘点使能
-MoveArea_counts = 5 # 应急避险使能
-KickAss_counts = 5  # 扫黑除暴使能
+GetBlock_counts = 5 # 人员施救计数
+PutBlock_counts = 5 # 紧急转移计数
+GetBBall_counts = 5 # 整装上阵计数
+UpTower_counts = 5  # 通信抢修计数
+GetRBall_counts = 5 # 高空排险计数
+PutBBall_counts = 5 # 派发物资计数
+PutHanoi1_counts = 10 # 物资盘点计数
+PutHanoi2_counts = 4300 # 物资盘点计数
+MoveArea1_counts = 2 # 应急避险计数
+MoveArea2_counts = 1700 # 应急避险第二阶段计数
+KickAss_counts = 2  # 扫黑除暴计数

cfg_subtask.toml

@@ -13,7 +13,13 @@
 [put_hanoi1]
 
 [put_hanoi2]
+pos_gap = 160 # 标定值，两个放置位置的标定距离 
+pos_lp = 1    # 1\2\3 数字越小越靠近红色置物区
+pos_mp = 3
 
 [move_area]
 
 [kick_ass]
+pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
+pos_gap2 = 100 # person 之间的距离
+target_person = 3

main.py

@@ -6,6 +6,7 @@ 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)
 
@@ -16,17 +17,17 @@ cfg_main = toml.load('cfg_main.toml')
 logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
 
 # 向任务队列添加任务
-# TODO 任务关闭相关联
 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']['PutBlock_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']['PutBBall_enable'])) 
-task_queue.put(sb.task(sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], True))  # 无论是否进行任务，检测标识并转向都是必须进行的
+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_area, cfg_main['find_counts']['MoveArea_counts'], cfg_main['task']['MoveArea_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'])) 
 
 # 将任务队列传入调度模块中
@@ -43,21 +44,26 @@ worker.start()
 if (cmd_py_obj.send_angle_camera(180) == -1):
     cmd_py_obj.send_angle_camera(180)
 time.sleep(2)
-# cmd_py_obj.send_speed_x(5)
 
-# cmd_py_obj.send_position_axis_z(10, 100)
 # 创建主任务
 main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
 
 # 主线程仅在子线程搜索 (SEARCHING) 和 空闲 (IDLE) 状态下进行操作
 # while task_queuem_t.busy is True:
-while True:
-    if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
-        pass
-    else:
-        # 模拟执行回归任务
-        # logger.info("***** sim huigui task *****")
-        main_task_t.run()
-        pass
+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")

majtask.py

@@ -2,6 +2,7 @@ from simple_pid import PID
 import zmq
 import time
 from loguru import logger
+import utils
 
 class PidWrap:
     def __init__(self, kp, ki, kd, setpoint=0, output_limits=1):
@@ -42,17 +43,18 @@ class main_task():
             
         else:
             pass
+
+        
     def run(self):
-        # TODO 请求和解析回归值待完成
-        try:
-            # data = self.queue.get_nowait()
-            # self.parse_data(data)
-            pass
-        except:
-            pass
         # 运行巡线任务
         self.lane_task()
 
+    def lane_ntask(self):
+        # time.sleep(0.002)
+        # self.socket.send_string("")
+        # resp = self.socket.recv_pyobj()
+        # return resp.get('data')[0] - 160
+        pass
     def lane_task(self):
         # TODO 巡航参数从配置文件中读取
         time.sleep(0.002)
@@ -60,6 +62,7 @@ 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 # 赋全局变量
             self.error_counts = 0
             self.x = 0
             self.y = 0
@@ -79,9 +82,9 @@ class main_task():
             else:
                 self.pid1.set(0.8, 0, 0)
                 self.by_cmd.send_speed_x(11)
-            # TODO 待引入控制接口
             
-            pid_out = self.pid1.get(self.lane_error*0.65)
+            # pid_out = self.pid1.get(self.lane_error*0.65)
+            pid_out = self.pid1.get(self.lane_error*0.75)
             self.by_cmd.send_speed_omega(pid_out)
         self.socket.send_string("")
         resp = self.socket.recv_pyobj()

subtask.py

@@ -2,6 +2,7 @@ from enum import Enum
 from loguru import logger
 from utils import label_filter
 from utils import tlabel
+import utils
 import toml
 import zmq
 import time
@@ -9,18 +10,67 @@ import time
 context = zmq.Context()
 socket = context.socket(zmq.REQ)
 socket.connect("tcp://localhost:6667")
-logger.info("socket init")
+logger.info("subtask yolo client init")
 
+cfg = toml.load('cfg_subtask.toml') # 加载任务配置
+logger.info("load subtask config")
 
 by_cmd = None
 filter = None
+
 def import_obj(_by_cmd):
     global by_cmd
     global filter
     by_cmd = _by_cmd
     filter = label_filter(socket)
+def car_stop():
+    pass
+# run_on 是否继续向前行进 等待 error<5 后停止 如果不需要前进（在校准前车已经停下）
+# 则不需要等待 error < 5 可以直接 aim_near
 
+def calibrate(label, offset, run_on = True, cali_speed = 10):
+    logger.info("开始校准")
+    # go on
+    if run_on:
+        for _ in range(3):
+            by_cmd.send_speed_x(7)
+            by_cmd.send_speed_omega(0)
+            time.sleep(0.1)
+    while True:
+        ret, error = filter.aim_near(label)
+        while not ret:
+            ret, error = filter.aim_near(label)
+        error += offset
+        if run_on:
+            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
+        else:
+            break
         
+    ret, error = filter.aim_near(label)
+    while not ret:
+        ret, error = filter.aim_near(label)
+    error += offset
+    time.sleep(1)
+    logger.error(error)
+    if abs(error) > 5:
+        logger.info("校准中")
+        if error > 0:
+            by_cmd.send_distance_x(-cali_speed, int(error*4))
+        else:
+            by_cmd.send_distance_x(cali_speed, int(-error*4))
+            logger.error(error)
+        time.sleep(1)
+    # stop
+    for _ in range(3):
+        by_cmd.send_speed_x(0)
+        time.sleep(0.2)
+        by_cmd.send_speed_omega(0)
+    pass
 
 # 任务类
 class task:  
@@ -36,8 +86,11 @@ class task:
         # 检查该任执行标志
         while True:
             # if self.func_find():
-            if self.task_t.find():
-                self.counts += 1
+            # 特殊地方可以直接跳出 find
+            ret = self.task_t.find()
+            self.counts += ret
+            # if self.task_t.find():
+            #     self.counts += 1
             if self.counts >= self.find_counts:
                 break
         # while self.func_find() is False:
@@ -47,11 +100,11 @@ class task:
         # 根据标志位确定是否执行该任务
         if self.enable is True:
             logger.debug(f"[Task ]# Executing task")  
-            # self.func_exec()  
             self.task_t.exec()
             logger.debug(f"[Task ]# Task completed")  
         else:
             logger.warning(f"[Task ]# Skip task")
+            self.task_t.nexec()
 
 # 任务队列状态类
 class task_queuem_status(Enum):
@@ -115,41 +168,40 @@ class get_block():
         else:
             return False
     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)
+        calibrate(tlabel.RBLOCK,15)
+        # 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.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)
@@ -179,6 +231,7 @@ class get_block():
         time.sleep(3)
         by_cmd.send_position_axis_x(4, 0)
     def nexec(self):
+        # TODO 完成不执行任务的空动作
         pass
 
 
@@ -194,12 +247,38 @@ class put_block():
         else:
             return False
     def exec(self):
-        cfg = toml.load('cfg_subtask.toml') # 加载任务配置
+        # TODO 测试对准效果
         logger.info("找到医院")
         for _ in range(3):
             by_cmd.send_speed_x(0)
             time.sleep(0.2)
             by_cmd.send_speed_omega(0)
+            time.sleep(0.1)
+        while True:
+            # logger.info("等待进入准确区域")
+            ret, error = filter.aim_near(tlabel.HOSPITAL)
+            while not ret:
+                ret, error = filter.aim_near(tlabel.HOSPITAL)
+            # 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.HOSPITAL)
+        while not ret:
+            ret, error = filter.aim_near(tlabel.HOSPITAL)
+        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)
 
         by_cmd.send_position_axis_z(10, 150)
         time.sleep(3)
@@ -208,6 +287,8 @@ class put_block():
         time.sleep(2)
         by_cmd.send_position_axis_z(10, 170)
         pass
+    def nexec(self):
+        pass
 
 # 整装上阵
 class get_bball():
@@ -215,7 +296,7 @@ class get_bball():
         by_cmd.send_position_axis_x(2, 140)
         logger.info("整装上阵初始化")
         time.sleep(0.5)
-        if (by_cmd.send_angle_camera(90) == -1):
+        while (by_cmd.send_angle_camera(90) == -1):
             by_cmd.send_angle_camera(90)
     def find(self):
         # 目标检测黄球
@@ -255,7 +336,7 @@ class get_bball():
                 by_cmd.send_distance_x(10, int(-error*3))
                 logger.error(error)
             time.sleep(1)
-        if (by_cmd.send_angle_camera(0) == -1):
+        while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
         by_cmd.send_position_axis_z(20, 160)
         time.sleep(2)
@@ -277,6 +358,8 @@ class get_bball():
         time.sleep(2)
         by_cmd.send_angle_claw(90)
         pass
+    def nexec(self):
+        pass
 
 # 通信抢修
 class up_tower():
@@ -322,12 +405,15 @@ class up_tower():
                 by_cmd.send_distance_x(10, int(-error*3))
                 logger.error(error)
             time.sleep(1)
+    def nexec(self):
+        pass
+
 
 # 高空排险
 class get_rball():
     def init(self):
         logger.info("高空排险初始化")
-        if (by_cmd.send_angle_camera(0) == -1):
+        while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
     def find(self):
         # 目标检测红球
@@ -338,18 +424,17 @@ class get_rball():
             return False
     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)
+        # TODO 高空排险 offset 需要调整
+        calibrate(tlabel.RBALL,15, True, 6)
+        pass
+    def nexec(self):
         pass
 
 # 派发物资
 class put_bball():
     def init(self):
         logger.info("派发物资初始化")
-        if (by_cmd.send_angle_camera(90) == -1):
+        while (by_cmd.send_angle_camera(90) == -1):
             by_cmd.send_angle_camera(90)
     def find(self):
         # 目标检测通信塔
@@ -365,8 +450,10 @@ class put_bball():
             time.sleep(0.2)
             by_cmd.send_speed_omega(0)
         time.sleep(1)
-        
         pass
+    def nexec(self):
+        pass
+
 direction = tlabel.RMARK
 direction_left = 0
 direction_right = 0
@@ -386,14 +473,25 @@ class put_hanoi1():
         ret2, list2 = filter.get(tlabel.LMARK)
         if ret1:
             logger.info("向右拐")
-            direction_right += 1
-            return True
+            list1 = list1[0]
+            area = (list1[2] - list1[0]) * (list1[3] - list1[1])
+            logger.info(area)
+            if area > 2500:
+                direction_right += 1
+                return True
+            return False
         elif ret2:
             logger.info("向左拐")
-            direction_left += 1
-            return True
+            list2 = list2[0]
+            area = (list2[2] - list2[0]) * (list2[3] - list2[1])
+            logger.info(area)
+            if area > 2500:
+                direction_left += 1
+                return True
+            return False
         return False
     def exec(self):
+        global direction
         for _ in range(3):
             by_cmd.send_speed_x(0)
             time.sleep(0.2)
@@ -401,70 +499,204 @@ class put_hanoi1():
         time.sleep(0.2)
         # if direction == tlabel.RMARK:
         if direction_right > direction_left:
-            by_cmd.send_angle_omega(-20,500)
+            direction = tlabel.RMARK
+            # by_cmd.send_distance_x(6, 200)
+            by_cmd.send_angle_omega(-20,380)
+            time.sleep(2)
+            while (by_cmd.send_angle_camera(90) == -1):
+                by_cmd.send_angle_camera(90)
         else:
+            direction = tlabel.LMARK
+            # by_cmd.send_distance_x(6, 200)
             by_cmd.send_angle_omega(20,500)
-            time.sleep(0.2)
-            if (by_cmd.send_angle_camera(180) == -1):
+            time.sleep(2)
+            while (by_cmd.send_angle_camera(180) == -1):
                 by_cmd.send_angle_camera(180)
-        time.sleep(2)
+        time.sleep(0.5)
         socket.send_string("1")
         socket.recv()
         pass
+    def nexec(self):
+        pass
 
+
+def sub_put_hanoi2(label,distance_type,run_on,back_flag):
+    # logger.info("找到大红色柱体")
+    for _ in range(3):
+        by_cmd.send_speed_x(0)
+        time.sleep(0.2)
+        by_cmd.send_speed_omega(0)
+    time.sleep(2)
+
+    # 对准大红色柱体
+    calibrate(label,15, run_on, 6)
+    # 抓取大红色柱体
+    logger.info("抓取柱体")
+    # 根据 direction 确定移动方向
+    by_cmd.send_distance_x(-8, distance_type)
+    # 移动 self.distance_lp 距离
+    # 放置物块
+    logger.info("开始放置柱体")
+    time.sleep(1)
+    if back_flag:
+        pass
+        # 回移相同距离
+        # if direction == tlabel.RMARK:
+        #     by_cmd.send_distance_x(8, distance_type)
+        # else:
+        #     by_cmd.send_distance_x(-8, distance_type)
 class put_hanoi2():
+    def __init__(self):
+        self.pos_lp = cfg['put_hanoi2']['pos_lp']
+        self.pos_mp = cfg['put_hanoi2']['pos_mp']
+        self.pos_sp = 6 - self.pos_lp - self.pos_mp
+        self.distance_lp = self.pos_lp * cfg['put_hanoi2']['pos_gap']
+        self.distance_mp = self.pos_mp * cfg['put_hanoi2']['pos_gap']
+        self.distance_sp = self.pos_sp * cfg['put_hanoi2']['pos_gap']
+        logger.info(f"setting hanoi pos_lp[{self.pos_lp}] pos_mp[{self.pos_mp}] pos_sp[{self.pos_sp}]") 
     def init(self):
         logger.info("物资盘点 2 初始化")
     def find(self):
-        # 目标检测左右转向标识
-        ret1, list1 = filter.get(tlabel.LPILLER)
+        time.sleep(0.001)
+        return True
+        # # 目标检测左右转向标识
+        # ret1, list1 = filter.get(tlabel.LPILLER)
+        # ret, _  = filter.get_mult([tlabel.LPILLER])
+        # if ret:
+        #     logger.info("看到了三个 直接停车")
+        #     return 100
+        # if ret1 > 0:
+        #     list1 = list1[0]
+        #     area = (list1[2] - list1[0]) * (list1[3] - list1[1])
+        #     logger.info(area)
+        #     if area > 3300:
+        #         return True
+        #     return False
+        # else:
+        #     return False
+    def exec(self):
+        # TODO 延时需要根据移动的 distance 判断
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        # 往回走一段 然后向前行进校准
+        by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
+        time.sleep(1)
+        logger.info(f"方向{direction}")
         
-        if ret1 > 0:
+        if utils.lane_error > 0:
+            by_cmd.send_angle_omega(-20,abs(utils.lane_error))
+        else:
+            by_cmd.send_angle_omega(20,abs(utils.lane_error))
+        
+        time.sleep(1)    
+        sub_put_hanoi2(tlabel.LPILLER, self.distance_lp, True, True)
+        time.sleep(5)
+        # 对准中蓝色柱体
+        sub_put_hanoi2(tlabel.MPILLER, self.distance_mp, True, True)
+        time.sleep(5)
+        # 根据 direction 确定移动方向
+        # 移动 self.distance_mp 距离
+        # 放置物块
+        # 回移相同距离
+        sub_put_hanoi2(tlabel.SPILLER, self.distance_sp, True, True)
+        time.sleep(5)
+        # 对准小红色柱体
+        # 根据 direction 确定移动方向
+        # 移动 self.distance_sp 距离
+        # 放置物块
+        pass
+    def nexec(self):
+        pass
+
+# 应急避险 第一阶段 找目标牌
+class move_area1():
+    def init(self):
+        logger.info("应急避险第一阶段初始化")
+        while (by_cmd.send_angle_camera(180) == -1):
+            by_cmd.send_angle_camera(180)
+    def find(self):
+        # 目标检测标志牌
+        ret, error = filter.aim_near(tlabel.SIGN)
+        if ret > 0 and abs(error) < 8:
             return True
         else:
             return False
     def exec(self):
-        logger.info("找到最大块")
+        logger.info("找到标示牌")
+        # for _ in range(3):
+        #     by_cmd.send_speed_x(0)
+        #     time.sleep(0.2)
+        #     by_cmd.send_speed_omega(0)
+        
+        pass
+    def nexec(self):
+        pass
+# 应急避险 第二阶段 找停车区域
+class move_area2():
+    def init(self):
+        logger.info("应急避险第二阶段初始化")
+    def find(self):
+        time.sleep(0.001)
+        return True
+    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)
+        calibrate(tlabel.SHELTER, 15, False, 6)
+        time.sleep(1)
+        # 进入停车区域
+        by_cmd.send_distance_y(10, 450)
+        time.sleep(3)
+        # 离开停车区域
+        by_cmd.send_distance_y(-10, 450)
+        time.sleep(3)
+        while True:
+            pass
+        
+        pass
+    def nexec(self):
+        pass
+# 扫黑除暴
+class kick_ass():
+    def init(self):
+        logger.info("扫黑除暴初始化")
+        self.pos_gap1 = cfg['kick_ass']['pos_gap1']
+        self.pos_gap2 = cfg['kick_ass']['pos_gap2']
+        self.target_person = cfg['kick_ass']['target_person']
+    def find(self):
+        ret, error = filter.aim_near(tlabel.SIGN)
+        if ret > 0 and abs(error) < 8:
+            return True
+        else:
+            return False
+    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 init(self):
-        logger.info("应急避险初始化")
-        if (by_cmd.send_angle_camera(180) == -1):
-            by_cmd.send_angle_camera(180)
-    def find(self):
-        # 目标检测标志牌
-        # TODO 如何确保在都检测标志牌的情况下，和下一个任务进行区分
-        ret1, list1 = filter.get(tlabel.SIGN)
-
-        if ret1 > 0:
-            return True
+        if utils.lane_error > 0:
+            by_cmd.send_angle_omega(-20,abs(utils.lane_error)*1.5)
         else:
-            return False
-    def exec(self):
-        logger.info("找到标示牌")
-        pass
+            by_cmd.send_angle_omega(20,abs(utils.lane_error)*1.5)
+        time.sleep(1)
+        calibrate(tlabel.SIGN, 8, False, 6)
+        time.sleep(0.5)
         
-# 扫黑除暴
-class kick_ass():
-    def init(self):
-        logger.info("扫黑除暴初始化")
-    def find(self):
-        # 目标检测标志牌
-        # TODO 如何确保在都检测标志牌的情况下，和上一个任务进行区分
-        ret1, list1 = filter.get(tlabel.SIGN)
 
-        if ret1 > 0:
-            return True
+        if self.target_person == 1:
+            by_cmd.send_distance_x(10, self.pos_gap1)
         else:
-            return False
-    def exec(self):
-        logger.info("找到标示牌")
+            by_cmd.send_distance_x(10, self.pos_gap1 + (self.target_person - 1) * self.pos_gap2)
+
+        time.sleep(2)
+        time.sleep(5)
+        pass
+    def nexec(self):
         pass
 

utils.py

@@ -1,24 +1,9 @@
+
 from enum import Enum
 import numpy as np
-# 根据标签修改
-# class tlabel(Enum):
-#     BBLOCK   = 5    # 蓝色方块
-#     RBLOCK   = 2    # 红色方块
-#     HOSPITAL = 3    # 医院
-#     BBALL    = 4    # 蓝球
-#     YBALL    = 5    # 黄球
-#     TOWER    = 6    # 通信塔
-#     RBALL    = 7    # 红球
-#     BASKET   = 8    # 球筐
-#     MARKL    = 9    # 指向标
-#     MARKR    = 10   # 指向标
-#     SPILLAR  = 11   # 小柱体 (红色)
-#     MPILLAR  = 12   # 中柱体 (蓝色)
-#     LPILLAR  = 13   # 大柱体 (红色)
-#     SIGN     = 14   # 文字标牌
-#     TARGET   = 15   # 目标靶
-#     SHELTER  = 16   # 停车区
-#     BASE     = 17   # 基地
+
+lane_error = 0
+
 class tlabel(Enum):
     TOWER = 0
     SIGN = 1
@@ -36,6 +21,9 @@ class tlabel(Enum):
     LMARK = 13
     BBLOCK = 14
     BBALL = 15
+'''
+description: label_filter 的测试数据
+'''
 test_resp = {
     'code': 0,
     'data': np.array([
@@ -48,6 +36,9 @@ test1_resp = {
     'code': 0,
     'data': np.array([])
 }
+'''
+description: yolo 目标检测标签过滤器，需要传入连接到 yolo server 的 socket 对象
+'''
 class label_filter:
     def __init__(self, socket, threshold=0.6):
         self.num = 0
@@ -56,15 +47,32 @@ class label_filter:
         self.threshold = threshold
         
         self.img_size = (320, 240) 
+    '''
+    description: 向 yolo server 请求目标检测数据
+    param {*} self
+    return {*}
+    '''    
     def get_resp(self):
         self.socket.send_string('')
         response = self.socket.recv_pyobj()
         return response
+    '''
+    description: 切换 yolo server 视频源 在分叉路口时目标检测需要使用前摄
+    param {*} self
+    param {*} camera_id 1 或者 2 字符串
+    return {*}
+    '''    
     def switch_camera(self,camera_id):
         if camera_id == 1 or camera_id == 2:
             self.socket.send_string(f'{camera_id}')
             response = self.socket.recv_pyobj()
             return response
+    '''
+    description: 对模型推理推理结果使用 threshold 过滤 默认阈值为 0.5
+    param {*} self
+    param {*} data get_resp 返回的数据
+    return {bool,array}
+    '''    
     def filter_box(self,data):
         if len(data) > 0:
             expect_boxes = (data[:, 1] > self.threshold) & (data[:, 0] > -1)
@@ -83,10 +91,15 @@ class label_filter:
             if len(results) > 0:
                 return True, np.array(results)
         return False, None
+    '''
+    description: 根据传入的标签过滤，返回该标签的个数以及 box
+    param {*} self
+    param {*} tlabel
+    return {int, array}
+    '''    
     def get(self, tlabel):
         # 循环查找匹配的标签值
         # 返回对应标签的个数，以及坐标列表
-        # TODO  self.filter_box none judge
         response = self.get_resp()
         if response['code'] == 0:
             ret, results = self.filter_box(response['data'])
@@ -97,8 +110,38 @@ class label_filter:
                 self.pos = boxes[:, 2:] # [[x1 y1 x2 y2]]
                 return self.num, self.pos
         return 0, []
+    '''
+    description: 
+    param {*} self
+    param {*} tlabel_list
+    return {*}
+    '''    
+    def get_mult(self, tlabel_list):
+        response = self.get_resp()
+        if response['code'] == 0:
+            ret, results = self.filter_box(response['data'])
+            target_counts = len(tlabel_list)
+            counts = 0
+            if ret:
+                for tlabel in tlabel_list:
+                    expect_boxes = (results[:, 0] == tlabel.value)
+                    has_true = np.any(expect_boxes)
+                    if has_true:
+                        counts += 1
+                    else:
+                        return False, []
+                if counts == target_counts:
+                    return True, counts
+                return False, []
+            return False, []    
+        return False, []
+    '''
+    description: 判断传入的标签是否存在，存在返回 True
+    param {*} self
+    param {*} tlabel
+    return {bool}
+    '''    
     def find(self, tlabel):
-        # 遍历返回的列表，有对应标签则返回 True
         response = self.get_resp()
         if response['code'] == 0:
             ret, results = self.filter_box(response['data'])
@@ -108,6 +151,12 @@ class label_filter:
                 if len(boxes) != 0:
                     return True 
         return False
+    '''
+    description: 根据传入的标签，
+    param {*} self
+    param {*} tlabel
+    return {*}
+    '''    
     def aim_left(self, tlabel):
         # 如果标签存在，则返回列表中位置最靠左的目标框和中心的偏移值
         response = self.get_resp()
@@ -151,7 +200,7 @@ class label_filter:
                 center_x_values = np.abs(boxes[:, 2] + boxes[:, 4] - self.img_size[0])
                 center_x_index = np.argmin(center_x_values)
                 error = (boxes[center_x_index][4] + boxes[center_x_index][2] - self.img_size[0]) / 2
-                return (True, error+15)
+                return (True, error)
         return (False, 0)
 
 # class Calibrate: