Add go_until_yellow_area function to task_3.py for yellow area detection and control. Enhanced robot movement logic to stop upon detecting specified yellow area ratio, including temporary image handling and smooth stopping mechanism.

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("完成原地站立", "站立")
     

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()