245 lines
9.3 KiB
Python
245 lines
9.3 KiB
Python
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() |