🎏 task-1 v0.2
This commit is contained in:
parent
c47213e8a3
commit
924a8e07c6
@ -7,7 +7,10 @@ import sys
|
||||
import os
|
||||
|
||||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
from utils.detect_track import detect_horizontal_track_edge, detect_dual_track_lines, detect_left_side_track
|
||||
from utils.detect_track import (
|
||||
detect_horizontal_track_edge, detect_horizontal_track_edge_v2,
|
||||
detect_dual_track_lines, detect_left_side_track
|
||||
)
|
||||
from base_move.turn_degree import turn_degree
|
||||
from base_move.go_straight import go_straight
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
@ -15,7 +18,7 @@ from utils.log_helper import LogHelper, get_logger, section, info, debug, warnin
|
||||
# 创建此模块特定的日志记录器
|
||||
logger = get_logger("横线移动")
|
||||
|
||||
def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||||
def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3, detect_func_version=1):
|
||||
"""
|
||||
控制机器人旋转到横向线水平的位置
|
||||
|
||||
@ -41,8 +44,14 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||||
while attempts < max_attempts and not aligned:
|
||||
section(f"尝试校准横线 {attempts+1}/{max_attempts}", "校准")
|
||||
|
||||
# 检测横向线边缘
|
||||
# 检测横向线边缘; 不同版本
|
||||
if detect_func_version == 1:
|
||||
edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
|
||||
elif detect_func_version == 2:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v2(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
|
||||
else:
|
||||
error("未指定检测版本，请检查参数", "失败")
|
||||
return False
|
||||
|
||||
print('😺', edge_info)
|
||||
|
||||
@ -291,7 +300,7 @@ def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False):
|
||||
"""
|
||||
# 首先校准到水平
|
||||
section("校准到横向线水平", "校准")
|
||||
aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
|
||||
aligned = align_to_horizontal_line(ctrl, msg, detect_func_version=2, observe=observe)
|
||||
|
||||
if not aligned:
|
||||
error("无法校准到横向线水平，停止移动", "停止")
|
||||
@ -301,7 +310,7 @@ def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False):
|
||||
|
||||
go_straight(ctrl, msg, distance=distance, speed=speed, observe=observe)
|
||||
|
||||
def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||||
def move_to_hori_line(ctrl, msg, target_distance=0.5, absolute_deg=200, observe=False):
|
||||
"""
|
||||
控制机器人校准并移动到横向线前的指定距离
|
||||
|
||||
@ -316,6 +325,9 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||||
"""
|
||||
# 首先校准到水平
|
||||
section("校准到横向线水平", "校准")
|
||||
if absolute_deg < 200:
|
||||
aligned = turn_degree(ctrl, msg, absolute_deg, absolute=True)
|
||||
else:
|
||||
aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
|
||||
|
||||
if not aligned:
|
||||
@ -426,10 +438,13 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||||
def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2,
|
||||
pass_align=False,
|
||||
radius=None,
|
||||
min_radius=0.1,
|
||||
max_radius=5,
|
||||
scan_qrcode=False,
|
||||
qr_check_interval=0.3,
|
||||
observe=False,
|
||||
bad_big_angle_corret=False):
|
||||
bad_big_angle_corret=False,
|
||||
no_end_reset=False,):
|
||||
"""
|
||||
对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
|
||||
参数:
|
||||
@ -443,6 +458,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2,
|
||||
qr_check_interval: QR码检查间隔时间(秒)，默认为0.3秒
|
||||
|
||||
bad_big_angle_corret: 是否对大角度错误进行修正，默认为False # TODO clear
|
||||
no_end_reset: 是否在旋转完成后不进行位置重置，默认为False
|
||||
返回:
|
||||
bool或元组: 如果scan_qrcode为False，返回bool表示是否成功完成操作；
|
||||
如果scan_qrcode为True，返回(bool, str)元组，表示(是否成功完成操作, QR码扫描结果)
|
||||
@ -538,8 +554,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2,
|
||||
w = math.copysign(base_w, angle_deg) # 保持角速度与角度同号
|
||||
|
||||
# 确保r有一个最小值，避免速度过低
|
||||
min_radius = 0.2 # 最小半径，单位：米
|
||||
effective_r = max(r, min_radius)
|
||||
effective_r = min(max_radius, max(r, min_radius))
|
||||
|
||||
# 线速度，必须与角速度和半径的关系一致：v = w * r
|
||||
# 这样才能保证走圆弧
|
||||
@ -548,7 +563,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2,
|
||||
info(f"开始移动圆弧，半径: {effective_r:.3f}米, 线速度: {v:.3f}m/s, 角速度: {w:.3f}rad/s")
|
||||
if observe:
|
||||
if effective_r != r:
|
||||
warning(f"注意: 实际半径 {r:.3f}米 太小，使用最小半径 {effective_r:.3f}米 计算速度", "警告")
|
||||
warning(f"注意: 实际半径 {r:.3f}米，使用半径 {effective_r:.3f}米 计算速度", "警告")
|
||||
|
||||
t = abs(angle_rad / w) # 运动时间，取绝对值保证为正
|
||||
|
||||
@ -855,7 +870,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2,
|
||||
if abs_angle_deg >= 170:
|
||||
adjustment_threshold = 10.0 # 大角度旋转使用更大的微调阈值，因为初步旋转已经确保大致方向正确
|
||||
|
||||
if angle_error > adjustment_threshold: # 如果误差超过阈值
|
||||
if angle_error > adjustment_threshold and not no_end_reset: # 如果误差超过阈值
|
||||
if observe:
|
||||
warning(f"角度误差: {angle_error:.2f}度，进行微调", "角度")
|
||||
|
||||
|
||||
@ -1,67 +0,0 @@
|
||||
2025-05-19 20:41:09 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250519_204109_537087.jpg
|
||||
2025-05-19 20:41:09 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250519_204109_537087', 'edge_point': (397, 816), 'distance_to_center': -563, 'slope': 0.0, 'distance_to_bottom': 264.0, 'intersection_point': (960, 816), 'is_truncated': False}
|
||||
2025-05-19 20:44:06 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-19 20:44:07 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-19 20:44:08 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-19 20:44:09 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-19 20:44:10 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-19 20:44:11 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 15 条直线
|
||||
2025-05-19 20:44:12 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 13 条水平线
|
||||
2025-05-19 20:44:13 | DEBUG | utils.log_helper - 👁️ 步骤7: 找到边缘点 (397, 816)
|
||||
2025-05-19 20:44:14 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-19 20:44:15 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250519_204415_945888.jpg
|
||||
2025-05-19 20:44:15 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250519_204415_945888', 'edge_point': (397, 816), 'distance_to_center': -563, 'slope': 0.0, 'distance_to_bottom': 264.0, 'intersection_point': (960, 816), 'is_truncated': False}
|
||||
2025-05-19 20:44:57 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-19 20:44:58 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-19 20:44:59 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-19 20:45:00 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-19 20:45:01 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-19 20:45:02 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 10 条直线
|
||||
2025-05-19 20:45:03 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 8 条水平线
|
||||
2025-05-19 20:45:04 | DEBUG | utils.log_helper - 👁️ 步骤7: 找到边缘点 (831, 971)
|
||||
2025-05-19 20:45:05 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-19 20:45:06 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250519_204506_674753.jpg
|
||||
2025-05-19 20:45:06 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250519_204506_674753', 'edge_point': (831, 971), 'distance_to_center': -129, 'slope': -0.07004830917874397, 'distance_to_bottom': 118.036231884058, 'intersection_point': (960, 961), 'is_truncated': False}
|
||||
2025-05-19 20:45:37 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-19 20:45:39 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-19 20:45:40 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-19 20:45:41 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-19 20:45:42 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-19 20:45:43 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 17 条直线
|
||||
2025-05-19 20:45:44 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 12 条水平线
|
||||
2025-05-19 20:45:45 | DEBUG | utils.log_helper - 👁️ 步骤7: 找到边缘点 (923, 849)
|
||||
2025-05-19 20:45:46 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-19 20:45:47 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250519_204547_188654.jpg
|
||||
2025-05-19 20:45:47 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250519_204547_188654', 'edge_point': (923, 849), 'distance_to_center': -37, 'slope': 0.0, 'distance_to_bottom': 231.0, 'intersection_point': (960, 849), 'is_truncated': False}
|
||||
2025-05-19 20:47:53 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-19 20:47:55 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-19 20:47:56 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-19 20:47:57 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-19 20:47:58 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-19 20:47:59 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 17 条直线
|
||||
2025-05-19 20:48:00 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 12 条水平线
|
||||
2025-05-19 20:48:01 | DEBUG | utils.log_helper - 👁️ 步骤7: 找到边缘点 (923, 849)
|
||||
2025-05-19 20:48:02 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-19 20:48:03 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250519_204803_276122.jpg
|
||||
2025-05-19 20:48:03 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250519_204803_276122', 'edge_point': (923, 849), 'distance_to_center': -37, 'slope': 0.0, 'distance_to_bottom': 231.0, 'intersection_point': (960, 849), 'is_truncated': False}
|
||||
2025-05-19 20:50:32 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-19 20:50:33 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-19 20:50:34 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-19 20:50:35 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-19 20:50:36 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-19 20:50:37 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 25 条直线
|
||||
2025-05-19 20:50:38 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 18 条水平线
|
||||
2025-05-19 20:50:39 | DEBUG | utils.log_helper - 👁️ 步骤7: 找到边缘点 (92, 1077)
|
||||
2025-05-19 20:50:40 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-19 20:50:41 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250519_205041_583040.jpg
|
||||
2025-05-19 20:50:41 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250519_205041_583040', 'edge_point': (92, 1077), 'distance_to_center': -868, 'slope': -0.07008086253369272, 'distance_to_bottom': 63.83018867924534, 'intersection_point': (960, 1016), 'is_truncated': False}
|
||||
2025-05-19 21:24:41 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-19 21:24:42 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-19 21:24:43 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-19 21:24:44 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-19 21:24:45 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-19 21:24:46 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 18 条直线
|
||||
2025-05-19 21:24:47 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 3 条水平线
|
||||
2025-05-19 21:24:48 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-19 21:24:49 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250519_212449_369767.jpg
|
||||
2025-05-19 21:24:49 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250519_212449_369767', 'edge_point': (540, 907), 'distance_to_center': -420, 'slope': -0.12202852614896989, 'distance_to_bottom': 224.25198098256737, 'intersection_point': (960, 855), 'score': 0.5611365011519962, 'valid': True, 'reason': ''}
|
||||
@ -1,10 +0,0 @@
|
||||
2025-05-20 09:24:31 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-20 09:24:32 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-20 09:24:33 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-20 09:24:34 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-20 09:24:35 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-20 09:24:36 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 18 条直线
|
||||
2025-05-20 09:24:37 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 3 条水平线
|
||||
2025-05-20 09:24:38 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-20 09:24:39 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250520_092439_823936.jpg
|
||||
2025-05-20 09:24:39 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250520_092439_823936', 'edge_point': (540, 907), 'distance_to_center': -420, 'slope': -0.12202852614896989, 'distance_to_bottom': 224.25198098256737, 'intersection_point': (960, 855), 'score': 0.5611365011519962, 'valid': True, 'reason': ''}
|
||||
@ -1,90 +0,0 @@
|
||||
2025-05-22 12:20:16 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-22 12:20:18 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-22 12:20:19 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-22 12:20:20 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-22 12:20:21 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-22 12:20:22 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 8 条直线
|
||||
2025-05-22 12:20:23 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 5 条水平线 (下方: 5, 上方: 0)
|
||||
2025-05-22 12:20:24 | DEBUG | utils.log_helper - 🐞 选择下方水平线
|
||||
2025-05-22 12:20:24 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-22 12:20:25 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122025_111891.jpg
|
||||
2025-05-22 12:20:25 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122025_111891', 'edge_point': (1919, 862), 'distance_to_center': 959, 'slope': 0.03486394557823129, 'distance_to_bottom': 251.43452380952385, 'intersection_point': (960, 828), 'score': 0.6679311933106575, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])}
|
||||
2025-05-22 12:20:38 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-22 12:20:39 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-22 12:20:40 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-22 12:20:41 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-22 12:20:42 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-22 12:20:43 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 8 条直线
|
||||
2025-05-22 12:20:44 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 5 条水平线 (下方: 5, 上方: 0)
|
||||
2025-05-22 12:20:45 | DEBUG | utils.log_helper - 🐞 选择下方水平线
|
||||
2025-05-22 12:20:45 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-22 12:20:47 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122046_987809.jpg
|
||||
2025-05-22 12:20:47 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122046_987809', 'edge_point': (1919, 862), 'distance_to_center': 959, 'slope': 0.03486394557823129, 'distance_to_bottom': 251.43452380952385, 'intersection_point': (960, 828), 'score': 0.6679311933106575, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])}
|
||||
2025-05-22 12:21:15 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-22 12:21:16 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-22 12:21:17 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-22 12:21:18 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-22 12:21:19 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-22 12:21:20 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 8 条直线
|
||||
2025-05-22 12:21:21 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 5 条水平线 (下方: 5, 上方: 0)
|
||||
2025-05-22 12:21:22 | DEBUG | utils.log_helper - 🐞 选择下方水平线
|
||||
2025-05-22 12:21:22 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-22 12:21:23 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122123_802326.jpg
|
||||
2025-05-22 12:21:23 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122123_802326', 'edge_point': (1919, 862), 'distance_to_center': 959, 'slope': 0.03486394557823129, 'distance_to_bottom': 251.43452380952385, 'intersection_point': (960, 828), 'score': 0.6679311933106575, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])}
|
||||
2025-05-22 12:22:26 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-22 12:22:28 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-22 12:22:29 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-22 12:22:30 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-22 12:22:31 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-22 12:22:32 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 8 条直线
|
||||
2025-05-22 12:22:33 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 5 条水平线 (下方: 5, 上方: 0)
|
||||
2025-05-22 12:22:34 | DEBUG | utils.log_helper - 🐞 选择下方水平线
|
||||
2025-05-22 12:22:34 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-22 12:22:35 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122235_241410.jpg
|
||||
2025-05-22 12:22:35 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122235_241410', 'edge_point': (1919, 862), 'distance_to_center': 959, 'slope': 0.03486394557823129, 'distance_to_bottom': 251.43452380952385, 'intersection_point': (960, 828), 'score': 0.6679311933106575, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])}
|
||||
2025-05-22 12:24:10 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-22 12:24:11 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-22 12:24:12 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-22 12:24:13 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-22 12:24:14 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-22 12:24:15 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 28 条直线
|
||||
2025-05-22 12:24:16 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 6 条水平线 (下方: 6, 上方: 0)
|
||||
2025-05-22 12:24:17 | DEBUG | utils.log_helper - 🐞 选择下方水平线
|
||||
2025-05-22 12:24:17 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-22 12:24:18 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122418_738782.jpg
|
||||
2025-05-22 12:24:18 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122418_738782', 'edge_point': (91, 1078), 'distance_to_center': -869, 'slope': -0.07092198581560284, 'distance_to_bottom': 63.631205673758814, 'intersection_point': (960, 1016), 'score': 0.8014868667518458, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])}
|
||||
2025-05-22 12:24:54 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-22 12:24:55 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-22 12:24:56 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-22 12:24:57 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-22 12:24:58 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-22 12:24:59 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 11 条直线
|
||||
2025-05-22 12:25:00 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 1 条水平线 (下方: 1, 上方: 0)
|
||||
2025-05-22 12:25:01 | DEBUG | utils.log_helper - 🐞 选择下方水平线
|
||||
2025-05-22 12:25:01 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-22 12:25:02 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122502_339369.jpg
|
||||
2025-05-22 12:25:02 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122502_339369', 'edge_point': (1439, 899), 'distance_to_center': 479, 'slope': -0.07083333333333333, 'distance_to_bottom': 147.07083333333333, 'intersection_point': (960, 932), 'score': 0.5594172208352652, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])}
|
||||
2025-05-22 12:33:02 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_123302_872381.jpg
|
||||
2025-05-22 12:33:02 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_123302_872381', 'edge_point': (320, 1009), 'distance_to_center': -640, 'slope': -0.07331047777324741, 'distance_to_bottom': 117.91870577487839, 'intersection_point': (960, 962)}
|
||||
2025-05-22 12:33:26 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_123326_235741.jpg
|
||||
2025-05-22 12:33:26 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_123326_235741', 'edge_point': (320, 1009), 'distance_to_center': -640, 'slope': -0.07331047777324741, 'distance_to_bottom': 117.91870577487839, 'intersection_point': (960, 962)}
|
||||
2025-05-22 12:34:09 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-22 12:34:10 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-22 12:34:10 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-22 12:34:11 | DEBUG | utils.log_helper - 🐞 正在处理底部边缘点
|
||||
2025-05-22 12:34:12 | DEBUG | utils.log_helper - 🐞 显示拟合线段
|
||||
2025-05-22 12:34:13 | DEBUG | utils.log_helper - 👁️ 步骤5: 找到边缘点 (320, 1009)
|
||||
2025-05-22 12:34:14 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-22 12:34:14 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_123414_979258.jpg
|
||||
2025-05-22 12:34:14 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_123414_979258', 'edge_point': (320, 1009), 'distance_to_center': -640, 'slope': -0.07331047777324741, 'distance_to_bottom': 117.91870577487839, 'intersection_point': (960, 962)}
|
||||
2025-05-22 12:34:34 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-22 12:34:35 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-22 12:34:36 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分
|
||||
2025-05-22 12:34:36 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点
|
||||
2025-05-22 12:34:37 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-22 12:34:38 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 11 条直线
|
||||
2025-05-22 12:34:39 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 1 条水平线 (下方: 1, 上方: 0)
|
||||
2025-05-22 12:34:40 | DEBUG | utils.log_helper - 🐞 选择下方水平线
|
||||
2025-05-22 12:34:40 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点
|
||||
2025-05-22 12:34:41 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_123441_043307.jpg
|
||||
2025-05-22 12:34:41 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_123441_043307', 'edge_point': (1439, 899), 'distance_to_center': 479, 'slope': -0.07083333333333333, 'distance_to_bottom': 147.07083333333333, 'intersection_point': (960, 932), 'score': 0.5594172208352652, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])}
|
||||
@ -50,7 +50,7 @@ def run_task_1(ctrl, msg):
|
||||
|
||||
section('任务1-2：移动到横线', "移动")
|
||||
# 执行常规的移动操作，不需要 QR 码扫描
|
||||
move_to_hori_line(ctrl, msg, target_distance=1, observe=observe)
|
||||
move_to_hori_line(ctrl, msg, target_distance=1.1, observe=observe)
|
||||
|
||||
section('任务1-3：转弯', "旋转")
|
||||
direction = False # if res['qr_result'] == 'A-2' else True
|
||||
@ -58,12 +58,14 @@ def run_task_1(ctrl, msg):
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
angle_deg=-170, # 负值表示右转
|
||||
target_distance=0.8,
|
||||
target_distance=0.6,
|
||||
min_radius=0.3,
|
||||
max_radius=0.4,
|
||||
#
|
||||
pass_align=True,
|
||||
observe=observe,
|
||||
# TODO clear
|
||||
bad_big_angle_corret=True
|
||||
bad_big_angle_corret=True,
|
||||
)
|
||||
|
||||
if turn_success:
|
||||
@ -73,7 +75,7 @@ def run_task_1(ctrl, msg):
|
||||
return
|
||||
|
||||
section('任务1-4：直线移动', "移动")
|
||||
move_distance = 0.5
|
||||
move_distance = 0.3
|
||||
move_speed = 0.5
|
||||
if not direction:
|
||||
# TODO to check
|
||||
@ -93,7 +95,7 @@ def run_task_1(ctrl, msg):
|
||||
ctrl.base_msg.stand_up()
|
||||
|
||||
section('任务1-6：返回', "移动")
|
||||
go_straight(ctrl, msg, distance=-(move_distance + res['radius'] + 0.2), speed=0.3, observe=observe)
|
||||
go_straight(ctrl, msg, distance=-(move_distance + res['radius'] + 0.3), observe=observe)
|
||||
|
||||
# turn and back
|
||||
turn_degree(ctrl, msg, degree=-90)
|
||||
@ -102,15 +104,16 @@ def run_task_1(ctrl, msg):
|
||||
turn_success, res = arc_turn_around_hori_line(
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
radius=res['radius'] * 2,
|
||||
angle_deg=85,
|
||||
radius=res['radius'] * 2 + 0.3,
|
||||
angle_deg=90,
|
||||
#
|
||||
pass_align=True,
|
||||
observe=observe
|
||||
observe=observe,
|
||||
no_end_reset=True,
|
||||
)
|
||||
|
||||
section('任务1-8：直线移动', "移动")
|
||||
move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
|
||||
move_to_hori_line(ctrl, msg, target_distance=0.35, absolute_deg=90, observe=observe)
|
||||
|
||||
section('任务1-9：90度旋转', "旋转")
|
||||
turn_degree(ctrl, msg, degree=0, absolute=True) # TODO
|
||||
@ -119,4 +122,3 @@ def run_task_1(ctrl, msg):
|
||||
# TODO
|
||||
|
||||
success("任务1完成", "完成")
|
||||
|
||||
|
||||
@ -19,7 +19,7 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True
|
||||
edge_point: 赛道前方边缘点的坐标 (x, y)
|
||||
edge_info: 边缘信息字典
|
||||
"""
|
||||
# observe = False # TSET
|
||||
observe = False # TEST
|
||||
# 如果输入是字符串（文件路径），则加载图像
|
||||
if isinstance(image, str):
|
||||
img = cv2.imread(image)
|
||||
@ -311,6 +311,12 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True
|
||||
timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f")
|
||||
log_dir = "logs/image"
|
||||
os.makedirs(log_dir, exist_ok=True)
|
||||
|
||||
# origin image
|
||||
origin_image_path = os.path.join(log_dir, f"origin_horizontal_edge_{timestamp}.jpg")
|
||||
cv2.imwrite(origin_image_path, img)
|
||||
info(f"保存原始图像到: {origin_image_path}", "日志")
|
||||
|
||||
img_path = os.path.join(log_dir, f"horizontal_edge_{timestamp}.jpg")
|
||||
cv2.imwrite(img_path, result_img)
|
||||
info(f"保存横向边缘检测结果图像到: {img_path}", "日志")
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user