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

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

Browse Source
This commit is contained in:
bmy
2024-07-14 13:34:18 +08:00
parent b1300fc8f1
commit 1edd292ac6
7 changed files with 215 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
cfg_subtask.7131.bak Normal file
View File

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

8
cfg_subtask.toml
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.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

2
cfg_subtask.toml.6_9.bak
View File

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

44
subtask.py
View File

@@ -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("放小平台")

48
test/test_cali.py Normal file
View File

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

78
utils.py
View File

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

2
variable.py
View File

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