Enhance arrow detection in task 2 by adding left and right direction counters, updating detection logic, and refining output messages. Adjust movement parameters in task 2.5 for improved execution. Update task 3 to increase maximum iterations for better stability during operations.
This commit is contained in:
parent
beb0c1b9f1
commit
f088f626e1
2
main.py
2
main.py
@ -51,7 +51,7 @@ def main():
|
||||
Ctrl.base_msg.stop() # BUG 垃圾指令 for eat
|
||||
|
||||
# time.sleep(100) # TEST,
|
||||
run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)
|
||||
# run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)
|
||||
|
||||
arrow_direction = run_task_2(Ctrl, msg, xy_flag=False)
|
||||
# arrow_direction = 'right' # TEST
|
||||
|
||||
@ -46,6 +46,10 @@ class AsyncArrowDetector:
|
||||
self.save_dir = "logs/image"
|
||||
os.makedirs(self.save_dir, exist_ok=True)
|
||||
|
||||
# 添加左右方向计数器
|
||||
self.left_count = 0
|
||||
self.right_count = 0
|
||||
|
||||
info("异步箭头检测器已初始化", "初始化")
|
||||
|
||||
def start_detection(self, interval=0.5):
|
||||
@ -94,8 +98,14 @@ class AsyncArrowDetector:
|
||||
self.arrow_result = direction
|
||||
self.result_time = current_time
|
||||
|
||||
# 更新方向计数
|
||||
if direction == "left":
|
||||
self.left_count += 1
|
||||
elif direction == "right":
|
||||
self.right_count += 1
|
||||
|
||||
if direction != "unknown":
|
||||
info(f"异步检测到{direction}箭头", "箭头检测")
|
||||
info(f"异步检测到{direction}箭头 (left: {self.left_count}, right: {self.right_count})", "箭头检测")
|
||||
|
||||
# 保存检测结果的可视化图像
|
||||
timestamp = time.strftime("%Y%m%d_%H%M%S")
|
||||
@ -111,9 +121,21 @@ class AsyncArrowDetector:
|
||||
time.sleep(0.05)
|
||||
|
||||
def get_last_result(self):
|
||||
"""获取最后一次成功的箭头检测结果"""
|
||||
"""获取最后一次成功的箭头检测结果和计数情况"""
|
||||
with self.lock:
|
||||
return self.arrow_result, self.result_time, self.last_processed_image
|
||||
# 根据计数确定最终方向
|
||||
final_direction = self.arrow_result
|
||||
if self.left_count > self.right_count:
|
||||
final_direction = "left"
|
||||
elif self.right_count > self.left_count:
|
||||
final_direction = "right"
|
||||
|
||||
return final_direction, self.result_time, self.last_processed_image
|
||||
|
||||
def get_counts(self):
|
||||
"""获取当前的左右方向计数"""
|
||||
with self.lock:
|
||||
return self.left_count, self.right_count
|
||||
|
||||
def run_task_2(ctrl, msg, xy_flag=False):
|
||||
# 微调 xy 和角度
|
||||
@ -209,7 +231,8 @@ def run_task_2(ctrl, msg, xy_flag=False):
|
||||
|
||||
if result is not None and result != "unknown":
|
||||
arrow_direction = result
|
||||
info(f"成功检测到箭头方向: {arrow_direction}", "箭头检测")
|
||||
left_count, right_count = arrow_detector.get_counts()
|
||||
info(f"成功检测到箭头方向: {arrow_direction} (left: {left_count}, right: {right_count})", "箭头检测")
|
||||
break
|
||||
|
||||
time.sleep(0.1)
|
||||
@ -224,7 +247,21 @@ def run_task_2(ctrl, msg, xy_flag=False):
|
||||
if image is not None:
|
||||
# 直接在当前图像上检测
|
||||
arrow_direction = detect_arrow_direction(image, observe=False)
|
||||
info(f"直接检测到箭头方向: {arrow_direction}", "箭头检测")
|
||||
|
||||
# 更新计数
|
||||
if arrow_direction == "left":
|
||||
arrow_detector.left_count += 1
|
||||
elif arrow_direction == "right":
|
||||
arrow_detector.right_count += 1
|
||||
|
||||
# 根据总计数确定最终方向
|
||||
left_count, right_count = arrow_detector.get_counts()
|
||||
if left_count > right_count:
|
||||
arrow_direction = "left"
|
||||
elif right_count > left_count:
|
||||
arrow_direction = "right"
|
||||
|
||||
info(f"直接检测到箭头方向: {arrow_direction} (left: {left_count}, right: {right_count})", "箭头检测")
|
||||
|
||||
# 保存检测结果的可视化图像
|
||||
timestamp = time.strftime("%Y%m%d_%H%M%S")
|
||||
@ -245,6 +282,10 @@ def run_task_2(ctrl, msg, xy_flag=False):
|
||||
print('y为',ctrl.odo_msg.xyz[1])
|
||||
print('z为',ctrl.odo_msg.xyz[2])
|
||||
|
||||
# 输出最终计数结果
|
||||
left_count, right_count = arrow_detector.get_counts()
|
||||
info(f"箭头检测最终结果: left={left_count}, right={right_count}, 选择={arrow_direction}", "箭头检测结果")
|
||||
|
||||
# 返回检测到的箭头方向
|
||||
return arrow_direction
|
||||
|
||||
|
||||
@ -14,7 +14,7 @@ observe = True
|
||||
|
||||
def run_task_2_5(Ctrl, msg, direction='left'):
|
||||
section('任务2.5：预备进入任务3', "启动")
|
||||
go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe)
|
||||
go_straight(Ctrl, msg, distance=-0.2, speed=0.5, observe=observe)
|
||||
|
||||
# TEST
|
||||
turn_degree_v2(Ctrl, msg, 90, absolute=observe)
|
||||
@ -24,14 +24,16 @@ def run_task_2_5(Ctrl, msg, direction='left'):
|
||||
turn_success, res = arc_turn_around_hori_line(
|
||||
Ctrl,
|
||||
msg,
|
||||
angle_deg=90 if direction == 'left' else -90,
|
||||
angle_deg=85 if direction == 'left' else -85,
|
||||
target_distance=0.2,
|
||||
detect_func_version=3,
|
||||
pass_align=True,
|
||||
radius=0.55,
|
||||
observe=observe,
|
||||
no_end_reset=True,
|
||||
)
|
||||
|
||||
go_straight(Ctrl, msg, distance=0.2, speed=0.5, observe=observe)
|
||||
go_straight(Ctrl, msg, distance=0.4, speed=0.5, observe=observe)
|
||||
section('任务2.5-2：第二次旋转', "移动")
|
||||
|
||||
def run_task_2_5_back(Ctrl, msg, direction='left'):
|
||||
|
||||
@ -101,7 +101,7 @@ def pass_up_down(ctrl, msg):
|
||||
stable_threshold = 8 # 连续15次检测z轴不再增加则认为已经停止
|
||||
z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升
|
||||
climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值
|
||||
max_iterations = 180 # 最大循环次数，作为安全保障
|
||||
max_iterations = 200 # 最大循环次数，作为安全保障
|
||||
min_iterations = 170 # 最小循环次数，作为安全保障
|
||||
|
||||
# 姿态判断参数
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user