feat: 增加 hanoi 跳过过滤条件（存在问题）

cfg_subtask.7131.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 0.9
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[get_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 1.3
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 0.6
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.3
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 1.3
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = false
+
+[kick_ass]
+pid_kp = 0.8
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

cfg_subtask.toml

@@ -1,5 +1,5 @@
 [get_block]
-pid_kp = 0.9
+pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
 first_block = "blue"
@@ -20,12 +20,12 @@ pid_ki = 0
 pid_kd = 0
 
 [get_rball]
-pid_kp = 0.6
+pid_kp = 0.8
 pid_ki = 0
 pid_kd = 0
 
 [put_bball]
-pid_kp = 1.3
+pid_kp = 1.5
 pid_ki = 0
 pid_kd = 0
 
@@ -35,7 +35,7 @@ pid_ki = 0
 pid_kd = 0
 
 [put_hanoi2]
-pid_kp = 1.0
+pid_kp = 2.5
 pid_ki = 0
 pid_kd = 0
 pos_gap = 160

cfg_subtask.toml.6_9.bak

@@ -35,7 +35,7 @@ pid_ki = 0
 pid_kd = 0
 
 [put_hanoi2]
-pid_kp = 1
+pid_kp = 1.0
 pid_ki = 0
 pid_kd = 0
 pos_gap = 160

subtask.py

@@ -242,7 +242,7 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
     return True
 def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5):
     stop_error = 0
-    error_record = CountRecord(3)
+    error_record = CountRecord(10)
     if run_direc == 1:
         by_cmd.send_speed_x(run_speed)
     else:
@@ -940,7 +940,7 @@ class put_hanoi1():
         else:
             var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
         pass
-        time.sleep(1.5)
+        # time.sleep(1.5)
 
 class put_hanoi2():
     def __init__(self):
@@ -958,13 +958,15 @@ class put_hanoi2():
             # 15
             self.offset = 14
             # self.platform_offset = -25
-            self.platform_offset = -19
+            # self.platform_offset = -19
+            self.platform_offset = -15
         else:
             self.offset = 14
-            #self.platform_offset = -30
+            # self.platform_offset = -30
-            self.platform_offset = -19
+            # self.platform_offset = -19
+            self.platform_offset = -15
         # 延时，防止过早看到 tplatform（虽然此现象相当少见且诡异）
-        time.sleep(0.5)
+        time.sleep(1.5)
 
     def find(self):
         # ret, box = filter.get(self.target_label)
@@ -999,7 +1001,7 @@ class put_hanoi2():
             time.sleep(2)
             by_cmd.send_angle_claw(40)
             time.sleep(0.5)
-            by_cmd.send_distance_axis_z(30, 20)
+            by_cmd.send_distance_axis_z(30, 30)
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
             time.sleep(1)
@@ -1011,7 +1013,7 @@ class put_hanoi2():
             time.sleep(2)
             by_cmd.send_angle_claw(40)
             time.sleep(0.5)
-            by_cmd.send_distance_axis_z(30, 20)
+            by_cmd.send_distance_axis_z(30, 30)
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
@@ -1029,9 +1031,9 @@ class put_hanoi2():
             time.sleep(1)
             by_cmd.send_angle_claw(81)
             time.sleep(0.5)
-            by_cmd.send_angle_claw(63)
-            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
             time.sleep(1)
             pass
         else:
@@ -1041,9 +1043,9 @@ class put_hanoi2():
             time.sleep(1)
             by_cmd.send_angle_claw(81)
             time.sleep(0.5)
-            by_cmd.send_angle_claw(63)
-            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
             time.sleep(1)
 
         ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed =  5)
@@ -1055,7 +1057,7 @@ class put_hanoi2():
         if utils.direction is tlabel.RMARK:
             by_cmd.send_position_axis_z(30, 10)
             by_cmd.send_position_axis_x(1, 150)
-            by_cmd.send_angle_claw(63)
+            by_cmd.send_angle_claw(60)
             time.sleep(2)
             by_cmd.send_angle_claw(35)
             time.sleep(0.5)
@@ -1067,7 +1069,7 @@ class put_hanoi2():
         else:
             by_cmd.send_position_axis_z(30, 10)
             by_cmd.send_position_axis_x(1, 40)
-            by_cmd.send_angle_claw(63)
+            by_cmd.send_angle_claw(60)
             time.sleep(2)
             by_cmd.send_angle_claw(35)
             time.sleep(0.5)
@@ -1079,7 +1081,7 @@ class put_hanoi2():
         # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
         ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
-            logger.error("跳过物资盘点 2 exec")
+            logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放中平台")
@@ -1122,8 +1124,8 @@ class put_hanoi2():
             time.sleep(1)
             by_cmd.send_distance_axis_z(30, 10)
             time.sleep(0.5)
-            by_cmd.send_position_axis_x(1, 10)
+            by_cmd.send_position_axis_x(1, 0)
-            time.sleep(1)
+            time.sleep(2)
             pass
         else:
             by_cmd.send_position_axis_z(30, 10)
@@ -1134,13 +1136,13 @@ class put_hanoi2():
             time.sleep(1)
             by_cmd.send_distance_axis_z(30, 10)
             time.sleep(0.5)
-            by_cmd.send_position_axis_x(1, 160)
+            by_cmd.send_position_axis_x(1, 170)
-            time.sleep(1)
+            time.sleep(2)
             pass
         # ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
-        ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5, )
         if not ret:
-            logger.error("跳过物资盘点 2 exec")
+            logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放小平台")

test/test_cali.py

@@ -0,0 +1,48 @@
+import os
+import sys
+parent_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+sys.path.append(parent_dir)
+from utils import label_filter
+from loguru import logger
+from utils import tlabel
+import zmq
+import time
+from by_cmd_py import by_cmd_py
+import time
+import signal
+
+context = zmq.Context()
+socket = context.socket(zmq.REQ)
+socket.connect("tcp://localhost:6667")
+logger.info("subtask yolo client init")
+
+filter = label_filter(socket)
+filter.switch_camera(2)
+
+
+
+cmd_py_obj = by_cmd_py()
+def car_stop():
+    for _ in range(3):
+        cmd_py_obj.send_speed_x(0)
+        time.sleep(0.2)
+        cmd_py_obj.send_speed_y(0)
+        time.sleep(0.2)
+        cmd_py_obj.send_speed_omega(0)
+def signal_handler(sig, frame):
+    car_stop()
+
+offset = 0
+signal.signal(signal.SIGTERM, signal_handler)
+while True:
+    time.sleep(0.2)
+    ret, box = filter.get(tlabel.LMARK)
+    if ret:
+        # 宽度大于 41 停车
+        print(f"width: {box[0][2] - box[0][0]} height: {box[0][3] - box[0][1]}")
+    # if ret:
+    #     error = (box[0][2] + box[0][0] - 320) / 2 + offset
+    #     print(error)
+        
+    #     cmd_py_obj.send_speed_omega(-error * 0.8)
+car_stop()

utils.py

@@ -106,6 +106,53 @@ class label_filter:
             if len(results) > 0:
                 return True, np.array(results)
         return False, None
+    # '''
+    # description: 对模型推理推理结果使用 threshold 和其他条件过滤 默认阈值为 0.5
+    # param {*} self
+    # param {*} data get_resp 返回的数据
+    # return {bool,array}
+    # '''    
+    # def filter_box_custom(self, data, ymax_range):  
+    #     if len(data) > 0:  
+    #         expect_boxes = (data[:, 1] > self.threshold) & (data[:, 0] > -1)  
+    #         np_boxes = data[expect_boxes, :]  
+    #         results = [  
+    #             [  
+    #                 item[0],  # 'label':  
+    #                 item[1],  # 'score':  
+    #                 item[2],  # 'xmin':  
+    #                 item[3],  # 'ymin':  
+    #                 item[4],  # 'xmax':  
+    #                 item[5],  # 'ymax':  
+    #                 not (ymax_range[0] < item[3] < ymax_range[1]), # 如果 ymin 处在范围内则返回 False，认为该目标不符合要求
+    #                 not (ymax_range[0] < item[5] < ymax_range[1]) # 如果 ymax 处在范围内则返回 False，认为该目标不符合要求
+    #             ]  
+    #             for item in np_boxes  
+    #         ]  
+    #         if len(results) > 0:  
+    #             return True, np.array(results)  
+    #     return False, None
+    
+    #原来的函数
+    def filter_box_custom(self,data):
+        if len(data) > 0:
+            expect_boxes = (data[:, 1] > self.threshold) & (data[:, 0] > -1)
+            np_boxes = data[expect_boxes, :]
+            results = [
+                [
+                    item[0], # 'label': 
+                    item[1], # 'score': 
+                    item[2], # 'xmin':
+                    item[3], # 'ymin':
+                    item[4], # 'xmax':
+                    item[5]  # 'ymax':
+                ]
+                for item in np_boxes
+                if item[5] < 180
+            ]
+            if len(results) > 0:
+                return True, np.array(results)
+        return False, None
     '''
     description: 根据传入的标签过滤，返回该标签的个数、box
     param {*} self
@@ -198,7 +245,7 @@ class label_filter:
     def get_two(self, target_label, label):
         response = self.get_resp()
         if response['code'] == 0:
-            ret, results = self.filter_box(response['data'])
+            ret, results = self.filter_box_custom(response['data'])
             if ret:
                 expect_boxes = (results[:, 0] == target_label.value)
                 boxes = results[expect_boxes, :]
@@ -216,6 +263,35 @@ class label_filter:
                     label_bool = False
                 return (target_bool, label_bool, target_box)
         return (False, False, None)
+    
+    # '''
+    # description: 查询两个目标 只有 target_label 返回 box
+    # param {*} self
+    # param {*} tlabel
+    # return {[bool]}
+    # ''' 
+    # def get_two_hanoi(self, target_label, label, ymax_range):
+    #     response = self.get_resp()
+    #     if response['code'] == 0:
+    #         ret, results = self.filter_box_custom(response['data'], ymax_range)
+    #         if ret:
+    #             expect_boxes = (results[:, 0] == target_label.value)
+    #             boxes = results[expect_boxes, :]
+    #             if len(boxes) != 0:
+    #                 target_bool = True
+    #                 target_box = boxes[:, 2:]
+    #             else:
+    #                 target_bool = False
+    #                 target_box = None
+    #             expect_boxes = (results[:, 0] == label.value)
+    #             boxes = results[expect_boxes, :]
+    #             if len(boxes) != 0:
+    #                 label_bool = True
+    #             else:
+    #                 label_bool = False
+    #             return (target_bool, label_bool, target_box)
+    #     return (False, False, None)
+
     '''
     description: 判断传入的多目标标签是否存在，存在返回 True
     param {*} self

variable.py

@@ -9,7 +9,7 @@ task_speed = 0
 pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0} # 1.2
 
 # 转向 pid 对象
-pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=50)
+pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=60) # FIXME 6_9 模型为 50
 
 llm_text = ''