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 = [
{"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"},
]

2
cfg_main.toml
View File

@@ -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

6
cfg_subtask.toml
View File

@@ -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

8
cfg_subtask.toml.6_9.bak
View File

@@ -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

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

@@ -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

16
majtask.py
View File

@@ -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:
# 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)

39
subtask.py
View File

@@ -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():

60
utils.py
View File

@@ -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:
'''
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)
else:
label_bool = False
return (target_bool, label_bool, target_box)
return (False, False, None)
'''
description: 判断传入的多目标标签是否存在，存在返回 True