diff --git a/app.py b/app.py index 064db04..438216b 100644 --- a/app.py +++ b/app.py @@ -15,7 +15,7 @@ from main_upper import main_func server_command = [ {"path": "/home/evan/Workplace/project_capture/build/", "script": "./capture"}, {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server.py"}, - {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server1.py"}, + # {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server1.py"}, {"path": "/home/evan/Workplace/project_infer/yolo_server/", "script": "yolo_infer_server.py"}, ] diff --git a/cfg_main.toml b/cfg_main.toml index 452aa97..71e1194 100644 --- a/cfg_main.toml +++ b/cfg_main.toml @@ -13,7 +13,7 @@ GetRBall_counts = 10 PutBBall_counts = 15 PutHanoi1_counts = 7 PutHanoi2_counts = 2 -PutHanoi3_counts = 2 +PutHanoi3_counts = 1 MoveArea1_counts = 6 MoveArea2_counts = 1700 KickAss_counts = 10 diff --git a/cfg_subtask.toml b/cfg_subtask.toml index fe4529f..064f76c 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -15,7 +15,7 @@ pid_ki = 0 pid_kd = 0 [up_tower] -pid_kp = 1.3 +pid_kp = 1.1 pid_ki = 0 pid_kd = 0 @@ -25,7 +25,7 @@ pid_ki = 0 pid_kd = 0 [put_bball] -pid_kp = 1.5 +pid_kp = 1.6 pid_ki = 0 pid_kd = 0 @@ -35,7 +35,7 @@ pid_ki = 0 pid_kd = 0 [put_hanoi2] -pid_kp = 2.5 +pid_kp = 1.0 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 88671b0..064f76c 100644 --- a/cfg_subtask.toml.6_9.bak +++ b/cfg_subtask.toml.6_9.bak @@ -1,5 +1,5 @@ [get_block] -pid_kp = 0.9 +pid_kp = 1.2 pid_ki = 0 pid_kd = 0 first_block = "blue" @@ -15,17 +15,17 @@ pid_ki = 0 pid_kd = 0 [up_tower] -pid_kp = 1.3 +pid_kp = 1.1 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.6 pid_ki = 0 pid_kd = 0 diff --git a/cfg_subtask.7131.bak b/cfg_subtask.toml.7131.bak similarity index 91% rename from cfg_subtask.7131.bak rename to cfg_subtask.toml.7131.bak index 88671b0..8135560 100644 --- a/cfg_subtask.7131.bak +++ b/cfg_subtask.toml.7131.bak @@ -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.6 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/majtask.py b/majtask.py index 8479108..826b509 100644 --- a/majtask.py +++ b/majtask.py @@ -13,9 +13,9 @@ class main_task(): self.socket = self.context.socket(zmq.REQ) self.socket.connect("tcp://localhost:6666") # lane infer server1 - self.context1 = zmq.Context() - self.socket1 = self.context.socket(zmq.REQ) - self.socket1.connect("tcp://localhost:6669") + # self.context1 = zmq.Context() + # self.socket1 = self.context.socket(zmq.REQ) + # self.socket1.connect("tcp://localhost:6669") # 赛道回归相关 self.x = 0 @@ -101,10 +101,12 @@ class main_task(): # pid_out = -pid_out # logger.debug(f"err={self.lane_error}, pwm out={pid_out}") self.by_cmd.send_speed_omega(pid_out) - if var.switch_lane_model: - self.socket1.send_string("") - resp = self.socket1.recv_pyobj() - else: - self.socket.send_string("") - resp = self.socket.recv_pyobj() + # if var.switch_lane_model: + # self.socket1.send_string("") + # resp = self.socket1.recv_pyobj() + # else: + # self.socket.send_string("") + # resp = self.socket.recv_pyobj() + self.socket.send_string("") + resp = self.socket.recv_pyobj() self.parse_data(resp) diff --git a/subtask.py b/subtask.py index 13aa357..2586e3c 100644 --- a/subtask.py +++ b/subtask.py @@ -240,6 +240,9 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5): break return True + +# 对准应知道是左还是右，右侧需在过滤器中进行翻转 +# flipv 为垂直翻转标志，转右侧开启 def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5): stop_error = 0 error_record = CountRecord(10) @@ -253,16 +256,19 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3. else: stop_error = 15 while True: - ret1, ret2, box = filter.get_two(target_label, error_label) + ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK) while not ret1: + # 如果找不到目标且跳过任务队列非空 (即指令跳过) if not global_skip_queue.empty(): _ = global_skip_queue.get() logger.error("跳过 hanoi_calibrate") return False + # 如果找不到目标且发现错误目标 (上次放置任务失败) if ret2: + # 如果连续计数超过阈值，则直接返回 if error_record(ret2): return False - ret1, ret2, box = filter.get_two(target_label, error_label) + ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK) error = (box[0][2] + box[0][0] - 320) / 2 + offset if ret1: @@ -916,7 +922,8 @@ class put_hanoi1(): # by_cmd.send_angle_omega(-25,430) # by_cmd.send_angle_omega(-55,194) # by_cmd.send_angle_omega(-45,238) - by_cmd.send_angle_omega(-45,252) + # by_cmd.send_angle_omega(-45,252) + by_cmd.send_angle_omega(-45,260) time.sleep(2) while (by_cmd.send_angle_camera(90) == -1): by_cmd.send_angle_camera(90) @@ -925,7 +932,8 @@ class put_hanoi1(): # by_cmd.send_angle_omega(25,430) # by_cmd.send_angle_omega(55,194) # by_cmd.send_angle_omega(45,238) - by_cmd.send_angle_omega(45,252) + # by_cmd.send_angle_omega(45,252) + by_cmd.send_angle_omega(45,260) time.sleep(2) while (by_cmd.send_angle_camera(0) == -1): by_cmd.send_angle_camera(0) @@ -934,9 +942,9 @@ class put_hanoi1(): def nexec(self): pass def after(self): - var.switch_lane_model = True + # var.switch_lane_model = True if utils.direction == tlabel.RMARK: - var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.2, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) + var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) else: var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) pass @@ -1057,7 +1065,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(60) + by_cmd.send_angle_claw(55) time.sleep(2) by_cmd.send_angle_claw(35) time.sleep(0.5) @@ -1069,7 +1077,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(60) + by_cmd.send_angle_claw(55) time.sleep(2) by_cmd.send_angle_claw(35) time.sleep(0.5) @@ -1092,7 +1100,7 @@ class put_hanoi2(): time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) - by_cmd.send_angle_claw(65) + by_cmd.send_angle_claw(55) time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) time.sleep(1) @@ -1104,7 +1112,7 @@ class put_hanoi2(): time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) - by_cmd.send_angle_claw(65) + by_cmd.send_angle_claw(55) time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) time.sleep(1) @@ -1140,7 +1148,7 @@ class put_hanoi2(): 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") return @@ -1175,16 +1183,17 @@ class put_hanoi2(): time.sleep(1.5) # while True: # pass - + by_cmd.send_speed_x(12) + time.sleep(1.5) def nexec(self): pass def after(self): - var.switch_lane_model = False + # var.switch_lane_model = False if utils.direction is tlabel.RMARK: - var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"] - 0.2, cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) + var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) else: var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) - + # time.sleep(2) pass class put_hanoi3(): diff --git a/utils.py b/utils.py index b62e38f..5443875 100644 --- a/utils.py +++ b/utils.py @@ -264,33 +264,41 @@ class label_filter: 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: 查询两个目标 只有 target_label 返回 box + param {*} self + param {*} tlabel + return {[bool]} + ''' + def get_two_hanoi(self, target_label, label, flipv): + response = self.get_resp() + if response['code'] == 0: + # FIXME 直接在外部过滤，不在 fliter 内过滤 + ret, results = self.filter_box(response['data']) + 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: + # 如果垂直翻转 (走右侧) 且 *ymin* 小于 60（走右侧） + if flipv: + label_bool = all(box[3] > 60 for box in boxes) + # 如果不垂直翻转 (走左侧) 且 *ymax* 大于 180（走左侧） + else: + label_bool = all(box[5] < 180 for box in boxes) + # label_bool = True + else: + label_bool = False + return (target_bool, label_bool, target_box) + return (False, False, None) ''' description: 判断传入的多目标标签是否存在，存在返回 True