btl143/project_main
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: 修復hanoi2右側圖像上下翻轉時過濾條件錯誤的情況

Browse Source 
pref: 6_9較好參數
pref: 關閉hanoi2循跡模型切換
This commit is contained in:
bmy
2024-07-15 16:55:03 +08:00
parent 1edd292ac6
commit 527fa4b04b
8 changed files with 83 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
app.py
View File

@@ -15,7 +15,7 @@ from main_upper import main_func
server_command = [ server_command = [
{"path": "/home/evan/Workplace/project_capture/build/", "script": "./capture"}, {"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_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"}, {"path": "/home/evan/Workplace/project_infer/yolo_server/", "script": "yolo_infer_server.py"},
] ]

2
cfg_main.toml
View File

@@ -13,7 +13,7 @@ GetRBall_counts = 10
PutBBall_counts = 15 PutBBall_counts = 15
PutHanoi1_counts = 7 PutHanoi1_counts = 7
PutHanoi2_counts = 2 PutHanoi2_counts = 2
PutHanoi3_counts = 2 PutHanoi3_counts = 1
MoveArea1_counts = 6 MoveArea1_counts = 6
MoveArea2_counts = 1700 MoveArea2_counts = 1700
KickAss_counts = 10 KickAss_counts = 10

6
cfg_subtask.toml
View File

@@ -15,7 +15,7 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
[up_tower] [up_tower]
pid_kp = 1.3 pid_kp = 1.1
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
@@ -25,7 +25,7 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
[put_bball] [put_bball]
pid_kp = 1.5 pid_kp = 1.6
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
@@ -35,7 +35,7 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
[put_hanoi2] [put_hanoi2]
pid_kp = 2.5 pid_kp = 1.0
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
pos_gap = 160 pos_gap = 160

8
cfg_subtask.toml.6_9.bak
View File

@@ -1,5 +1,5 @@
[get_block] [get_block]
pid_kp = 0.9 pid_kp = 1.2
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
first_block = "blue" first_block = "blue"
@@ -15,17 +15,17 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
[up_tower] [up_tower]
pid_kp = 1.3 pid_kp = 1.1
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
[get_rball] [get_rball]
pid_kp = 0.6 pid_kp = 0.8
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
[put_bball] [put_bball]
pid_kp = 1.3 pid_kp = 1.6
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0

8
cfg_subtask.7131.bak → cfg_subtask.toml.7131.bak
View File

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

16
majtask.py
View File

@@ -13,9 +13,9 @@ class main_task():
self.socket = self.context.socket(zmq.REQ) self.socket = self.context.socket(zmq.REQ)
self.socket.connect("tcp://localhost:6666") self.socket.connect("tcp://localhost:6666")
# lane infer server1 # lane infer server1
self.context1 = zmq.Context() # self.context1 = zmq.Context()
self.socket1 = self.context.socket(zmq.REQ) # self.socket1 = self.context.socket(zmq.REQ)
self.socket1.connect("tcp://localhost:6669") # self.socket1.connect("tcp://localhost:6669")
# 赛道回归相关 # 赛道回归相关
self.x = 0 self.x = 0
@@ -101,10 +101,12 @@ class main_task():
# pid_out = -pid_out # pid_out = -pid_out
# logger.debug(f"err={self.lane_error}, pwm out={pid_out}") # logger.debug(f"err={self.lane_error}, pwm out={pid_out}")
self.by_cmd.send_speed_omega(pid_out) self.by_cmd.send_speed_omega(pid_out)
if var.switch_lane_model: # if var.switch_lane_model:
self.socket1.send_string("") # self.socket1.send_string("")
resp = self.socket1.recv_pyobj() # resp = self.socket1.recv_pyobj()
else: # else:
# self.socket.send_string("")
# resp = self.socket.recv_pyobj()
self.socket.send_string("") self.socket.send_string("")
resp = self.socket.recv_pyobj() resp = self.socket.recv_pyobj()
self.parse_data(resp) self.parse_data(resp)

39
subtask.py
View File

@@ -240,6 +240,9 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
break break
return True return True
# 对准应知道是左还是右，右侧需在过滤器中进行翻转
# flipv 为垂直翻转标志，转右侧开启
def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5): def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5):
stop_error = 0 stop_error = 0
error_record = CountRecord(10) error_record = CountRecord(10)
@@ -253,16 +256,19 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
else: else:
stop_error = 15 stop_error = 15
while True: 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: while not ret1:
# 如果找不到目标且跳过任务队列非空 (即指令跳过)
if not global_skip_queue.empty(): if not global_skip_queue.empty():
_ = global_skip_queue.get() _ = global_skip_queue.get()
logger.error("跳过 hanoi_calibrate") logger.error("跳过 hanoi_calibrate")
return False return False
# 如果找不到目标且发现错误目标 (上次放置任务失败)
if ret2: if ret2:
# 如果连续计数超过阈值，则直接返回
if error_record(ret2): if error_record(ret2):
return False 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 error = (box[0][2] + box[0][0] - 320) / 2 + offset
if ret1: if ret1:
@@ -916,7 +922,8 @@ class put_hanoi1():
# by_cmd.send_angle_omega(-25,430) # by_cmd.send_angle_omega(-25,430)
# by_cmd.send_angle_omega(-55,194) # by_cmd.send_angle_omega(-55,194)
# by_cmd.send_angle_omega(-45,238) # 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) time.sleep(2)
while (by_cmd.send_angle_camera(90) == -1): while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90) 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(25,430)
# by_cmd.send_angle_omega(55,194) # by_cmd.send_angle_omega(55,194)
# by_cmd.send_angle_omega(45,238) # 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) time.sleep(2)
while (by_cmd.send_angle_camera(0) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0) by_cmd.send_angle_camera(0)
@@ -934,9 +942,9 @@ class put_hanoi1():
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):
var.switch_lane_model = True # var.switch_lane_model = True
if utils.direction == tlabel.RMARK: 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: else:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
pass pass
@@ -1057,7 +1065,7 @@ class put_hanoi2():
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(60) by_cmd.send_angle_claw(55)
time.sleep(2) time.sleep(2)
by_cmd.send_angle_claw(35) by_cmd.send_angle_claw(35)
time.sleep(0.5) time.sleep(0.5)
@@ -1069,7 +1077,7 @@ class put_hanoi2():
else: else:
by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(60) by_cmd.send_angle_claw(55)
time.sleep(2) time.sleep(2)
by_cmd.send_angle_claw(35) by_cmd.send_angle_claw(35)
time.sleep(0.5) time.sleep(0.5)
@@ -1092,7 +1100,7 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
by_cmd.send_distance_axis_z(30, -20) by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(65) by_cmd.send_angle_claw(55)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10) by_cmd.send_position_axis_x(1, 10)
time.sleep(1) time.sleep(1)
@@ -1104,7 +1112,7 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
by_cmd.send_distance_axis_z(30, -20) by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(65) by_cmd.send_angle_claw(55)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160) by_cmd.send_position_axis_x(1, 160)
time.sleep(1) time.sleep(1)
@@ -1140,7 +1148,7 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass pass
# ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) # 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: if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return return
@@ -1175,16 +1183,17 @@ class put_hanoi2():
time.sleep(1.5) time.sleep(1.5)
# while True: # while True:
# pass # pass
by_cmd.send_speed_x(12)
time.sleep(1.5)
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):
var.switch_lane_model = False # var.switch_lane_model = False
if utils.direction is tlabel.RMARK: 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: else:
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], 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"])
# time.sleep(2)
pass pass
class put_hanoi3(): class put_hanoi3():

60
utils.py
View File

@@ -264,33 +264,41 @@ class label_filter:
return (target_bool, label_bool, target_box) return (target_bool, label_bool, target_box)
return (False, False, None) return (False, False, None)
# ''' '''
# description: 查询两个目标 只有 target_label 返回 box description: 查询两个目标 只有 target_label 返回 box
# param {*} self param {*} self
# param {*} tlabel param {*} tlabel
# return {[bool]} return {[bool]}
# ''' '''
# def get_two_hanoi(self, target_label, label, ymax_range): def get_two_hanoi(self, target_label, label, flipv):
# response = self.get_resp() response = self.get_resp()
# if response['code'] == 0: if response['code'] == 0:
# ret, results = self.filter_box_custom(response['data'], ymax_range) # FIXME 直接在外部过滤，不在 fliter 内过滤
# if ret: ret, results = self.filter_box(response['data'])
# expect_boxes = (results[:, 0] == target_label.value) if ret:
# boxes = results[expect_boxes, :] expect_boxes = (results[:, 0] == target_label.value)
# if len(boxes) != 0: boxes = results[expect_boxes, :]
# target_bool = True if len(boxes) != 0:
# target_box = boxes[:, 2:] target_bool = True
# else: target_box = boxes[:, 2:]
# target_bool = False else:
# target_box = None target_bool = False
# expect_boxes = (results[:, 0] == label.value) target_box = None
# boxes = results[expect_boxes, :] expect_boxes = (results[:, 0] == label.value)
# if len(boxes) != 0: 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 # label_bool = True
# else: else:
# label_bool = False label_bool = False
# return (target_bool, label_bool, target_box) return (target_bool, label_bool, target_box)
# return (False, False, None) return (False, False, None)
''' '''
description: 判断传入的多目标标签是否存在，存在返回 True description: 判断传入的多目标标签是否存在，存在返回 True