更新日志文件，增加双轨迹线检测的详细调试信息和结果记录，同时删除过时的图像文件以优化存储。增强了对不同路况的处理能力，提升了检测准确性。
This commit is contained in:
parent
c46ddbf7a4
commit
cc2a6ac41e
268
base_move/center_on_dual_tracks.py
Normal file
268
base_move/center_on_dual_tracks.py
Normal file
@ -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
|
||||
@ -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 - ℹ️ 选择石板路模式结果
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 57 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 59 KiB |
73
test/test_image.py
Normal file
73
test/test_image.py
Normal file
@ -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()
|
||||
243
test/test_offline_centering.py
Normal file
243
test/test_offline_centering.py
Normal file
@ -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()
|
||||
101
test_center_on_tracks.py
Normal file
101
test_center_on_tracks.py
Normal file
@ -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()
|
||||
Loading…
x
Reference in New Issue
Block a user