diff --git a/base_move/center_on_dual_tracks.py b/base_move/center_on_dual_tracks.py new file mode 100644 index 0000000..629c670 --- /dev/null +++ b/base_move/center_on_dual_tracks.py @@ -0,0 +1,268 @@ +import math +import time +import sys +import os + +sys.path.append(os.path.dirname(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 center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, + mode=11, gait_id=26, step_height=[0.06, 0.06], + stone_path_mode=None): + """ + 控制机器狗仅使用Y轴移动调整到双轨道线的中间位置 + + 参数: + ctrl: Robot_Ctrl 对象，包含里程计信息 + msg: robot_control_cmd_lcmt 对象，用于发送命令 + max_time: 最大调整时间(秒)，默认为15秒 + max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中，默认为10像素 + observe: 是否输出中间状态信息和可视化结果，默认为False + mode: 控制模式，默认为11 + gait_id: 步态ID，默认为26 + step_height: 抬腿高度，默认为[0.06, 0.06] + stone_path_mode: 是否使用石板路模式，None表示自动检测，True或False表示强制使用或不使用 + + 返回: + bool: 是否成功调整到中心位置 + """ + section("开始双轨道居中", "轨道居中") + + # 设置移动命令基本参数 + msg.mode = mode + msg.gait_id = gait_id + msg.duration = 0 # wait next cmd + msg.step_height = step_height + + # 记录起始时间 + start_time = time.time() + + # 记录起始位置 + start_position = list(ctrl.odo_msg.xyz) + if observe: + debug(f"起始位置: {start_position}", "位置") + # 在起点放置绿色标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True) + + # PID控制参数 - 仅使用比例控制以避免过冲 + kp = 0.003 # 比例系数 - 较小的值以获得平滑的移动 + + # 帧间滤波参数 + filter_size = 5 # 滤波队列大小 + deviation_queue = [] # 偏差值队列 + + # 统计变量 + detection_success_count = 0 + detection_total_count = 0 + + # 稳定计数器 - 连续几次在中心位置才认为稳定 + stable_count = 0 + required_stable_count = 3 + + # 开始调整循环 + while time.time() - start_time < max_time: + # 获取当前图像 + image = ctrl.image_processor.get_current_image() + + # 检测双轨道线 + detection_total_count += 1 + + if stone_path_mode is None: + # 自动检测模式 + center_info, left_info, right_info = auto_detect_dual_track_lines(image, observe=observe, save_log=True) + else: + # 指定模式 + center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True, 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"原始偏差: {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", "完成") + break + else: + # 不在中心，重置稳定计数 + stable_count = 0 + + # 计算横向移动速度 (只使用y轴移动) + # 注意：偏差为正表示需要向左移动（y轴正方向），偏差为负表示需要向右移动（y轴负方向） + lateral_velocity = kp * filtered_deviation + + # 限制横向移动速度 + max_lateral_velocity = 0.3 # 最大横向速度 (米/秒) + lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity)) + + if observe: + debug(f"横向移动速度: {lateral_velocity:.3f}m/s", "控制") + + # 设置速度命令 - 只使用y轴移动，不前进和转向 + msg.vel_des = [0, lateral_velocity, 0] # [前进速度, 侧向速度, 角速度] + + # 发送命令 + msg.life_count += 1 + ctrl.Send_cmd(msg) + else: + warning("未检测到双轨道线", "警告") + + # 如果已经有了一些有效的检测，暂时停止移动 + if len(deviation_queue) > 0: + msg.vel_des = [0, 0, 0] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe: + warning("暂停移动，等待有效检测", "暂停") + else: + # 如果一开始就没有检测到，可以尝试小范围搜索 + if detection_total_count < 10: + if detection_total_count % 2 == 0: + # 向右搜索 + msg.vel_des = [0, -0.1, 0] + else: + # 向左搜索 + msg.vel_des = [0, 0.1, 0] + + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe: + debug("搜索轨道线...", "搜索") + else: + # 超过一定次数仍未检测到，停止搜索 + msg.vel_des = [0, 0, 0] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe: + error("无法检测到轨道线，放弃调整", "失败") + break + + # 短暂延时 + time.sleep(0.05) + + # 停止移动 + ctrl.base_msg.stop() + + # 计算最终位置与起始位置的变化 + final_position = ctrl.odo_msg.xyz + dx = final_position[0] - start_position[0] + dy = final_position[1] - start_position[1] + + if observe: + # 在终点放置红色标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(final_position[0], final_position[1], final_position[2] if len(final_position) > 2 else 0.0, 'red', observe=True) + + info(f"横向移动距离: {abs(dy):.3f}米", "统计") + + # 显示检测成功率 + 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 = False + if time.time() - start_time >= max_time: + if observe: + warning("超过最大调整时间", "超时") + else: + # 如果因为已稳定在中心而退出循环，则认为成功 + if stable_count >= required_stable_count: + success = True + + return success + +def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_time=15, observe=False, + mode=11, gait_id=26, step_height=[0.06, 0.06], + stone_path_mode=None): + """ + 先居中到双轨道线中间，然后沿轨道线行走指定距离 + + 参数: + ctrl: Robot_Ctrl 对象，包含里程计信息 + msg: robot_control_cmd_lcmt 对象，用于发送命令 + distance: 目标前进距离(米) + speed: 前进速度(米/秒)，默认为0.5米/秒 + max_centering_time: 最大居中调整时间(秒)，默认为15秒 + observe: 是否输出中间状态信息和可视化结果，默认为False + mode: 控制模式，默认为11 + gait_id: 步态ID，默认为26 + step_height: 抬腿高度，默认为[0.06, 0.06] + stone_path_mode: 是否使用石板路模式，None表示自动检测 + + 返回: + bool: 是否成功完成居中和轨道跟随 + """ + section("开始双轨道居中和跟随", "轨道任务") + + # 第一步：居中到轨道中间 + if observe: + info("步骤1: 调整到轨道中间", "居中") + + centering_success = center_on_dual_tracks( + ctrl, msg, + max_time=max_centering_time, + observe=observe, + mode=mode, + gait_id=gait_id, + step_height=step_height, + stone_path_mode=stone_path_mode + ) + + if not centering_success: + if observe: + error("居中调整失败，无法继续跟随轨道", "失败") + return False + + # 第二步：沿轨道跟随指定距离 + if observe: + info(f"步骤2: 沿轨道前进 {distance:.2f}米", "跟随") + + # 导入轨道跟随函数 + from base_move.follow_dual_tracks import follow_dual_tracks + + following_success = follow_dual_tracks( + ctrl, msg, + speed=speed, + target_distance=distance, + observe=observe, + mode=mode, + gait_id=gait_id, + step_height=step_height + ) + + if observe: + if following_success: + success("成功完成轨道居中和跟随任务", "完成") + else: + warning("轨道跟随未完全成功", "警告") + + return following_success \ No newline at end of file diff --git a/logs/robot_2025-05-28.log b/logs/robot_2025-05-28.log index ed18f9d..70e42b6 100644 --- a/logs/robot_2025-05-28.log +++ b/logs/robot_2025-05-28.log @@ -500,3 +500,40 @@ 2025-05-28 22:51:32 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.89 2025-05-28 22:51:33 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_225133_573204.jpg 2025-05-28 22:51:33 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_225133_573204', 'center_point': (872, 1080), 'deviation': -1.1368683772161603e-13, 'left_track_mid_x': 539.8450926202986, 'right_track_mid_x': 1405.6136261449096, 'track_width': 865.768533524611, 'center_slope': -0.13454341879828116, 'stone_path_mode': True} +2025-05-28 23:01:24 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:01:26 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:01:28 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:01:30 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线 +2025-05-28 23:01:32 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 8 条垂直线 +2025-05-28 23:01:34 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.92 +2025-05-28 23:01:36 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_230136_236353.jpg +2025-05-28 23:01:36 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_230136_236353', '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:01:36 | DEBUG | utils.log_helper - 🐞 增强对比度和颜色检测 +2025-05-28 23:01:38 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:01:40 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:01:42 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:01:44 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 127 条直线 +2025-05-28 23:01:46 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 30 条垂直线 +2025-05-28 23:01:48 | DEBUG | utils.log_helper - 🐞 步骤4.5: 合并后找到 9 条垂直线 +2025-05-28 23:01:50 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.96 +2025-05-28 23:01:52 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_230152_516789.jpg +2025-05-28 23:01:52 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_230152_516789', '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:01:52 | DEBUG | utils.log_helper - 🐞 增强对比度和颜色检测 +2025-05-28 23:01:54 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:01:56 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:01:58 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:02:00 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 127 条直线 +2025-05-28 23:02:02 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 30 条垂直线 +2025-05-28 23:02:04 | DEBUG | utils.log_helper - 🐞 步骤4.5: 合并后找到 9 条垂直线 +2025-05-28 23:02:06 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.96 +2025-05-28 23:02:08 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250528_230208_672297.jpg +2025-05-28 23:02:08 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250528_230208_672297', '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:02:08 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-28 23:02:10 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-28 23:02:12 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-28 23:02:14 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线 +2025-05-28 23:02:16 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 8 条垂直线 +2025-05-28 23:02:18 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: 0.92 +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 - ℹ️ 选择石板路模式结果 diff --git a/res/path/test/result_image_20250513_162556.png/result_image_20250513_162556.png b/res/path/test/result_image_20250513_162556.png/result_image_20250513_162556.png deleted file mode 100644 index 9394635..0000000 Binary files a/res/path/test/result_image_20250513_162556.png/result_image_20250513_162556.png and /dev/null differ diff --git a/res/path/test/result_image_20250514_024347.png b/res/path/test/result_image_20250514_024347.png deleted file mode 100644 index 04fd57f..0000000 Binary files a/res/path/test/result_image_20250514_024347.png and /dev/null differ diff --git a/test/test_image.py b/test/test_image.py new file mode 100644 index 0000000..54aeba5 --- /dev/null +++ b/test/test_image.py @@ -0,0 +1,73 @@ +import cv2 +import os +from utils.detect_dual_track_lines import detect_dual_track_lines, auto_detect_dual_track_lines + +# 图片路径 +image_path = "res/path/image_20250514_024347.png" + +# 确保图片存在 +if not os.path.exists(image_path): + print(f"图片 {image_path} 不存在!") + exit(1) + +# 先尝试普通模式 +print("正在使用普通模式检测...") +center_info, left_track_info, right_track_info = detect_dual_track_lines( + image_path, + observe=True, # 设置为True以查看处理过程和结果 + delay=2000, # 增加显示时间以便观察 + save_log=True, + stone_path_mode=False +) + +if center_info: + print("\n普通模式检测结果:") + print(f"中心点: {center_info['point']}") + print(f"偏差: {center_info['deviation']:.2f}") + print(f"斜率: {center_info['slope']:.2f}") + print(f"轨道宽度: {center_info['track_width']:.2f}") +else: + print("普通模式检测失败") + +# 再尝试石板路模式 +print("\n正在使用石板路模式检测...") +center_info, left_track_info, right_track_info = detect_dual_track_lines( + image_path, + observe=True, + delay=2000, + save_log=True, + stone_path_mode=True +) + +if center_info: + print("\n石板路模式检测结果:") + print(f"中心点: {center_info['point']}") + print(f"偏差: {center_info['deviation']:.2f}") + print(f"斜率: {center_info['slope']:.2f}") + print(f"轨道宽度: {center_info['track_width']:.2f}") +else: + print("石板路模式检测失败") + +# 最后尝试自动检测模式 +print("\n正在使用自动检测模式...") +center_info, left_track_info, right_track_info = auto_detect_dual_track_lines( + image_path, + observe=True, + delay=2000, + save_log=True +) + +if center_info: + print("\n自动检测模式结果:") + print(f"中心点: {center_info['point']}") + print(f"偏差: {center_info['deviation']:.2f}") + print(f"斜率: {center_info['slope']:.2f}") + print(f"轨道宽度: {center_info['track_width']:.2f}") + print(f"使用石板路模式: {center_info['stone_path_mode']}") +else: + print("自动检测模式失败") + +# 等待用户按键关闭窗口 +print("\n按任意键退出...") +cv2.waitKey(0) +cv2.destroyAllWindows() \ No newline at end of file diff --git a/test/test_offline_centering.py b/test/test_offline_centering.py new file mode 100644 index 0000000..9a4ee48 --- /dev/null +++ b/test/test_offline_centering.py @@ -0,0 +1,243 @@ +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() \ No newline at end of file diff --git a/test_center_on_tracks.py b/test_center_on_tracks.py new file mode 100644 index 0000000..2f81ec7 --- /dev/null +++ b/test_center_on_tracks.py @@ -0,0 +1,101 @@ +import sys +import os +import time +import argparse + +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 + +def main(): + """ + 测试双轨道线居中功能 + """ + # 解析命令行参数 + parser = argparse.ArgumentParser(description='测试双轨道线居中功能') + parser.add_argument('--distance', type=float, default=0.0, + help='居中后要前进的距离(米)，默认为0，即仅执行居中操作') + parser.add_argument('--speed', type=float, default=0.5, + help='前进速度(米/秒)，默认为0.5') + parser.add_argument('--max_time', type=float, default=15.0, + help='最大居中时间(秒)，默认为15秒') + parser.add_argument('--stone_path', action='store_true', + help='强制使用石板路模式') + parser.add_argument('--normal_path', action='store_true', + help='强制使用普通路径模式') + args = parser.parse_args() + + # 初始化日志 + LogHelper.init('dual_track_centering') + section("双轨道线居中测试", "开始") + + try: + # 导入机器人控制模块 + from robot_controller import RobotCtrl + + # 初始化机器人控制器 + ctrl = RobotCtrl(observe=True) + msg = ctrl.initialize() + + if msg is None: + error("初始化机器人控制器失败", "失败") + return + + info("机器人控制器初始化成功", "初始化") + + # 等待机器人准备就绪 + time.sleep(1) + + # 确定使用哪种模式 + stone_path_mode = None # 默认自动检测 + if args.stone_path: + stone_path_mode = True + info("强制使用石板路模式", "模式") + elif args.normal_path: + stone_path_mode = False + info("强制使用普通路径模式", "模式") + else: + info("使用自动检测模式", "模式") + + # 导入双轨道线居中模块 + from base_move.center_on_dual_tracks import center_on_dual_tracks, center_and_follow_dual_tracks + + # 执行居中操作 + if args.distance > 0: + # 居中后沿轨道前进 + info(f"执行居中并前进 {args.distance} 米，速度 {args.speed} m/s", "任务") + success = center_and_follow_dual_tracks( + ctrl, msg, + distance=args.distance, + speed=args.speed, + max_centering_time=args.max_time, + observe=True, + stone_path_mode=stone_path_mode + ) + else: + # 仅执行居中 + info("仅执行居中操作", "任务") + success = center_on_dual_tracks( + ctrl, msg, + max_time=args.max_time, + observe=True, + stone_path_mode=stone_path_mode + ) + + # 显示结果 + if success: + success("测试成功完成", "结果") + else: + warning("测试未完全成功", "结果") + + # 清理资源 + ctrl.cleanup() + + except KeyboardInterrupt: + warning("用户中断测试", "中断") + except Exception as e: + error(f"测试过程中发生异常: {str(e)}", "异常") + + section("双轨道线居中测试", "结束") + +if __name__ == "__main__": + main() \ No newline at end of file