From 8acf5fb31cb18f34c78de3a7bf83aefeace25286 Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Wed, 28 May 2025 23:11:38 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=97=A5=E5=BF=97=E6=96=87?= =?UTF-8?q?=E4=BB=B6=EF=BC=8C=E5=A2=9E=E5=8A=A0=E7=A6=BB=E7=BA=BF=E5=8F=8C?= =?UTF-8?q?=E8=BD=A8=E9=81=93=E7=BA=BF=E5=B1=85=E4=B8=AD=E6=B5=8B=E8=AF=95?= =?UTF-8?q?=E7=9A=84=E8=AF=A6=E7=BB=86=E4=BF=A1=E6=81=AF=E5=92=8C=E8=B0=83?= =?UTF-8?q?=E8=AF=95=E8=AE=B0=E5=BD=95=EF=BC=8C=E4=BF=AE=E5=A4=8D=E4=BA=86?= =?UTF-8?q?=E8=87=AA=E5=8A=A8=E6=A3=80=E6=B5=8B=E6=A8=A1=E5=BC=8F=E4=B8=8B?= =?UTF-8?q?=E7=9A=84=E5=BC=82=E5=B8=B8=EF=BC=8C=E6=8F=90=E5=8D=87=E4=BA=86?= =?UTF-8?q?=E6=A3=80=E6=B5=8B=E8=BF=87=E7=A8=8B=E7=9A=84=E7=A8=B3=E5=AE=9A?= =?UTF-8?q?=E6=80=A7=E5=92=8C=E5=87=86=E7=A1=AE=E6=80=A7=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- logs/robot_2025-05-28.log | 75 ++++++++++++ test_offline_centering.py | 245 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 320 insertions(+) create mode 100644 test_offline_centering.py diff --git a/logs/robot_2025-05-28.log b/logs/robot_2025-05-28.log index 70e42b6..13bed17 100644 --- a/logs/robot_2025-05-28.log +++ b/logs/robot_2025-05-28.log @@ -537,3 +537,78 @@ 2025-05-28 23:02:20 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_230220_810282.jpg 2025-05-28 23:02:20 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_230220_810282', 'center_point': (852, 1080), 'deviation': -2.2737367544323206e-13, 'left_track_mid_x': 397.0, 'right_track_mid_x': 1351.5, 'track_width': 954.5, 'center_slope': -0.16286380575687864, 'stone_path_mode': False} 2025-05-28 23:02:20 | INFO | utils.log_helper - ℹ️ 选择石板路模式结果 +2025-05-28 23:08:28 | INFO | utils.log_helper - ========== 🚀 离线双轨道线居中测试 🚀 ========== +2025-05-28 23:08:28 | INFO | utils.log_helper - ℹ️ 使用自动检测模式 +2025-05-28 23:08:28 | INFO | utils.log_helper - ============ 🚀 开始模拟双轨道居中 🚀 ============ +2025-05-28 23:08:28 | ERROR | utils.log_helper - 🚫 测试过程中发生异常: name 'auto_detect_dual_track_lines' is not defined +2025-05-28 23:08:28 | INFO | utils.log_helper - ========== 离线双轨道线居中测试 ========== +2025-05-28 23:09:42 | INFO | utils.log_helper - ========== 🚀 离线双轨道线居中测试 🚀 ========== +2025-05-28 23:09:42 | INFO | utils.log_helper - ℹ️ 使用自动检测模式 +2025-05-28 23:09:42 | INFO | utils.log_helper - ============ 🚀 开始模拟双轨道居中 🚀 ============ +2025-05-28 23:09:42 | DEBUG | utils.log_helper - 🐞 增强对比度和颜色检测 +2025-05-28 23:09:43 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:09:43 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:09:44 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:09:44 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 127 条直线 +2025-05-28 23:09:45 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 30 条垂直线 +2025-05-28 23:09:45 | DEBUG | utils.log_helper - 🐞 步骤4.5: 合并后找到 9 条垂直线 +2025-05-28 23:09:46 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.96 +2025-05-28 23:09:46 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_230946_842936.jpg +2025-05-28 23:09:46 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_230946_842936', 'center_point': (838, 1080), 'deviation': -2.2737367544323206e-13, 'left_track_mid_x': 409.3663817066892, 'right_track_mid_x': 1378.603238665647, 'track_width': 969.2368569589578, 'center_slope': -0.16918184085806812, 'stone_path_mode': True} +2025-05-28 23:09:46 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:09:47 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:09:47 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:09:48 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线 +2025-05-28 23:09:48 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 8 条垂直线 +2025-05-28 23:09:49 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.92 +2025-05-28 23:09:49 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_230949_965350.jpg +2025-05-28 23:09:49 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_230949_965350', 'center_point': (852, 1080), 'deviation': -2.2737367544323206e-13, 'left_track_mid_x': 397.0, 'right_track_mid_x': 1351.5, 'track_width': 954.5, 'center_slope': -0.16286380575687864, 'stone_path_mode': False} +2025-05-28 23:09:49 | INFO | utils.log_helper - ℹ️ 选择石板路模式结果 +2025-05-28 23:09:49 | DEBUG | utils.log_helper - 👁️ 迭代 1/50: 原始偏差: -0.0px, 滤波后: -0.0px +2025-05-28 23:09:49 | INFO | utils.log_helper - 📍 已接近中心，稳定计数: 1/3 +2025-05-28 23:09:50 | DEBUG | utils.log_helper - 🐞 增强对比度和颜色检测 +2025-05-28 23:09:50 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:09:51 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:09:51 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:09:52 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 127 条直线 +2025-05-28 23:09:52 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 30 条垂直线 +2025-05-28 23:09:53 | DEBUG | utils.log_helper - 🐞 步骤4.5: 合并后找到 9 条垂直线 +2025-05-28 23:09:53 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.96 +2025-05-28 23:09:54 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_230954_224929.jpg +2025-05-28 23:09:54 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_230954_224929', 'center_point': (838, 1080), 'deviation': -2.2737367544323206e-13, 'left_track_mid_x': 409.3663817066892, 'right_track_mid_x': 1378.603238665647, 'track_width': 969.2368569589578, 'center_slope': -0.16918184085806812, 'stone_path_mode': True} +2025-05-28 23:09:54 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:09:54 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:09:55 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:09:55 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线 +2025-05-28 23:09:56 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 8 条垂直线 +2025-05-28 23:09:56 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.92 +2025-05-28 23:09:57 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_230957_339325.jpg +2025-05-28 23:09:57 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_230957_339325', 'center_point': (852, 1080), 'deviation': -2.2737367544323206e-13, 'left_track_mid_x': 397.0, 'right_track_mid_x': 1351.5, 'track_width': 954.5, 'center_slope': -0.16286380575687864, 'stone_path_mode': False} +2025-05-28 23:09:57 | INFO | utils.log_helper - ℹ️ 选择石板路模式结果 +2025-05-28 23:09:57 | DEBUG | utils.log_helper - 👁️ 迭代 2/50: 原始偏差: -0.0px, 滤波后: -0.0px +2025-05-28 23:09:57 | INFO | utils.log_helper - 📍 已接近中心，稳定计数: 2/3 +2025-05-28 23:09:57 | DEBUG | utils.log_helper - 🐞 增强对比度和颜色检测 +2025-05-28 23:09:58 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:09:58 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:09:59 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:09:59 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 127 条直线 +2025-05-28 23:10:00 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 30 条垂直线 +2025-05-28 23:10:00 | DEBUG | utils.log_helper - 🐞 步骤4.5: 合并后找到 9 条垂直线 +2025-05-28 23:10:01 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.96 +2025-05-28 23:10:01 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_231001_584135.jpg +2025-05-28 23:10:01 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_231001_584135', 'center_point': (838, 1080), 'deviation': -2.2737367544323206e-13, 'left_track_mid_x': 409.3663817066892, 'right_track_mid_x': 1378.603238665647, 'track_width': 969.2368569589578, 'center_slope': -0.16918184085806812, 'stone_path_mode': True} +2025-05-28 23:10:01 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:10:02 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:10:02 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:10:03 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线 +2025-05-28 23:10:03 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 8 条垂直线 +2025-05-28 23:10:04 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.92 +2025-05-28 23:10:04 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_231004_698144.jpg +2025-05-28 23:10:04 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_231004_698144', 'center_point': (852, 1080), 'deviation': -2.2737367544323206e-13, 'left_track_mid_x': 397.0, 'right_track_mid_x': 1351.5, 'track_width': 954.5, 'center_slope': -0.16286380575687864, 'stone_path_mode': False} +2025-05-28 23:10:04 | INFO | utils.log_helper - ℹ️ 选择石板路模式结果 +2025-05-28 23:10:04 | DEBUG | utils.log_helper - 👁️ 迭代 3/50: 原始偏差: -0.0px, 滤波后: -0.0px +2025-05-28 23:10:04 | INFO | utils.log_helper - 📍 已接近中心，稳定计数: 3/3 +2025-05-28 23:10:04 | SUCCESS | utils.log_helper - 🏁 成功居中，最终偏差: -0.0px +2025-05-28 23:10:11 | INFO | utils.log_helper - ℹ️ 轨迹检测成功率: 100.0% (3/3) +2025-05-28 23:10:11 | SUCCESS | utils.log_helper - ✅ 测试成功完成 +2025-05-28 23:10:11 | INFO | utils.log_helper - ========== 离线双轨道线居中测试 ========== diff --git a/test_offline_centering.py b/test_offline_centering.py new file mode 100644 index 0000000..27feb20 --- /dev/null +++ b/test_offline_centering.py @@ -0,0 +1,245 @@ +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 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 + +# 全局日志记录器 +logger = get_logger("离线双轨道居中") + +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() + + # 直接使用全局日志记录器 + 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_flag = simulate_center_on_dual_tracks( + args.image, + max_iterations=args.iterations, + observe=True, + stone_path_mode=stone_path_mode + ) + + # 显示结果 + if success_flag: + success("测试成功完成", "结果") + else: + warning("测试未完全成功", "结果") + + except KeyboardInterrupt: + warning("用户中断测试", "中断") + except Exception as e: + error(f"测试过程中发生异常: {str(e)}", "异常") + + section("离线双轨道线居中测试", "结束") + +if __name__ == "__main__": + main() \ No newline at end of file