diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index c94fc12..c8725c0 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -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) \ No newline at end of file diff --git a/logs/robot_2025-05-18.log b/logs/robot_2025-05-18.log new file mode 100644 index 0000000..b008187 --- /dev/null +++ b/logs/robot_2025-05-18.log @@ -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} diff --git a/res/path/test/left_track_results/result_image_20250513_162556.png b/res/path/test/left_track_results/result_image_20250513_162556.png new file mode 100644 index 0000000..bb43db2 Binary files /dev/null and b/res/path/test/left_track_results/result_image_20250513_162556.png differ diff --git a/test/task-path-track/left_track_demo.py b/test/task-path-track/left_track_demo.py new file mode 100644 index 0000000..dacc369 --- /dev/null +++ b/test/task-path-track/left_track_demo.py @@ -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() \ No newline at end of file diff --git a/utils/detect_track.py b/utils/detect_track.py index 37ab7a5..5ec3b64 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -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