diff --git a/task_3/task_3.py b/task_3/task_3.py index 3432722..298fbc0 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -6,6 +6,8 @@ import copy import math import lcm import numpy as np +import cv2 +import tempfile # 添加父目录到路径,以便能够导入utils sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) @@ -17,6 +19,7 @@ from base_move.turn_degree import turn_degree, turn_degree_v2 from base_move.go_straight import go_straight from base_move.go_to_xy import go_to_x_v2 from file_send_lcmt import file_send_lcmt +from utils.yellow_area_analyzer import analyze_yellow_area_ratio # 创建本模块特定的日志记录器 logger = get_logger("任务3") @@ -35,6 +38,7 @@ robot_cmd = { 'value':0, 'duration':0 } + def pass_up_down(ctrl, msg): usergait_msg = file_send_lcmt() lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") @@ -332,6 +336,164 @@ def pass_up_down(ctrl, msg): pass +def go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3, max_time=30, observe=True): + """ + 控制机器人直走,直到摄像头检测到黄色区域比例超过指定阈值后开始下降时停止 + + 参数: + ctrl: Robot_Ctrl 对象,包含里程计信息 + msg: robot_control_cmd_lcmt 对象,用于发送命令 + yellow_ratio_threshold: 黄色区域占比阈值(0-1之间的浮点数),默认为0.15 + speed: 前进速度(米/秒),默认为0.3米/秒 + max_time: 最大行走时间(秒),默认为30秒 + observe: 是否输出中间状态信息和可视化结果,默认为True + + 返回: + bool: 是否成功检测到黄色区域并停止 + """ + section("开始直行寻找黄色区域", "黄色检测") + + # 设置移动命令基本参数 + msg.mode = 11 # Locomotion模式 + msg.gait_id = 26 # 自变频步态 + msg.duration = 0 # wait next cmd + msg.step_height = [0.06, 0.06] # 抬腿高度 + msg.vel_des = [speed, 0, 0] # [前进速度, 侧向速度, 角速度] + + # 记录起始时间和位置 + start_time = time.time() + start_position = list(ctrl.odo_msg.xyz) + + if observe: + info(f"开始寻找黄色区域,阈值: {yellow_ratio_threshold:.2%}", "启动") + debug(f"起始位置: {start_position}", "位置") + + # 检测间隔 + check_interval = 0.3 # 秒 + last_check_time = 0 + + # 黄色区域监测变量 + yellow_peak_detected = False # 是否检测到峰值 + yellow_decreasing = False # 是否开始下降 + max_yellow_ratio = 0.0 # 记录最大黄色区域占比 + yellow_ratio_history = [] # 记录黄色区域占比历史 + history_window_size = 5 # 历史窗口大小,用于平滑处理 + + try: + # 开始移动 + msg.life_count += 1 + ctrl.Send_cmd(msg) + + # 持续检测黄色区域 + while time.time() - start_time < max_time and not yellow_decreasing: + current_time = time.time() + + # 定期发送移动命令保持移动状态 + if current_time - last_check_time >= check_interval: + # 获取当前图像并保存到临时文件 + current_image = ctrl.image_processor.get_current_image() + + # 创建临时文件保存图像 + with tempfile.NamedTemporaryFile(suffix='.jpg', delete=False) as temp_file: + temp_filename = temp_file.name + cv2.imwrite(temp_filename, current_image) + + try: + # 分析图像中的黄色区域 + yellow_ratio = analyze_yellow_area_ratio(temp_filename, debug=False, save_result=False) + + # 添加到历史记录 + yellow_ratio_history.append(yellow_ratio) + if len(yellow_ratio_history) > history_window_size: + yellow_ratio_history.pop(0) + + # 计算平滑后的当前黄色占比(使用最近几次的平均值以减少噪声) + current_smooth_ratio = sum(yellow_ratio_history) / len(yellow_ratio_history) + + # 计算已移动距离(仅用于显示) + 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: + info(f"当前黄色区域占比: {yellow_ratio:.2%}, 平滑值: {current_smooth_ratio:.2%}, 已移动: {distance_moved:.2f}米", "检测") + + # 检测是否达到阈值(开始监测峰值) + if current_smooth_ratio >= yellow_ratio_threshold: + # 更新最大值 + if current_smooth_ratio > max_yellow_ratio: + max_yellow_ratio = current_smooth_ratio + if not yellow_peak_detected: + yellow_peak_detected = True + if observe: + info(f"黄色区域占比超过阈值,开始监测峰值", "检测") + # 检测是否开始下降 + elif yellow_peak_detected and current_smooth_ratio < max_yellow_ratio * 0.9: # 下降到峰值的90%以下认为开始下降 + yellow_decreasing = True + if observe: + success(f"检测到黄色区域占比开始下降,峰值: {max_yellow_ratio:.2%}, 当前: {current_smooth_ratio:.2%}", "检测") + + finally: + # 删除临时文件 + try: + os.unlink(temp_filename) + except: + pass + + # 更新心跳 + msg.life_count += 1 + ctrl.Send_cmd(msg) + last_check_time = current_time + + # 小间隔 + time.sleep(0.05) + + # 平滑停止 + if yellow_decreasing: + 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: + if yellow_decreasing: + success(f"成功检测到黄色区域峰值并停止,峰值占比: {max_yellow_ratio:.2%}, 总移动距离: {final_distance:.2f}米", "完成") + else: + warning(f"未能在限定时间内检测到黄色区域峰值,总移动距离: {final_distance:.2f}米", "超时") + + return yellow_decreasing + + except KeyboardInterrupt: + # 处理键盘中断 + ctrl.base_msg.stop() + if observe: + warning("操作被用户中断", "中断") + return False + except Exception as e: + # 处理其他异常 + ctrl.base_msg.stop() + if observe: + error(f"发生错误: {str(e)}", "错误") + return False + + def run_task_3(ctrl, msg): section('任务3:步态切换', "启动") info('开始执行任务3...', "启动") @@ -342,4 +504,19 @@ def run_task_3(ctrl, msg): # pass_up_down(ctrl, msg) section('任务3-2:yellow stop', "开始") + go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3) + + # 原地站立3秒 + section("原地站立3秒", "站立") + msg.mode = 11 # Locomotion模式 + msg.gait_id = 26 # 自变频步态 + msg.duration = 0 # wait next cmd + msg.step_height = [0.06, 0.06] # 抬腿高度 + msg.vel_des = [0, 0, 0] # 零速度,原地站立 + msg.life_count += 1 + ctrl.Send_cmd(msg) + + info("开始原地站立3秒", "站立") + time.sleep(3) + info("完成原地站立", "站立") diff --git a/utils/yellow_area_analyzer.py b/utils/yellow_area_analyzer.py new file mode 100644 index 0000000..006453f --- /dev/null +++ b/utils/yellow_area_analyzer.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import cv2 +import numpy as np +import os +import argparse +import matplotlib.pyplot as plt + +def analyze_yellow_area_ratio(image_path, debug=False, save_result=False): + """ + 专门针对黄色区域的分析算法 + + 参数: + image_path: 图片路径 + debug: 是否显示处理过程中的图像,用于调试 + save_result: 是否保存处理结果图像 + + 返回: + yellow_ratio: 黄色区域占比(0-1之间的浮点数) + """ + # 读取图片 + img = cv2.imread(image_path) + if img is None: + raise ValueError(f"无法读取图片: {image_path}") + + # 获取图片文件名(不带路径和扩展名) + filename = os.path.splitext(os.path.basename(image_path))[0] + + # 转换为HSV色彩空间(更适合颜色分割) + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + # 提取图像的各个通道 + h, s, v = cv2.split(hsv) + + # 黄色在HSV中的范围:色调约为20-30度(OpenCV中为10-30) + # 黄色通常有较高的饱和度和亮度 + yellow_hue_lower = np.array([20, 100, 100]) + yellow_hue_upper = np.array([40, 255, 255]) + + # 创建黄色区域掩码 + yellow_mask = cv2.inRange(hsv, yellow_hue_lower, yellow_hue_upper) + + # 应用形态学操作 + kernel = np.ones((5, 5), np.uint8) + yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_OPEN, kernel) + yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_CLOSE, kernel) + + # 使用连通区域分析,去除小的噪点区域 + num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(yellow_mask, connectivity=8) + + # 过滤小的连通区域 + min_size = 500 # 最小连通区域大小 + filtered_yellow_mask = np.zeros_like(yellow_mask) + + # 从索引1开始,因为0是背景 + for i in range(1, num_labels): + if stats[i, cv2.CC_STAT_AREA] >= min_size: + filtered_yellow_mask[labels == i] = 255 + + # 计算黄色区域占比 + height, width = yellow_mask.shape + total_pixels = height * width + yellow_pixels = np.sum(filtered_yellow_mask == 255) + yellow_ratio = yellow_pixels / total_pixels + + # 在原图上标记黄色区域 + result = img.copy() + overlay = img.copy() + overlay[filtered_yellow_mask > 0] = [0, 165, 255] # 用橙色标记黄色区域 + cv2.addWeighted(overlay, 0.4, img, 0.6, 0, result) # 半透明效果 + + # 显示检测结果信息 + cv2.putText(result, f"Yellow Ratio: {yellow_ratio:.2%}", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 165, 255), 2) + + # 调试模式:显示处理过程图像 + if debug: + plt.figure(figsize=(15, 10)) + + plt.subplot(231) + plt.title("Original Image") + plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) + + plt.subplot(232) + plt.title("Hue Channel") + plt.imshow(h, cmap='hsv') + + plt.subplot(233) + plt.title("Saturation Channel") + plt.imshow(s, cmap='gray') + + plt.subplot(234) + plt.title("Initial Yellow Mask") + plt.imshow(yellow_mask, cmap='gray') + + plt.subplot(235) + plt.title("Filtered Yellow Mask") + plt.imshow(filtered_yellow_mask, cmap='gray') + + plt.subplot(236) + plt.title("Yellow Detection Result") + plt.imshow(cv2.cvtColor(result, cv2.COLOR_BGR2RGB)) + + plt.tight_layout() + plt.show() + + # 保存结果 + if save_result: + result_dir = "results" + os.makedirs(result_dir, exist_ok=True) + output_path = os.path.join(result_dir, f"{filename}_yellow_area_result.jpg") + cv2.imwrite(output_path, result) + print(f"结果已保存至: {output_path}") + + return yellow_ratio + +def main(): + parser = argparse.ArgumentParser(description='分析图片中黄色区域占比') + parser.add_argument('--image_path', default='./image_20250525_090252.png', type=str, help='图片路径') + parser.add_argument('--debug', default=False, action='store_true', help='显示处理过程图像') + parser.add_argument('--save', action='store_true', help='保存处理结果图像') + args = parser.parse_args() + + try: + yellow_ratio = analyze_yellow_area_ratio(args.image_path, args.debug, args.save) + print(f"黄色区域占比: {yellow_ratio:.2%}") + except Exception as e: + print(f"错误: {e}") + +if __name__ == "__main__": + main() \ No newline at end of file