havoc420ubuntu
cb002c5b9f
- Update image saving path in ImageSubscriber class - Remove unused imports and code related to task_4 and arrow detection - Add import for run_task_test - Comment out run_task_4 and uncomment run_task_test in main function
57 lines
1.8 KiB
Python
57 lines
1.8 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
|
||
from task_test.detect_arrow_direction import ArrowDetector
|
||
from base_move.turn_degree import turn_degree
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("任务5")
|
||
|
||
def run_task_5(ctrl, msg, image_processor=None):
|
||
"""
|
||
执行任务5：检测箭头方向并转向
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: 控制消息对象
|
||
image_processor: 可选的图像处理器实例
|
||
"""
|
||
section("任务5：箭头检测与转向", "启动")
|
||
|
||
# 初始化箭头检测器
|
||
info("初始化箭头检测器", "启动")
|
||
arrow_detector = ArrowDetector(image_processor)
|
||
|
||
# 获取箭头方向
|
||
info("检测箭头方向", "检测")
|
||
direction = arrow_detector.get_arrow_direction()
|
||
|
||
if direction == "unknown":
|
||
warning("无法确定箭头方向，默认旋转90度", "警告")
|
||
turn_angle = 90
|
||
else:
|
||
turn_angle = 90 if direction == "right" else -90
|
||
info(f"检测到{direction}箭头，将旋转{turn_angle}度", "旋转")
|
||
|
||
# 执行旋转
|
||
start_time = time.time()
|
||
turn_result = turn_degree(ctrl, msg, turn_angle)
|
||
elapsed = time.time() - start_time
|
||
|
||
if turn_result:
|
||
success(f"旋转{turn_angle}度成功，耗时{elapsed:.2f}秒", "完成")
|
||
else:
|
||
error(f"旋转{turn_angle}度失败", "失败")
|
||
|
||
# 如果创建了新的检测器，需要销毁它
|
||
if image_processor is None and hasattr(arrow_detector, 'destroy'):
|
||
arrow_detector.destroy()
|
||
|
||
return turn_result
|
||
|