import sys import os import time import argparse import cv2 import numpy as np import math sys.path.append(os.path.dirname(os.path.abspath(__file__))) from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing from utils.detect_dual_track_lines import detect_dual_track_lines, auto_detect_dual_track_lines def simulate_center_on_dual_tracks(image_path, max_iterations=50, max_deviation=10.0, observe=True, stone_path_mode=None): """ 模拟机器狗仅使用Y轴移动调整到双轨道线的中间位置 参数: image_path: 图像路径 max_iterations: 最大迭代次数 max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中 observe: 是否输出中间状态信息和可视化结果 stone_path_mode: 是否使用石板路模式，None表示自动检测 返回: bool: 是否成功调整到中心位置 """ section("开始模拟双轨道居中", "离线模拟") # 确保图像存在 if not os.path.exists(image_path): error(f"图像不存在: {image_path}", "失败") return False # 加载原始图像 original_image = cv2.imread(image_path) if original_image is None: error(f"无法加载图像: {image_path}", "失败") return False # 获取图像尺寸 height, width = original_image.shape[:2] center_x = width // 2 # 创建图像平移函数 def shift_image(img, shift_x): """水平平移图像，模拟机器人横向移动""" M = np.float32([[1, 0, shift_x], [0, 1, 0]]) return cv2.warpAffine(img, M, (width, height)) # PID控制参数 - 仅使用比例控制以避免过冲 kp = 0.3 # 比例系数 # 帧间滤波参数 filter_size = 5 deviation_queue = [] # 统计变量 detection_success_count = 0 detection_total_count = 0 # 稳定计数器 stable_count = 0 required_stable_count = 3 # 当前横向位置偏移量（模拟机器人位置） current_offset = 0 # 创建可视化窗口 if observe: cv2.namedWindow("模拟居中", cv2.WINDOW_NORMAL) cv2.resizeWindow("模拟居中", 800, 600) # 开始模拟调整循环 for iteration in range(max_iterations): # 应用当前偏移创建模拟图像 shifted_image = shift_image(original_image, current_offset) # 检测双轨道线 detection_total_count += 1 if stone_path_mode is None: # 自动检测模式 center_info, left_info, right_info = auto_detect_dual_track_lines(shifted_image, observe=observe, delay=500 if observe else 1) else: # 指定模式 center_info, left_info, right_info = detect_dual_track_lines(shifted_image, observe=observe, delay=500 if observe else 1, stone_path_mode=stone_path_mode) if center_info is not None: detection_success_count += 1 # 获取当前偏差 current_deviation = center_info["deviation"] # 添加到队列 deviation_queue.append(current_deviation) if len(deviation_queue) > filter_size: deviation_queue.pop(0) # 计算滤波后的偏差值 if len(deviation_queue) >= 3: filtered_deviations = sorted(deviation_queue)[1:-1] if len(deviation_queue) > 2 else deviation_queue filtered_deviation = sum(filtered_deviations) / len(filtered_deviations) else: filtered_deviation = current_deviation if observe: debug(f"迭代 {iteration+1}/{max_iterations}: 原始偏差: {current_deviation:.1f}px, 滤波后: {filtered_deviation:.1f}px", "偏差") # 判断是否已经居中 if abs(filtered_deviation) <= max_deviation: stable_count += 1 if observe: info(f"已接近中心，稳定计数: {stable_count}/{required_stable_count}", "居中") if stable_count >= required_stable_count: # 已经稳定在中心位置 if observe: success(f"成功居中，最终偏差: {filtered_deviation:.1f}px", "完成") # 在结果图像上显示成功信息 result_image = shifted_image.copy() cv2.putText(result_image, f"成功居中! 偏差: {filtered_deviation:.1f}px", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) cv2.imshow("模拟居中", result_image) cv2.waitKey(0) break else: # 不在中心，重置稳定计数 stable_count = 0 # 计算横向移动量 lateral_move = kp * filtered_deviation # 限制单次移动量 max_move = 50 # 最大单次移动像素数 lateral_move = max(-max_move, min(max_move, lateral_move)) # 更新当前偏移 current_offset += lateral_move if observe: debug(f"横向移动: {lateral_move:.1f}px, 当前总偏移: {current_offset:.1f}px", "控制") else: warning(f"迭代 {iteration+1}/{max_iterations}: 未检测到双轨道线", "警告") # 显示当前模拟状态 if observe: # 在模拟图像上显示当前信息 info_image = shifted_image.copy() cv2.putText(info_image, f"迭代: {iteration+1}/{max_iterations}", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2) if center_info: # 如果成功检测，显示偏差信息 cv2.putText(info_image, f"偏差: {filtered_deviation:.1f}px", (50, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2) # 绘制居中目标 cv2.line(info_image, (center_x, 0), (center_x, height), (0, 0, 255), 1) else: # 如果检测失败，显示警告 cv2.putText(info_image, "未检测到轨道线", (50, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2) cv2.imshow("模拟居中", info_image) key = cv2.waitKey(100) if key == 27: # ESC键退出 break # 清理资源 if observe: cv2.destroyAllWindows() # 显示检测成功率 if detection_total_count > 0: detection_rate = (detection_success_count / detection_total_count) * 100 info(f"轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计") # 判断是否成功 success_flag = False if iteration >= max_iterations - 1: if observe: warning("超过最大迭代次数", "超时") else: # 如果因为已稳定在中心而退出循环，则认为成功 if stable_count >= required_stable_count: success_flag = True return success_flag def main(): """ 测试离线双轨道线居中功能 """ # 解析命令行参数 parser = argparse.ArgumentParser(description='测试离线双轨道线居中功能') parser.add_argument('--image', type=str, default="res/path/image_20250514_024347.png", help='测试图像路径') parser.add_argument('--iterations', type=int, default=50, help='最大迭代次数，默认为50') parser.add_argument('--stone_path', action='store_true', help='强制使用石板路模式') parser.add_argument('--normal_path', action='store_true', help='强制使用普通路径模式') args = parser.parse_args() # 初始化日志 LogHelper.init('offline_dual_track_centering') section("离线双轨道线居中测试", "开始") try: # 确定使用哪种模式 stone_path_mode = None # 默认自动检测 if args.stone_path: stone_path_mode = True info("强制使用石板路模式", "模式") elif args.normal_path: stone_path_mode = False info("强制使用普通路径模式", "模式") else: info("使用自动检测模式", "模式") # 执行离线模拟 success = simulate_center_on_dual_tracks( args.image, max_iterations=args.iterations, observe=True, stone_path_mode=stone_path_mode ) # 显示结果 if success: success("测试成功完成", "结果") else: warning("测试未完全成功", "结果") except KeyboardInterrupt: warning("用户中断测试", "中断") except Exception as e: error(f"测试过程中发生异常: {str(e)}", "异常") section("离线双轨道线居中测试", "结束") if __name__ == "__main__": main()