diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..bc6ab22 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,2 @@ +opencv-python>=4.5.0 +numpy>=1.19.0 \ No newline at end of file diff --git a/res/arrows/left/image-1.png b/res/arrows/left/image-1.png new file mode 100644 index 0000000..d663aa4 Binary files /dev/null and b/res/arrows/left/image-1.png differ diff --git a/res/arrows/left/image-2.png b/res/arrows/left/image-2.png new file mode 100644 index 0000000..b2d5f0b Binary files /dev/null and b/res/arrows/left/image-2.png differ diff --git a/res/arrows/left/image-3.png b/res/arrows/left/image-3.png new file mode 100644 index 0000000..283c0d2 Binary files /dev/null and b/res/arrows/left/image-3.png differ diff --git a/res/arrows/left/image-4.png b/res/arrows/left/image-4.png new file mode 100644 index 0000000..ada5316 Binary files /dev/null and b/res/arrows/left/image-4.png differ diff --git a/res/arrows/left/image-5.png b/res/arrows/left/image-5.png new file mode 100644 index 0000000..7c32305 Binary files /dev/null and b/res/arrows/left/image-5.png differ diff --git a/res/arrows/right/image-1.png b/res/arrows/right/image-1.png new file mode 100644 index 0000000..95fc9af Binary files /dev/null and b/res/arrows/right/image-1.png differ diff --git a/res/arrows/right/image-2.png b/res/arrows/right/image-2.png new file mode 100644 index 0000000..d8485ff Binary files /dev/null and b/res/arrows/right/image-2.png differ diff --git a/res/arrows/right/image-3.png b/res/arrows/right/image-3.png new file mode 100644 index 0000000..c596a06 Binary files /dev/null and b/res/arrows/right/image-3.png differ diff --git a/task_5/detect_arrow_direction.py b/task_5/detect_arrow_direction.py new file mode 100644 index 0000000..72dabea --- /dev/null +++ b/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() \ No newline at end of file diff --git a/task_5/task_5.py b/task_5/task_5.py index e37cd81..e246a8f 100644 --- a/task_5/task_5.py +++ b/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() diff --git a/test/task-arrow/README_arrow_detection.md b/test/task-arrow/README_arrow_detection.md new file mode 100644 index 0000000..4fc7e27 --- /dev/null +++ b/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阈值 \ No newline at end of file diff --git a/test/task-arrow/test_arrow.py b/test/task-arrow/test_arrow.py new file mode 100644 index 0000000..88286ba --- /dev/null +++ b/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() \ No newline at end of file diff --git a/test/task-arrow/test_arrow_with_image.py b/test/task-arrow/test_arrow_with_image.py new file mode 100644 index 0000000..cdc97ac --- /dev/null +++ b/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() \ No newline at end of file diff --git a/test_right_arrow.py b/test_right_arrow.py new file mode 100644 index 0000000..0df9ee4 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/utils/decode_arrow.py b/utils/decode_arrow.py new file mode 100644 index 0000000..e840478 --- /dev/null +++ b/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)