feat(task_5): 添加箭头方向检测与运动控制

- 初始化箭头检测器并获取图像
- 根据检测到的箭头方向调整运动方向
- 更新运动参数，包括速度和步态
- 添加资源清理逻辑以确保检测器正常关闭

requirements.txt

@@ -0,0 +1,2 @@
+opencv-python>=4.5.0
+numpy>=1.19.0 

task_5/detect_arrow_direction.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import time
+import cv2
+
+# 添加父目录到路径，以便能够导入utils
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from utils.image_raw import ImageProcessor
+from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection
+
+class ArrowDetector:
+    def __init__(self, image_processor=None):
+        """
+        初始化箭头检测器
+        
+        参数:
+            image_processor: 可选的ImageProcessor实例，如果不提供则会创建一个新的
+        """
+        # 如果提供了图像处理器，则使用它，否则创建新的
+        if image_processor is not None:
+            self.image_processor = image_processor
+            self.should_destroy = False  # 不应该在destroy方法中销毁外部传入的实例
+        else:
+            self.image_processor = ImageProcessor()
+            self.image_processor.run()
+            self.should_destroy = True  # 应该在destroy方法中销毁自己创建的实例
+            
+        print("箭头检测器已初始化")
+    
+    def get_arrow_direction(self):
+        """
+        获取当前图像中箭头的方向
+        
+        返回:
+            direction: 字符串，"left"表示左箭头，"right"表示右箭头，"unknown"表示无法确定
+        """
+        # 获取当前图像
+        image = self.image_processor.get_current_image()
+        
+        if image is None:
+            print("警告: 无法获取图像")
+            return "unknown"
+        
+        # 检测箭头方向
+        direction = detect_arrow_direction(image)
+        return direction
+    
+    def visualize_current_detection(self, save_path=None):
+        """
+        可视化当前图像的箭头检测过程
+        
+        参数:
+            save_path: 保存结果图像的路径（可选）
+        """
+        # 获取当前图像
+        image = self.image_processor.get_current_image()
+        
+        if image is None:
+            print("警告: 无法获取图像")
+            return
+        
+        # 可视化箭头检测
+        visualize_arrow_detection(image, save_path)
+    
+    def destroy(self):
+        """
+        清理资源
+        """
+        if self.should_destroy:
+            self.image_processor.destroy()
+        print("箭头检测器已销毁")
+
+def main():
+    """
+    演示箭头检测器的用法
+    """
+    try:
+        # 初始化箭头检测器
+        detector = ArrowDetector()
+        
+        # 等待一段时间，确保图像已经接收
+        print("等待接收图像...")
+        time.sleep(3)
+        
+        # 持续检测箭头方向
+        for i in range(10):  # 检测10次
+            direction = detector.get_arrow_direction()
+            print(f"检测到的箭头方向 ({i+1}/10): {direction}")
+            
+            # 可选: 保存第一次检测的可视化结果
+            if i == 0:
+                detector.visualize_current_detection("arrow_detection_result.jpg")
+            
+            time.sleep(1)  # 每秒检测一次
+    
+    except KeyboardInterrupt:
+        print("\n程序被用户中断")
+    except Exception as e:
+        print(f"发生错误: {e}")
+    finally:
+        # 清理资源
+        print("正在清理资源...")
+        if 'detector' in locals():
+            detector.destroy()
+        print("程序已退出")
+
+if __name__ == "__main__":
+    main() 

task_5/task_5.py

@@ -1,52 +1,74 @@
 import time
+import sys
+import os
 
-def run_task_5(ctrl, msg):
-    # DEBUG
-    # msg.mode = 11  # 运动模式
-    # msg.gait_id = 26
-    # msg.vel_des = [0, 0, 2]  # 期望速度
-    # msg.duration = 0  # 零时长表示持续运动，直到接收到新命令
-    # msg.step_height = [0.02, 0.02]  # 持续运动时摆动腿的离地高度
-    # msg.life_count += 1
-    # ctrl.Send_cmd(msg)
-    # time.sleep(1.1)  # 持续5秒钟
+# 添加父目录到路径，以便能够导入utils
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 
-    # msg.mode = 11
-    # msg.gait_id = 3
-    # msg.vel_des = [1, 0, 0]
-    # msg.duration = 1000
-    # msg.step_height = [0.03, 0.03]
-    # msg.life_count += 1
-    # print(msg.pos_des)
-    # ctrl.Send_cmd(msg)
-    # ctrl.Wait_finish(11, msg.gait_id)
+from task_5.detect_arrow_direction import ArrowDetector
 
-    # 设置俯身姿态
-    msg.mode = 3  # 姿态控制模式
-    msg.gait_id = 0
-    msg.pos_des = [0, 0, 0.15]  # 设置较低的姿态高度
-    msg.life_count += 1
-    ctrl.Send_cmd(msg)
-    ctrl.Wait_finish(3, msg.gait_id)
+def run_task_5(ctrl, msg, image_processor=None):
+    # 初始化箭头检测器
+    detector = ArrowDetector() if image_processor is None else ArrowDetector(image_processor)
     
-    # 等待姿态稳定
-    time.sleep(1.0)
+    try:
+        # 设置俯身姿态
+        msg.mode = 3  # 姿态控制模式
+        msg.gait_id = 0
+        msg.pos_des = [0, 0, 0.15]  # 设置较低的姿态高度
+        msg.life_count += 1
+        ctrl.Send_cmd(msg)
+        ctrl.Wait_finish(3, msg.gait_id)
+        
+        # 等待姿态稳定
+        time.sleep(1.0)
+        
+        # 等待获取图像和检测箭头方向
+        print("正在检测箭头方向...")
+        time.sleep(2.0)  # 给检测器一些时间来获取图像
+        
+        # 检测箭头方向
+        direction = detector.get_arrow_direction()
+        print(f"检测到的箭头方向: {direction}")
+        
+        # 保存检测结果的可视化图像
+        detector.visualize_current_detection("arrow_detection_result.jpg")
+        
+        # 根据箭头方向决定移动方向
+        vel_x = 0.5  # 前进速度
+        vel_y = 0.0  # 初始侧向速度为0
+        
+        if direction == "left":
+            # 如果是左箭头，向左移动
+            vel_y = 0.3  # 向左移动的速度
+            print("根据箭头方向，向左移动")
+        elif direction == "right":
+            # 如果是右箭头，向右移动
+            vel_y = -0.3  # 向右移动的速度
+            print("根据箭头方向，向右移动")
+        else:
+            # 如果无法确定方向，直接前进
+            print("无法确定箭头方向，直接前进")
+        
+        # 开始运动
+        msg.mode = 11  # 运动控制模式
+        msg.gait_id = 3  # 使用 trot 步态
+        msg.vel_des = [vel_x, vel_y, 0]  # 设置移动速度，x和y方向
+        msg.duration = 3000  # 运动持续3秒
+        msg.step_height = [0.03, 0.03]  # 设置步高
+        msg.life_count += 1
+        ctrl.Send_cmd(msg)
+        ctrl.Wait_finish(11, msg.gait_id)
+        
+        # 恢复站立姿态
+        msg.mode = 3
+        msg.gait_id = 0
+        msg.pos_des = [0, 0, 0.25]  # 恢复到正常站立高度
+        msg.life_count += 1
+        ctrl.Send_cmd(msg)
+        ctrl.Wait_finish(3, msg.gait_id)
     
-    # 开始前进运动
-    msg.mode = 11  # 运动控制模式
-    msg.gait_id = 3  # 使用 trot 步态
-    msg.vel_des = [0.5, 0, 0]  # 设置前进速度，x方向0.5m/s
-    msg.duration = 2000  # 运动持续2秒
-    msg.step_height = [0.03, 0.03]  # 设置步高
-    msg.life_count += 1
-    ctrl.Send_cmd(msg)
-    ctrl.Wait_finish(11, msg.gait_id)
-    
-    # 恢复站立姿态
-    msg.mode = 3
-    msg.gait_id = 0
-    msg.pos_des = [0, 0, 0.25]  # 恢复到正常站立高度
-    msg.life_count += 1
-    ctrl.Send_cmd(msg)
-    ctrl.Wait_finish(3, msg.gait_id)
+    finally:
+        # 清理资源
+        detector.destroy()
 

test/task-arrow/README_arrow_detection.md

@@ -0,0 +1,101 @@
+# 箭头方向检测功能
+
+本模块提供了从图像中检测绿色箭头指向方向的功能。可以集成到机器人控制系统中，用于根据视觉信息引导机器人运动。
+
+## 功能特性
+
+- 从图像中提取绿色箭头
+- 判断箭头指向方向（左或右）
+- 可视化检测过程
+- 与ROS图像订阅集成
+- 可作为独立工具使用
+
+## 文件结构
+
+- `utils/decode_arrow.py` - 核心箭头检测算法
+- `task_5/detect_arrow_direction.py` - 与ImageProcessor集成的箭头检测器
+- `test/test_arrow.py` - 命令行测试工具
+- `test/test_arrow_with_image.py` - 带参数的图像测试工具
+
+## 使用方法
+
+### 1. 独立测试箭头检测
+
+```bash
+# 基本用法
+python test/test_arrow.py path/to/image.png
+
+# 高级用法
+python test/test_arrow_with_image.py path/to/image.png --save result.jpg --show --debug
+```
+
+### 2. 在机器人任务中使用
+
+箭头检测功能已集成到`task_5.py`中。机器人会根据检测到的箭头方向（左或右）来调整其移动方向。
+
+```python
+from task_5.task_5 import run_task_5
+
+# 机器人控制代码
+run_task_5(ctrl, msg, image_processor)  # image_processor可选
+```
+
+### 3. 作为独立模块使用
+
+```python
+from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection
+
+# 检测箭头方向
+image = cv2.imread("path/to/image.jpg")
+direction = detect_arrow_direction(image)
+print(f"检测到的箭头方向: {direction}")
+
+# 可视化检测过程
+visualize_arrow_detection(image, "result.jpg")
+```
+
+或者使用集成的ArrowDetector类：
+
+```python
+from task_5.detect_arrow_direction import ArrowDetector
+
+# 初始化检测器
+detector = ArrowDetector()
+
+# 获取箭头方向
+direction = detector.get_arrow_direction()
+print(f"检测到的箭头方向: {direction}")
+
+# 可视化并保存结果
+detector.visualize_current_detection("result.jpg")
+
+# 清理资源
+detector.destroy()
+```
+
+## 算法原理
+
+1. 将图像转换为HSV颜色空间
+2. 使用颜色阈值提取绿色区域
+3. 查找绿色区域的轮廓
+4. 分析轮廓的几何特性，确定箭头方向
+5. 计算轮廓的左右区域像素密度，判断指向方向
+
+## 参数调整
+
+如果检测效果不理想，可以调整以下参数：
+
+1. `utils/decode_arrow.py`中的绿色HSV阈值：
+   ```python
+   lower_green = np.array([40, 50, 50])  # 绿色的HSV下限
+   upper_green = np.array([80, 255, 255])  # 绿色的HSV上限
+   ```
+
+2. 根据实际情况调整像素密度比较的逻辑。
+
+## 注意事项
+
+- 确保图像中的箭头颜色为绿色
+- 图像中应该只有一个明显的箭头
+- 光照条件会影响绿色提取的效果
+- 如果检测结果不准确，可能需要调整HSV阈值

test/task-arrow/test_arrow.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+
+import cv2
+import sys
+import os
+
+# 添加父目录到路径，以便能够导入utils
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection
+
+def main():
+    # 检查命令行参数
+    if len(sys.argv) < 2:
+        print("使用方法: python test_arrow.py <图像路径>")
+        sys.exit(1)
+    
+    # 获取图像路径
+    image_path = sys.argv[1]
+    
+    # 检查文件是否存在
+    if not os.path.exists(image_path):
+        print(f"错误: 文件 '{image_path}' 不存在")
+        sys.exit(1)
+    
+    print(f"正在处理图像: {image_path}")
+    
+    # 检测箭头方向
+    direction = detect_arrow_direction(image_path)
+    print(f"检测到的箭头方向: {direction}")
+    
+    # 可视化检测过程
+    visualize_arrow_detection(image_path)
+
+if __name__ == "__main__":
+    main() 

test/task-arrow/test_arrow_with_image.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import cv2
+import numpy as np
+import argparse
+
+# 添加父目录到路径，以便能够导入utils
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection
+
+def main():
+    # 创建参数解析器
+    parser = argparse.ArgumentParser(description='测试箭头方向检测')
+    parser.add_argument('image_path', help='图像文件路径')
+    parser.add_argument('--save', help='保存可视化结果的路径', default=None)
+    parser.add_argument('--show', help='显示可视化结果', action='store_true')
+    parser.add_argument('--debug', help='输出详细的调试信息', action='store_true')
+    
+    args = parser.parse_args()
+    
+    # 检查文件是否存在
+    if not os.path.exists(args.image_path):
+        print(f"错误: 文件 '{args.image_path}' 不存在")
+        sys.exit(1)
+    
+    print(f"正在处理图像: {args.image_path}")
+    
+    # 加载图像
+    img = cv2.imread(args.image_path)
+    if img is None:
+        print(f"错误: 无法加载图像 '{args.image_path}'")
+        sys.exit(1)
+    
+    # 如果需要，显示原始图像
+    if args.debug:
+        cv2.imshow('Original Image', img)
+        cv2.waitKey(0)
+    
+    # 检测箭头方向
+    direction = detect_arrow_direction(img)
+    print(f"检测到的箭头方向: {direction}")
+    
+    # 如果需要，显示可视化结果
+    if args.show or args.save:
+        visualize_arrow_detection(img, args.save)
+    
+    # 如果不需要显示可视化结果但需要保存
+    if args.save and not args.show:
+        print(f"可视化结果已保存到: {args.save}")
+    
+    return direction
+
+if __name__ == "__main__":
+    main() 

test_right_arrow.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import cv2
+import argparse
+
+# 添加工作目录到路径
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+
+from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection
+
+def main():
+    # 创建参数解析器
+    parser = argparse.ArgumentParser(description='箭头方向检测测试')
+    parser.add_argument('--image', default="image_20250511_121219.png", 
+                        help='图像文件路径 (默认: image_20250511_121219.png)')
+    parser.add_argument('--save', default="arrow_detection_result.jpg",
+                        help='保存可视化结果的路径 (默认: arrow_detection_result.jpg)')
+    parser.add_argument('--show', action='store_true',
+                        help='显示可视化结果')
+    
+    args = parser.parse_args()
+    
+    # 获取图像路径
+    image_path = args.image
+    
+    # 检查文件是否存在
+    if not os.path.exists(image_path):
+        print(f"错误: 文件 '{image_path}' 不存在")
+        sys.exit(1)
+    
+    print(f"正在处理图像: {image_path}")
+    
+    # 加载图像
+    img = cv2.imread(image_path)
+    if img is None:
+        print(f"错误: 无法加载图像 '{image_path}'")
+        sys.exit(1)
+    
+    # 检测箭头方向
+    direction = detect_arrow_direction(img)
+    print(f"检测到的箭头方向: {direction}")
+    
+    # 可视化检测过程并保存结果
+    visualize_arrow_detection(img, args.save)
+    
+    print(f"可视化结果已保存到: {args.save}")
+    
+    # 如果需要显示结果，等待用户按键
+    if args.show:
+        print("按任意键退出...")
+        cv2.waitKey(0)
+        cv2.destroyAllWindows()
+    
+    return direction
+
+if __name__ == "__main__":
+    main()

utils/decode_arrow.py

@@ -0,0 +1,180 @@
+import cv2
+import numpy as np
+
+def detect_arrow_direction(image):
+    """
+    从图像中提取绿色箭头并判断其指向方向（左或右）
+    
+    参数:
+        image: 输入图像，可以是文件路径或者已加载的图像数组
+        
+    返回:
+        direction: 字符串，"left"表示左箭头，"right"表示右箭头，"unknown"表示无法确定
+    """
+    # 如果输入是字符串（文件路径），则加载图像
+    if isinstance(image, str):
+        img = cv2.imread(image)
+    else:
+        img = image.copy()
+    
+    if img is None:
+        print("无法加载图像")
+        return "unknown"
+    
+    # 转换到HSV颜色空间以便更容易提取绿色
+    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+    
+    # 绿色的HSV范围
+    # 调整这些值以匹配图像中绿色的具体色调··
+    lower_green = np.array([40, 50, 50])
+    upper_green = np.array([80, 255, 255])
+    
+    # 创建绿色的掩码
+    mask = cv2.inRange(hsv, lower_green, upper_green)
+    
+    # 应用掩码，只保留绿色部分
+    green_only = cv2.bitwise_and(img, img, mask=mask)
+    
+    # 将掩码转为灰度图
+    gray = mask.copy()
+    
+    # 查找轮廓
+    contours, _ = cv2.findContours(gray, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    
+    # 如果没有找到轮廓，返回未知
+    if not contours:
+        return "unknown"
+    
+    # 找到最大的轮廓（假设是箭头）
+    max_contour = max(contours, key=cv2.contourArea)
+    
+    # 获取轮廓的最小外接矩形
+    x, y, w, h = cv2.boundingRect(max_contour)
+    
+    # 计算轮廓的矩，用于确定箭头方向
+    M = cv2.moments(max_contour)
+    
+    # 避免除以零
+    if M["m00"] != 0:
+        cx = int(M["m10"] / M["m00"])
+        cy = int(M["m01"] / M["m00"])
+    else:
+        cx, cy = x + w//2, y + h//2
+    
+    # 将图像分为左右两部分
+    left_region = gray[y:y+h, x:cx]
+    right_region = gray[y:y+h, cx:x+w]
+    
+    # 计算每个区域中白色像素的数量（箭头部分）
+    left_pixels = cv2.countNonZero(left_region)
+    right_pixels = cv2.countNonZero(right_region)
+    
+    # 计算每个区域中白色像素的密度
+    left_density = left_pixels / (left_region.size + 1e-10)
+    right_density = right_pixels / (right_region.size + 1e-10)
+    
+    # 根据左右区域的像素密度确定箭头方向
+    # 如果箭头指向右侧，右侧区域的箭头头部应该有更多的像素密度
+    # 如果箭头指向左侧，左侧区域的箭头头部应该有更多的像素密度
+    if right_density > left_density:
+        return "right"
+    else:
+        return "left"
+
+def visualize_arrow_detection(image, save_path=None):
+    """
+    可视化箭头检测过程，显示中间结果
+    
+    参数:
+        image: 输入图像，可以是文件路径或者已加载的图像数组
+        save_path: 保存结果图像的路径（可选）
+    """
+    # 如果输入是字符串（文件路径），则加载图像
+    if isinstance(image, str):
+        img = cv2.imread(image)
+    else:
+        img = image.copy()
+    
+    if img is None:
+        print("无法加载图像")
+        return
+    
+    # 转换到HSV颜色空间
+    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+    
+    # 绿色的HSV范围
+    lower_green = np.array([40, 50, 50])
+    upper_green = np.array([80, 255, 255])
+    
+    # 创建绿色的掩码
+    mask = cv2.inRange(hsv, lower_green, upper_green)
+    
+    # 应用掩码，只保留绿色部分
+    green_only = cv2.bitwise_and(img, img, mask=mask)
+    
+    # 查找轮廓
+    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    
+    # 创建输出图像
+    output = img.copy()
+    
+    # 如果找到轮廓，绘制最大轮廓
+    if contours:
+        max_contour = max(contours, key=cv2.contourArea)
+        cv2.drawContours(output, [max_contour], -1, (0, 0, 255), 2)
+        
+        # 获取轮廓的最小外接矩形
+        x, y, w, h = cv2.boundingRect(max_contour)
+        cv2.rectangle(output, (x, y), (x + w, y + h), (255, 0, 0), 2)
+        
+        # 计算轮廓的矩
+        M = cv2.moments(max_contour)
+        
+        # 避免除以零
+        if M["m00"] != 0:
+            cx = int(M["m10"] / M["m00"])
+            cy = int(M["m01"] / M["m00"])
+            
+            # 绘制中心点
+            cv2.circle(output, (cx, cy), 5, (255, 0, 0), -1)
+            
+            # 绘制分割线
+            cv2.line(output, (cx, y), (cx, y + h), (0, 255, 255), 2)
+    
+    # 获取箭头方向
+    direction = detect_arrow_direction(img)
+    
+    # 在图像上添加方向文本
+    cv2.putText(output, f"Direction: {direction}", (10, 30), 
+                cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
+    
+    # 如果提供了保存路径，保存结果图像
+    if save_path:
+        cv2.imwrite(save_path, output)
+    
+    # 创建一个包含所有图像的窗口
+    result = np.hstack((img, green_only, output))
+    
+    # 调整大小以便查看
+    scale_percent = 50  # 缩放到原来的50%
+    width = int(result.shape[1] * scale_percent / 100)
+    height = int(result.shape[0] * scale_percent / 100)
+    dim = (width, height)
+    resized = cv2.resize(result, dim, interpolation=cv2.INTER_AREA)
+    
+    # 显示结果
+    cv2.imshow('Arrow Detection Process', resized)
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+# 用法示例
+if __name__ == "__main__":
+    # 替换为实际图像路径
+    image_path = "path/to/arrow/image.png"
+    
+    # 检测箭头方向
+    direction = detect_arrow_direction(image_path)
+    print(f"检测到的箭头方向: {direction}")
+    
+    # 可视化检测过程
+    visualize_arrow_detection(image_path)