✨ 在 move_base_hori_line.py 中新增 follow_left_side_track 函数，实现机器狗沿左侧黄色轨迹线移动的控制逻辑。同时在 detect_track.py 中添加 detect_left_side_track 函数，用于检测左侧轨迹线，增强了机器狗的轨迹跟随能力和稳定性。更新了相关参数和日志记录功能，提升了调试信息的可读性。
This commit is contained in:
parent
d40c7d1922
commit
26527e7775
@ -7,7 +7,7 @@ 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
|
||||
from utils.detect_track import detect_horizontal_track_edge, 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
|
||||
@ -1123,6 +1123,255 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None,
|
||||
|
||||
return True
|
||||
|
||||
def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=30, observe=False):
|
||||
"""
|
||||
控制机器狗向左侧移动并靠近左侧的黄色轨迹线
|
||||
|
||||
参数:
|
||||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||||
target_distance: 目标与左侧线的像素距离，默认为150像素（保持一定间距）
|
||||
speed: 移动速度(米/秒)，默认为0.3米/秒
|
||||
max_time: 最大执行时间(秒)，默认为30秒
|
||||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||||
|
||||
返回:
|
||||
bool: 是否成功达到目标位置
|
||||
"""
|
||||
section("开始左侧轨迹线跟随", "左侧跟踪")
|
||||
|
||||
# 设置移动命令基本参数
|
||||
msg.mode = 11 # Locomotion模式
|
||||
msg.gait_id = 26 # 自变频步态
|
||||
msg.duration = 0 # wait next cmd
|
||||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||||
|
||||
# 记录起始时间
|
||||
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控制参数 - 侧向移动的PID参数
|
||||
kp_side = 0.002 # 比例系数
|
||||
ki_side = 0.0001 # 积分系数
|
||||
kd_side = 0.0005 # 微分系数
|
||||
|
||||
# 记录目标到达状态和稳定计数
|
||||
target_reached = False
|
||||
stable_count = 0
|
||||
required_stable_count = 10 # 需要连续稳定的检测次数
|
||||
|
||||
# PID控制变量
|
||||
previous_error = 0
|
||||
integral = 0
|
||||
|
||||
# 最大侧向速度限制
|
||||
max_side_velocity = 0.3 # m/s
|
||||
|
||||
# 检测成功计数器
|
||||
detection_success_count = 0
|
||||
detection_total_count = 0
|
||||
|
||||
# 保存上一次有效的检测结果，用于检测失败时的平滑过渡
|
||||
last_valid_track_info = None
|
||||
|
||||
# 滤波队列
|
||||
filter_size = 5
|
||||
distance_queue = []
|
||||
|
||||
# 开始跟踪循环
|
||||
while time.time() - start_time < max_time:
|
||||
# 获取当前图像
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
|
||||
# 检测左侧轨迹线
|
||||
detection_total_count += 1
|
||||
track_info, tracking_point = detect_left_side_track(image, observe=observe, delay=500 if observe else 0, save_log=True)
|
||||
|
||||
if track_info is not None:
|
||||
detection_success_count += 1
|
||||
|
||||
# 保存有效的轨迹信息
|
||||
last_valid_track_info = track_info
|
||||
|
||||
# 获取当前与左侧线的距离
|
||||
current_distance = track_info["distance_to_left"]
|
||||
|
||||
# 添加到滤波队列
|
||||
distance_queue.append(current_distance)
|
||||
if len(distance_queue) > filter_size:
|
||||
distance_queue.pop(0)
|
||||
|
||||
# 计算滤波后的距离值 (去除最大和最小值后的平均)
|
||||
if len(distance_queue) >= 3:
|
||||
filtered_distances = sorted(distance_queue)[1:-1] if len(distance_queue) > 2 else distance_queue
|
||||
filtered_distance = sum(filtered_distances) / len(filtered_distances)
|
||||
else:
|
||||
filtered_distance = current_distance
|
||||
|
||||
if observe and time.time() % 0.5 < 0.02:
|
||||
debug(f"原始左侧距离: {current_distance:.1f}px, 滤波后: {filtered_distance:.1f}px, 目标: {target_distance}px", "距离")
|
||||
|
||||
# 计算误差 - 正误差表示需要向左移动，负误差表示需要向右移动
|
||||
error = target_distance - filtered_distance
|
||||
|
||||
# 检查是否达到目标位置
|
||||
if abs(error) < 20: # 误差小于20像素视为达到目标
|
||||
stable_count += 1
|
||||
if stable_count >= required_stable_count:
|
||||
if not target_reached:
|
||||
target_reached = True
|
||||
if observe:
|
||||
success(f"已达到目标位置，误差: {abs(error):.1f}px", "成功")
|
||||
else:
|
||||
stable_count = 0
|
||||
target_reached = False
|
||||
|
||||
# 比例项
|
||||
p_control = kp_side * error
|
||||
|
||||
# 积分项 (添加积分限制以防止积分饱和)
|
||||
integral += error
|
||||
integral = max(-500, min(500, integral)) # 限制积分范围
|
||||
i_control = ki_side * integral
|
||||
|
||||
# 微分项
|
||||
derivative = error - previous_error
|
||||
d_control = kd_side * derivative
|
||||
previous_error = error
|
||||
|
||||
# 计算侧向速度
|
||||
side_velocity = p_control + i_control + d_control
|
||||
|
||||
# 限制侧向速度范围
|
||||
side_velocity = max(-max_side_velocity, min(max_side_velocity, side_velocity))
|
||||
|
||||
# 根据目标状态调整速度
|
||||
forward_speed = speed
|
||||
if target_reached:
|
||||
# 如果已达到目标，降低侧向速度以减少振荡
|
||||
side_velocity *= 0.5
|
||||
# 降低前进速度让侧向移动更平稳
|
||||
forward_speed *= 0.7
|
||||
|
||||
if observe and time.time() % 0.5 < 0.02:
|
||||
debug(f"误差: {error:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}", "控制")
|
||||
debug(f"控制指令: 前进={forward_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "速度")
|
||||
|
||||
# 设置速度命令 - [前进速度, 侧向速度, 角速度]
|
||||
# 侧向速度为正表示向左移动，为负表示向右移动
|
||||
msg.vel_des = [forward_speed, side_velocity, 0]
|
||||
|
||||
# 使用轨迹线斜率进行小角度调整，确保机器人方向与轨迹线平行
|
||||
# 斜率大于0表示向右倾斜，需要顺时针旋转(负角速度)
|
||||
# 斜率小于0表示向左倾斜，需要逆时针旋转(正角速度)
|
||||
if not track_info["is_vertical"]: # 对于非垂直线，进行角度校正
|
||||
slope = track_info["slope"]
|
||||
# 计算旋转角度 - 垂直线的ideal_angle应该是0
|
||||
angle_correction = -math.atan(1/slope) if abs(slope) > 0.01 else 0
|
||||
# 将角度校正应用为小的角速度
|
||||
angular_velocity = angle_correction * 0.2 # 角速度控制系数
|
||||
# 限制角速度范围
|
||||
angular_velocity = max(-0.2, min(0.2, angular_velocity))
|
||||
|
||||
if abs(angular_velocity) > 0.02: # 只在需要明显校正时应用
|
||||
msg.vel_des[2] = angular_velocity
|
||||
if observe and time.time() % 0.5 < 0.02:
|
||||
debug(f"应用角度校正，斜率: {slope:.2f}, 角速度: {angular_velocity:.3f}", "校正")
|
||||
|
||||
else:
|
||||
warning("未检测到左侧轨迹线", "警告")
|
||||
|
||||
# 如果之前有有效检测结果，使用上一次的控制值但降低强度
|
||||
if last_valid_track_info is not None and len(distance_queue) > 0:
|
||||
# 使用上一次的距离，但降低侧向速度
|
||||
reduced_speed = speed * 0.5 # 降低前进速度
|
||||
|
||||
# 如果上次距离已经接近目标，维持当前位置
|
||||
last_error = target_distance - distance_queue[-1]
|
||||
if abs(last_error) < 50: # 如果接近目标距离
|
||||
side_velocity = 0 # 停止侧向移动
|
||||
if observe:
|
||||
info("接近目标位置，保持当前侧向位置", "保持")
|
||||
else:
|
||||
# 向目标方向缓慢移动
|
||||
side_velocity = 0.05 * (1 if last_error > 0 else -1)
|
||||
if observe:
|
||||
info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整")
|
||||
|
||||
msg.vel_des = [reduced_speed, side_velocity, 0]
|
||||
if observe:
|
||||
warning(f"使用降低强度的控制: 前进={reduced_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "恢复")
|
||||
else:
|
||||
# 如果从未检测到轨迹线，降低速度直接向前
|
||||
reduced_speed = speed * 0.3
|
||||
msg.vel_des = [reduced_speed, 0, 0]
|
||||
if observe:
|
||||
warning(f"无法检测到左侧轨迹线，降低速度前进: {reduced_speed:.2f}m/s", "警告")
|
||||
|
||||
# 发送命令
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 短暂延时
|
||||
time.sleep(0.05)
|
||||
|
||||
# 计算已移动距离(仅用于记录)
|
||||
current_position = ctrl.odo_msg.xyz
|
||||
dx = current_position[0] - start_position[0]
|
||||
dy = current_position[1] - start_position[1]
|
||||
distance_moved = math.sqrt(dx*dx + dy*dy)
|
||||
|
||||
if observe and time.time() % 2.0 < 0.02: # 每2秒左右打印一次
|
||||
info(f"已移动: {distance_moved:.3f}米, 已用时间: {time.time() - start_time:.1f}秒", "移动")
|
||||
# 显示检测成功率
|
||||
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})", "统计")
|
||||
|
||||
# 平滑停止
|
||||
if observe:
|
||||
info("开始平滑停止", "停止")
|
||||
|
||||
# 先降低速度再停止，实现平滑停止
|
||||
slowdown_steps = 5
|
||||
for i in range(slowdown_steps, 0, -1):
|
||||
slowdown_factor = i / slowdown_steps
|
||||
msg.vel_des = [speed * slowdown_factor, 0, 0]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
time.sleep(0.1)
|
||||
|
||||
# 最后完全停止
|
||||
ctrl.base_msg.stop()
|
||||
|
||||
# 计算最终移动距离
|
||||
final_position = ctrl.odo_msg.xyz
|
||||
dx = final_position[0] - start_position[0]
|
||||
dy = final_position[1] - start_position[1]
|
||||
final_distance = math.sqrt(dx*dx + dy*dy)
|
||||
|
||||
if observe:
|
||||
# 在终点放置红色标记
|
||||
end_position = list(final_position)
|
||||
if hasattr(ctrl, 'place_marker'):
|
||||
ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True)
|
||||
|
||||
success(f"左侧轨迹跟随完成，总移动距离: {final_distance:.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})", "统计")
|
||||
|
||||
return target_reached
|
||||
|
||||
# 用法示例
|
||||
if __name__ == "__main__":
|
||||
move_to_hori_line(None, None, observe=True)
|
||||
@ -1132,4 +1381,6 @@ if __name__ == "__main__":
|
||||
arc_turn_around_hori_line(None, None, angle_deg=-90, observe=True)
|
||||
# 双轨道跟随
|
||||
follow_dual_tracks(None, None, observe=True)
|
||||
# 左侧轨迹跟随
|
||||
follow_left_side_track(None, None, observe=True)
|
||||
|
||||
25
logs/robot_2025-05-18.log
Normal file
25
logs/robot_2025-05-18.log
Normal file
@ -0,0 +1,25 @@
|
||||
2025-05-18 16:58:17 | ERROR | utils.log_helper - ❌ 左侧区域未检测到垂直线
|
||||
2025-05-18 16:58:45 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-18 16:58:46 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-18 16:58:47 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||
2025-05-18 16:58:48 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-18 16:58:49 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 17 条直线
|
||||
2025-05-18 16:58:50 | ERROR | utils.log_helper - ❌ 左侧区域未检测到垂直线
|
||||
2025-05-18 16:59:14 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-18 16:59:15 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-18 16:59:16 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||
2025-05-18 16:59:17 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-18 16:59:18 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 26 条直线
|
||||
2025-05-18 16:59:19 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线
|
||||
2025-05-18 16:59:20 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||
2025-05-18 16:59:21 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250518_165921_631983.jpg
|
||||
2025-05-18 16:59:21 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250518_165921_631983', 'tracking_point': (95, 1077), 'ground_intersection': (91, 1080), 'distance_to_left': 216.5, 'slope': -0.7530864197530864, 'line_mid_x': 216.5}
|
||||
2025-05-18 16:59:34 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-18 16:59:35 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-18 16:59:36 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||
2025-05-18 16:59:37 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-18 16:59:38 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 26 条直线
|
||||
2025-05-18 16:59:39 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线
|
||||
2025-05-18 16:59:40 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||
2025-05-18 16:59:41 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250518_165941_364168.jpg
|
||||
2025-05-18 16:59:41 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250518_165941_364168', 'tracking_point': (95, 1077), 'ground_intersection': (91, 1080), 'distance_to_left': 216.5, 'slope': -0.7530864197530864, 'line_mid_x': 216.5}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 46 KiB |
231
test/task-path-track/left_track_demo.py
Normal file
231
test/task-path-track/left_track_demo.py
Normal file
@ -0,0 +1,231 @@
|
||||
import cv2
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import argparse
|
||||
|
||||
# 添加父目录到系统路径
|
||||
current_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
project_root = os.path.dirname(os.path.dirname(current_dir))
|
||||
sys.path.append(project_root)
|
||||
|
||||
from utils.detect_track import detect_left_side_track
|
||||
|
||||
def process_image(image_path, save_dir=None, show_steps=False):
|
||||
"""处理单张图像"""
|
||||
print(f"处理图像: {image_path}")
|
||||
|
||||
# 检测左侧轨迹线
|
||||
start_time = time.time()
|
||||
track_info, tracking_point = detect_left_side_track(image_path, observe=show_steps, save_log=True)
|
||||
processing_time = time.time() - start_time
|
||||
|
||||
# 输出结果
|
||||
if track_info is not None and tracking_point is not None:
|
||||
print(f"处理时间: {processing_time:.3f}秒")
|
||||
print(f"最佳跟踪点: ({tracking_point[0]}, {tracking_point[1]})")
|
||||
print(f"距左边界: {track_info['distance_to_left']:.1f}像素")
|
||||
print(f"线段斜率: {track_info['slope']:.4f}")
|
||||
print(f"是否垂直: {track_info['is_vertical']}")
|
||||
print(f"线段中点: ({track_info['mid_x']:.1f}, {track_info['mid_y']:.1f})")
|
||||
print(f"地面交点: ({track_info['ground_intersection'][0]}, {track_info['ground_intersection'][1]})")
|
||||
|
||||
# 提取线段坐标
|
||||
x1, y1, x2, y2 = track_info['line']
|
||||
print(f"线段端点: ({x1}, {y1}) - ({x2}, {y2})")
|
||||
print("-" * 30)
|
||||
|
||||
# 如果指定了保存目录，加载原始图像并绘制检测结果
|
||||
if save_dir:
|
||||
if not os.path.exists(save_dir):
|
||||
os.makedirs(save_dir)
|
||||
|
||||
# 构建输出文件路径
|
||||
base_name = os.path.basename(image_path)
|
||||
out_path = os.path.join(save_dir, f"result_{base_name}")
|
||||
|
||||
# 加载原始图像
|
||||
img = cv2.imread(image_path)
|
||||
if img is not None:
|
||||
# 绘制检测结果
|
||||
height, width = img.shape[:2]
|
||||
center_x = width // 2
|
||||
|
||||
# 绘制左侧区域范围
|
||||
cv2.rectangle(img, (0, 0), (center_x, height), (255, 0, 0), 2)
|
||||
|
||||
# 绘制检测到的线段
|
||||
cv2.line(img, (x1, y1), (x2, y2), (0, 255, 0), 2)
|
||||
|
||||
# 绘制跟踪点
|
||||
cv2.circle(img, tracking_point, 8, (0, 0, 255), -1)
|
||||
|
||||
# 绘制地面交点
|
||||
cv2.circle(img, track_info['ground_intersection'], 8, (255, 0, 255), -1)
|
||||
|
||||
# 添加文本信息
|
||||
cv2.putText(img, f"斜率: {track_info['slope']:.2f}", (10, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
|
||||
cv2.putText(img, f"距左边界: {track_info['distance_to_left']:.1f}px", (10, 70),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
|
||||
|
||||
# 保存结果图像
|
||||
cv2.imwrite(out_path, img)
|
||||
print(f"结果已保存至: {out_path}")
|
||||
else:
|
||||
print("未能检测到左侧黄色轨迹线")
|
||||
|
||||
return track_info, tracking_point
|
||||
|
||||
def process_video(video_path, save_dir=None, show_steps=False):
|
||||
"""处理视频"""
|
||||
print(f"处理视频: {video_path}")
|
||||
|
||||
# 打开视频文件
|
||||
cap = cv2.VideoCapture(video_path)
|
||||
if not cap.isOpened():
|
||||
print("错误：无法打开视频文件")
|
||||
return
|
||||
|
||||
# 获取视频信息
|
||||
fps = cap.get(cv2.CAP_PROP_FPS)
|
||||
width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
|
||||
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
|
||||
frame_count = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
|
||||
|
||||
print(f"视频信息: {width}x{height}, {fps}fps, 总帧数: {frame_count}")
|
||||
|
||||
# 如果需要保存结果视频
|
||||
video_writer = None
|
||||
if save_dir:
|
||||
if not os.path.exists(save_dir):
|
||||
os.makedirs(save_dir)
|
||||
|
||||
# 构建输出文件路径
|
||||
base_name = os.path.basename(video_path)
|
||||
name, ext = os.path.splitext(base_name)
|
||||
out_path = os.path.join(save_dir, f"result_{name}{ext}")
|
||||
|
||||
# 创建视频写入器
|
||||
fourcc = cv2.VideoWriter_fourcc(*'mp4v') # 可以根据需要修改编码器
|
||||
video_writer = cv2.VideoWriter(out_path, fourcc, fps, (width, height))
|
||||
|
||||
# 帧计数器
|
||||
frame_idx = 0
|
||||
detect_success_count = 0
|
||||
processing_times = []
|
||||
|
||||
# 处理每一帧
|
||||
while True:
|
||||
ret, frame = cap.read()
|
||||
if not ret:
|
||||
break
|
||||
|
||||
frame_idx += 1
|
||||
print(f"\r处理帧 {frame_idx}/{frame_count}", end="")
|
||||
|
||||
# 检测左侧轨迹线
|
||||
start_time = time.time()
|
||||
track_info, tracking_point = detect_left_side_track(frame, observe=False, save_log=False)
|
||||
processing_time = time.time() - start_time
|
||||
processing_times.append(processing_time)
|
||||
|
||||
# 如果检测成功
|
||||
if track_info is not None and tracking_point is not None:
|
||||
detect_success_count += 1
|
||||
|
||||
# 提取线段坐标
|
||||
x1, y1, x2, y2 = track_info['line']
|
||||
|
||||
# 在帧上绘制检测结果
|
||||
center_x = width // 2
|
||||
|
||||
# 绘制左侧区域范围
|
||||
cv2.rectangle(frame, (0, 0), (center_x, height), (255, 0, 0), 2)
|
||||
|
||||
# 绘制检测到的线段
|
||||
cv2.line(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
|
||||
|
||||
# 绘制跟踪点
|
||||
cv2.circle(frame, tracking_point, 8, (0, 0, 255), -1)
|
||||
|
||||
# 绘制地面交点
|
||||
cv2.circle(frame, track_info['ground_intersection'], 8, (255, 0, 255), -1)
|
||||
|
||||
# 添加文本信息
|
||||
cv2.putText(frame, f"斜率: {track_info['slope']:.2f}", (10, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
|
||||
cv2.putText(frame, f"距左边界: {track_info['distance_to_left']:.1f}px", (10, 70),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
|
||||
cv2.putText(frame, f"帧: {frame_idx}/{frame_count}", (10, height-30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
|
||||
else:
|
||||
# 如果检测失败，显示错误消息
|
||||
cv2.putText(frame, "未检测到轨迹线", (10, 50),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
|
||||
|
||||
# 如果需要显示
|
||||
if show_steps:
|
||||
cv2.imshow('Left Track Detection', frame)
|
||||
key = cv2.waitKey(1)
|
||||
if key == 27: # ESC键退出
|
||||
break
|
||||
|
||||
# 如果需要保存
|
||||
if video_writer is not None:
|
||||
video_writer.write(frame)
|
||||
|
||||
# 清理资源
|
||||
cap.release()
|
||||
if video_writer is not None:
|
||||
video_writer.release()
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
# 打印统计信息
|
||||
print(f"\n视频处理完成")
|
||||
print(f"总帧数: {frame_count}")
|
||||
print(f"成功检测帧数: {detect_success_count}")
|
||||
print(f"检测成功率: {detect_success_count/frame_count*100:.2f}%")
|
||||
if processing_times:
|
||||
avg_time = sum(processing_times) / len(processing_times)
|
||||
print(f"平均处理时间: {avg_time*1000:.2f}ms")
|
||||
print(f"处理帧率: {1/avg_time:.2f}fps")
|
||||
|
||||
if save_dir:
|
||||
print(f"结果已保存至: {out_path}")
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='左侧黄色轨迹线检测演示程序')
|
||||
parser.add_argument('--input', type=str, default='res/path/image_20250513_162556.png', help='输入图像或视频的路径')
|
||||
parser.add_argument('--output', type=str, default='res/path/test/left_track_results/', help='输出结果的保存目录')
|
||||
parser.add_argument('--type', type=str, choices=['image', 'video'], help='输入类型，不指定会自动检测')
|
||||
parser.add_argument('--show', default=True, help='显示处理步骤')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# 检查输入路径
|
||||
if not os.path.exists(args.input):
|
||||
print(f"错误：文件 '{args.input}' 不存在")
|
||||
return
|
||||
|
||||
# 如果未指定类型，根据文件扩展名判断
|
||||
if args.type is None:
|
||||
ext = os.path.splitext(args.input)[1].lower()
|
||||
if ext in ['.jpg', '.jpeg', '.png', '.bmp']:
|
||||
args.type = 'image'
|
||||
elif ext in ['.mp4', '.avi', '.mov']:
|
||||
args.type = 'video'
|
||||
else:
|
||||
print(f"错误：无法确定文件类型 '{ext}'")
|
||||
return
|
||||
|
||||
# 根据类型处理
|
||||
if args.type == 'image':
|
||||
process_image(args.input, args.output, args.show)
|
||||
elif args.type == 'video':
|
||||
process_video(args.input, args.output, args.show)
|
||||
else:
|
||||
print("错误：不支持的类型")
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -610,3 +610,240 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True):
|
||||
}
|
||||
|
||||
return center_info, left_track_info, right_track_info
|
||||
|
||||
def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||||
"""
|
||||
检测视野左侧黄色轨道线，用于机器狗左侧靠线移动
|
||||
|
||||
参数:
|
||||
image: 输入图像，可以是文件路径或者已加载的图像数组
|
||||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||||
delay: 展示每个步骤的等待时间(毫秒)
|
||||
save_log: 是否保存日志和图像
|
||||
|
||||
返回:
|
||||
tuple: (线信息字典, 最佳跟踪点)
|
||||
"""
|
||||
# 如果输入是字符串（文件路径），则加载图像
|
||||
if isinstance(image, str):
|
||||
img = cv2.imread(image)
|
||||
else:
|
||||
img = image.copy()
|
||||
|
||||
if img is None:
|
||||
error("无法加载图像", "失败")
|
||||
return None, None
|
||||
|
||||
# 获取图像尺寸
|
||||
height, width = img.shape[:2]
|
||||
|
||||
# 计算图像中间和左侧区域的范围
|
||||
center_x = width // 2
|
||||
# 主要关注视野的左半部分
|
||||
left_region_width = center_x
|
||||
left_region_height = height
|
||||
left_bound = 0
|
||||
right_bound = center_x
|
||||
bottom_bound = height
|
||||
top_bound = 0
|
||||
|
||||
if observe:
|
||||
debug("步骤1: 原始图像已加载", "加载")
|
||||
region_img = img.copy()
|
||||
# 绘制左侧搜索区域
|
||||
cv2.rectangle(region_img, (left_bound, top_bound), (right_bound, bottom_bound), (255, 0, 0), 2)
|
||||
cv2.line(region_img, (center_x, 0), (center_x, height), (0, 0, 255), 2) # 中线
|
||||
cv2.imshow("左侧搜索区域", region_img)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 转换到HSV颜色空间以便更容易提取黄色
|
||||
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||||
|
||||
# 黄色的HSV范围 - 扩大范围以更好地捕捉不同光照条件下的黄色
|
||||
lower_yellow = np.array([15, 80, 80]) # 更宽松的黄色下限
|
||||
upper_yellow = np.array([35, 255, 255]) # 更宽松的黄色上限
|
||||
|
||||
# 创建黄色的掩码
|
||||
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
|
||||
|
||||
# 形态学操作以改善掩码
|
||||
kernel = np.ones((5, 5), np.uint8) # 增大kernel尺寸
|
||||
mask = cv2.dilate(mask, kernel, iterations=1)
|
||||
mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1) # 添加腐蚀操作去除噪点
|
||||
|
||||
if observe:
|
||||
debug("步骤2: 创建黄色掩码", "处理")
|
||||
cv2.imshow("黄色掩码", mask)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 裁剪左侧区域
|
||||
left_region_mask = mask[:, left_bound:right_bound]
|
||||
|
||||
if observe:
|
||||
debug("步骤3: 左侧区域掩码", "处理")
|
||||
cv2.imshow("左侧区域掩码", left_region_mask)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 边缘检测
|
||||
edges = cv2.Canny(mask, 50, 150, apertureSize=3)
|
||||
|
||||
if observe:
|
||||
debug("步骤4: 边缘检测", "处理")
|
||||
cv2.imshow("边缘检测", edges)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 霍夫变换检测直线
|
||||
lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25,
|
||||
minLineLength=height*0.15, maxLineGap=50) # 调整参数以检测更长的线段
|
||||
|
||||
if lines is None or len(lines) == 0:
|
||||
error("未检测到直线", "失败")
|
||||
return None, None
|
||||
|
||||
if observe:
|
||||
debug(f"步骤5: 检测到 {len(lines)} 条直线", "处理")
|
||||
lines_img = img.copy()
|
||||
for line in lines:
|
||||
x1, y1, x2, y2 = line[0]
|
||||
cv2.line(lines_img, (x1, y1), (x2, y2), (0, 255, 0), 2)
|
||||
cv2.imshow("检测到的直线", lines_img)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 筛选左侧区域内的近似垂直线
|
||||
left_vertical_lines = []
|
||||
for line in lines:
|
||||
x1, y1, x2, y2 = line[0]
|
||||
|
||||
# 确保线在左侧区域内
|
||||
if not (max(x1, x2) <= right_bound):
|
||||
continue
|
||||
|
||||
# 计算斜率 (避免除零错误)
|
||||
if abs(x2 - x1) < 5: # 几乎垂直的线
|
||||
slope = 100 # 设置一个较大的值表示接近垂直
|
||||
else:
|
||||
slope = (y2 - y1) / (x2 - x1)
|
||||
|
||||
# 筛选接近垂直的线 (斜率较大)
|
||||
if abs(slope) > 0.7: # 设置较宽松的垂直线斜率阈值
|
||||
line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2)
|
||||
# 计算线的中点坐标
|
||||
mid_x = (x1 + x2) / 2
|
||||
mid_y = (y1 + y2) / 2
|
||||
|
||||
# 保存线段、其坐标、斜率和长度
|
||||
left_vertical_lines.append((line[0], mid_x, mid_y, slope, line_length))
|
||||
|
||||
if len(left_vertical_lines) == 0:
|
||||
error("左侧区域未检测到垂直线", "失败")
|
||||
return None, None
|
||||
|
||||
if observe:
|
||||
debug(f"步骤6: 左侧区域找到 {len(left_vertical_lines)} 条垂直线", "处理")
|
||||
left_lines_img = img.copy()
|
||||
for line_info in left_vertical_lines:
|
||||
line, _, _, slope, _ = line_info
|
||||
x1, y1, x2, y2 = line
|
||||
cv2.line(left_lines_img, (x1, y1), (x2, y2), (0, 255, 255), 2)
|
||||
# 显示斜率
|
||||
cv2.putText(left_lines_img, f"{slope:.2f}", ((x1+x2)//2, (y1+y2)//2),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)
|
||||
cv2.imshow("左侧垂直线", left_lines_img)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 按线段长度和位置进行评分，优先选择更长且更靠近图像左边的线
|
||||
def score_left_line(line_info):
|
||||
_, mid_x, _, _, length = line_info
|
||||
# 线段越长分数越高
|
||||
length_score = min(1.0, length / (height * 0.3))
|
||||
# 越靠近左边分数越高
|
||||
position_score = 1.0 - (mid_x / center_x)
|
||||
# 综合评分
|
||||
return length_score * 0.7 + position_score * 0.3
|
||||
|
||||
# 对线段进行评分并排序
|
||||
left_vertical_lines = sorted(left_vertical_lines, key=score_left_line, reverse=True)
|
||||
|
||||
# 选择最佳的左侧线段
|
||||
best_left_line = left_vertical_lines[0]
|
||||
line, mid_x, mid_y, slope, length = best_left_line
|
||||
x1, y1, x2, y2 = line
|
||||
|
||||
# 确保线段的顺序是从上到下
|
||||
if y1 > y2:
|
||||
x1, x2 = x2, x1
|
||||
y1, y2 = y2, y1
|
||||
|
||||
# 计算最佳跟踪点 - 选择线段底部较靠近机器人的点
|
||||
tracking_point = (x2, y2) if y2 > y1 else (x1, y1)
|
||||
|
||||
# 计算线与地面的交点
|
||||
# 使用线段的方程: (y - y1) = slope * (x - x1)
|
||||
# 地面对应图像底部: y = height
|
||||
# 解这个方程得到交点的x坐标
|
||||
if abs(slope) < 0.01: # 几乎垂直
|
||||
ground_intersection_x = x1
|
||||
else:
|
||||
ground_intersection_x = x1 + (height - y1) / slope
|
||||
ground_intersection = (int(ground_intersection_x), height)
|
||||
|
||||
# 计算线与图像左边界的距离（以像素为单位）
|
||||
distance_to_left = mid_x
|
||||
|
||||
result_img = None
|
||||
if observe or save_log:
|
||||
result_img = img.copy()
|
||||
# 绘制检测到的最佳左侧线
|
||||
cv2.line(result_img, (x1, y1), (x2, y2), (255, 0, 0), 2)
|
||||
# 绘制图像中线
|
||||
cv2.line(result_img, (center_x, 0), (center_x, height), (0, 0, 255), 1)
|
||||
# 标记最佳跟踪点和地面交点
|
||||
cv2.circle(result_img, tracking_point, 10, (0, 255, 0), -1)
|
||||
cv2.circle(result_img, ground_intersection, 10, (0, 0, 255), -1)
|
||||
|
||||
# 显示信息
|
||||
cv2.putText(result_img, f"斜率: {slope:.2f}", (10, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
|
||||
cv2.putText(result_img, f"距左边界: {distance_to_left:.1f}px", (10, 70),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
|
||||
cv2.putText(result_img, f"地面交点: ({ground_intersection[0]}, {ground_intersection[1]})", (10, 110),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
|
||||
|
||||
if observe:
|
||||
debug("步骤7: 左侧最佳跟踪线和点", "显示")
|
||||
cv2.imshow("左侧最佳跟踪线和点", result_img)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 保存日志图像
|
||||
if save_log and result_img is not None:
|
||||
timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f")
|
||||
log_dir = "logs/image"
|
||||
os.makedirs(log_dir, exist_ok=True)
|
||||
img_path = os.path.join(log_dir, f"left_track_{timestamp}.jpg")
|
||||
cv2.imwrite(img_path, result_img)
|
||||
info(f"保存左侧轨迹线检测结果图像到: {img_path}", "日志")
|
||||
|
||||
# 保存文本日志信息
|
||||
log_info = {
|
||||
"timestamp": timestamp,
|
||||
"tracking_point": tracking_point,
|
||||
"ground_intersection": ground_intersection,
|
||||
"distance_to_left": distance_to_left,
|
||||
"slope": slope,
|
||||
"line_mid_x": mid_x
|
||||
}
|
||||
info(f"左侧轨迹线检测结果: {log_info}", "日志")
|
||||
|
||||
# 创建线段信息字典
|
||||
track_info = {
|
||||
"line": line,
|
||||
"slope": slope,
|
||||
"tracking_point": tracking_point,
|
||||
"ground_intersection": ground_intersection,
|
||||
"distance_to_left": distance_to_left,
|
||||
"mid_x": mid_x,
|
||||
"mid_y": mid_y,
|
||||
"is_vertical": abs(slope) > 5.0 # 判断是否接近垂直
|
||||
}
|
||||
|
||||
return track_info, tracking_point
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user