From 1edd292ac65e6ffa507695a80d93450f4070d3a0 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Sun, 14 Jul 2024 13:34:18 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=A2=9E=E5=8A=A0=20hanoi=20=E8=B7=B3?= =?UTF-8?q?=E8=BF=87=E8=BF=87=E6=BB=A4=E6=9D=A1=E4=BB=B6=EF=BC=88=E5=AD=98?= =?UTF-8?q?=E5=9C=A8=E9=97=AE=E9=A2=98=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cfg_subtask.7131.bak | 61 +++++++++++++++++++++++++++++++ cfg_subtask.toml | 8 ++--- cfg_subtask.toml.6_9.bak | 2 +- subtask.py | 44 ++++++++++++----------- test/test_cali.py | 48 +++++++++++++++++++++++++ utils.py | 78 +++++++++++++++++++++++++++++++++++++++- variable.py | 2 +- 7 files changed, 215 insertions(+), 28 deletions(-) create mode 100644 cfg_subtask.7131.bak create mode 100644 test/test_cali.py diff --git a/cfg_subtask.7131.bak b/cfg_subtask.7131.bak new file mode 100644 index 0000000..88671b0 --- /dev/null +++ b/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 diff --git a/cfg_subtask.toml b/cfg_subtask.toml index 88671b0..fe4529f 100644 --- a/cfg_subtask.toml +++ b/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 diff --git a/cfg_subtask.toml.6_9.bak b/cfg_subtask.toml.6_9.bak index 6ad28f2..88671b0 100644 --- a/cfg_subtask.toml.6_9.bak +++ b/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 diff --git a/subtask.py b/subtask.py index 73927e9..13aa357 100644 --- a/subtask.py +++ b/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 = -19 + # self.platform_offset = -30 + # 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) - time.sleep(1) + by_cmd.send_position_axis_x(1, 0) + 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) - time.sleep(1) + by_cmd.send_position_axis_x(1, 170) + 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("放小平台") diff --git a/test/test_cali.py b/test/test_cali.py new file mode 100644 index 0000000..cc2a702 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/utils.py b/utils.py index 9b7aa0f..b62e38f 100644 --- a/utils.py +++ b/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 diff --git a/variable.py b/variable.py index a79b4b8..29421cf 100644 --- a/variable.py +++ b/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 = ''