Merge branch 'main-v2' of ssh://120.27.199.238:222/Havoc420mac/mi-task into main-v2

main.py

@@ -60,42 +60,27 @@ def main():
     msg = robot_control_cmd_lcmt()
     print('2')
     try:
-        # TEST
-        # print('yuyin')
-        # cv_image = Ctrl.image_processor.get_current_image('ai')
-        # print('111')
-        # cv2.imwrite(f"saved_images/firstai.jpg", cv_image)
-        # print('try out')
-        # speak('nihao')
-
-        # TEST
-        # go_straight_with_enhanced_calibration(Ctrl, msg, distance = 5, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21])
-        # pass_up_down(Ctrl, msg)
-        # pass_bar(Ctrl, msg)
-        # run_task_2(Ctrl, msg)
-
         # INFO Real Task
         # TAG task - 0
         info("Recovery stand", "info")
         Ctrl.base_msg.stand_up()
         Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat # INFO 但是正式比赛或许也有用
         
-        # image = Ctrl.image_processor.get_current_image('ai')
-        # timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")
-        # filename = f"saved_images/rgb_{timestamp}.jpg"
-        # cv2.imwrite(filename, image)
-
         if TASK == TaskType.PASS_BAR:
             from task_4.pass_bar import pass_bar
             # TEST #1: pass-bar
             pass_bar(Ctrl, msg)
         elif TASK == TaskType.YELLOW_LIGHT: # TODO image
             from task_3.task_3 import go_until_yellow_area
+<<<<<<< HEAD
             # turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)
             go_until_yellow_area(Ctrl, msg, yellow_ratio_threshold=0.012, speed=0.3, max_time=30, observe=True)
+=======
+            go_until_yellow_area(Ctrl, msg, yellow_ratio_threshold=0.05)
+>>>>>>> 00b77fe2476f74be45267ef52a45138e41114e7b
         elif TASK == TaskType.RED_BAR:
             from task_4.task_4 import go_straight_until_red_bar
-            go_straight_until_red_bar(Ctrl, msg)
+            go_straight_until_red_bar(Ctrl, msg, red_ratio_threshold=0.18, step_distance=0.3)
         elif TASK == TaskType.UP_AND_DOWN:
             from task_3.task_3 import go_straight_with_enhanced_calibration
             go_straight_with_enhanced_calibration(Ctrl, msg, distance = 5, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21])
@@ -108,16 +93,18 @@ def main():
         elif TASK == TaskType.CENTER_ON_DUAL_TRACKS:
             from base_move.center_on_dual_tracks import center_on_dual_tracks
             center_on_dual_tracks(Ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3)
+<<<<<<< HEAD
         else:
             pass
         
+=======
+>>>>>>> 00b77fe2476f74be45267ef52a45138e41114e7b
         if TASK != TaskType.TASK:
             # 如果不是 task 类型，直接返回
             pass
         
         # TAG task - 1
         # run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)
-       
         
         # TAG task - 2
         # arrow_direction='left'
@@ -126,8 +113,12 @@ def main():
         # arrow_direction='left'
         # print('🏹 arrow_direction: ', arrow_direction)
 
+<<<<<<< HEAD
         
         # arrow_direction = 'right'
+=======
+
+>>>>>>> 00b77fe2476f74be45267ef52a45138e41114e7b
         # if(arrow_direction=='left'): speak("左侧路线")
         # else: speak("右侧路线")
 

task_3/task_3.py

@@ -27,7 +27,7 @@ from utils.speech_demo import speak
 logger = get_logger("任务3")
 
 observe = False
-YELLOW_RATIO_THRESHOLD = 0.03  # TODO 黄色区域比例阈值
+YELLOW_RATIO_THRESHOLD = 0.04  # TODO 黄色区域比例阈值
 
 def run_task_3(ctrl, msg, time_sleep=5000):
     section('任务3：上下坡', "启动")

task_4/task_4.py

@@ -87,7 +87,7 @@ def run_task_4_back(ctrl, msg):
 
 def go_straight_until_red_bar(ctrl, msg,
                                     red_ratio_threshold=0.2,
-                                    step_distance=0.5,
+                                    step_distance=0.3,
                                     max_distance=5,
                                     speed=0.3
                                     ):

test/text-image/README.md

@@ -0,0 +1,49 @@
+# 数字识别程序
+
+这是一个使用OpenCV识别图片中数字1和2的Python程序。
+
+## 功能特点
+
+- 使用OpenCV进行图像预处理和特征提取
+- 基于轮廓检测和特征分析的数字分类
+- 能够区分数字1和2的形态特征
+- 自动保存识别结果图片
+
+## 安装依赖
+
+```bash
+pip install -r requirements.txt
+```
+
+## 使用方法
+
+1. 将需要识别的图片放在`imgs/`文件夹中
+2. 运行程序：
+   ```bash
+   python text_image.py
+   ```
+
+## 程序流程
+
+1. **图像预处理**：转换为灰度图、高斯模糊去噪、自适应阈值二值化、形态学操作
+2. **轮廓检测**：查找图像中的轮廓，筛选可能的数字区域
+3. **特征提取**：提取ROI的像素密度、水平和垂直投影等特征
+4. **数字分类**：基于特征分析判断是数字1还是2
+5. **结果输出**：在原图上标注识别结果并保存
+
+## 识别原理
+
+- **数字1**：通常较细，垂直投影集中在中间，方差较小
+- **数字2**：通常较宽，有弯曲部分，垂直投影方差较大
+
+## 输出文件
+
+程序会在`imgs/`文件夹中生成：
+- `result_1.jpg`：第一张图片的识别结果
+- `result_2.jpg`：第二张图片的识别结果
+
+## 注意事项
+
+- 确保图片清晰，数字对比度足够
+- 程序会自动过滤太小的轮廓
+- 识别结果包含位置信息和置信度

test/text-image/requirements.txt

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

test/text-image/text_image.py

@@ -0,0 +1,180 @@
+import cv2
+import numpy as np
+import os
+from pathlib import Path
+
+def preprocess_image(image):
+    """
+    预处理图像：转换为HSV色彩空间，检测白色区域
+    """
+    # 转换为HSV色彩空间
+    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+    
+    # 定义白色的HSV范围 - 放宽范围
+    # 白色在HSV中：饱和度低，明度高
+    # 原来的范围：lower_white = np.array([0, 0, 200]), upper_white = np.array([180, 30, 255])
+    # 放宽后的范围：
+    lower_white = np.array([0, 0, 150])    # 降低明度下限，从200降到150
+    upper_white = np.array([180, 60, 255]) # 提高饱和度上限，从30提高到60
+    
+    # 创建白色掩码
+    white_mask = cv2.inRange(hsv, lower_white, upper_white)
+    
+    # 形态学操作：开运算去除小噪点，闭运算填充小孔
+    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
+    cleaned = cv2.morphologyEx(white_mask, cv2.MORPH_OPEN, kernel)
+    cleaned = cv2.morphologyEx(cleaned, cv2.MORPH_CLOSE, kernel)
+    
+    return cleaned
+
+def find_white_rectangles(binary_image):
+    """
+    查找白色矩形区域
+    """
+    contours, _ = cv2.findContours(
+        binary_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
+    )
+    
+    # 筛选合适的矩形轮廓
+    valid_rectangles = []
+    for contour in contours:
+        area = cv2.contourArea(contour)
+        if area > 200:  # 过滤太小的区域，从500降低到200
+            # 计算轮廓的边界矩形
+            x, y, w, h = cv2.boundingRect(contour)
+            
+            # 计算轮廓的近似多边形
+            epsilon = 0.02 * cv2.arcLength(contour, True)
+            approx = cv2.approxPolyDP(contour, epsilon, True)
+            
+            # 检查是否为四边形（矩形）
+            if len(approx) == 4:
+                # 计算宽高比
+                aspect_ratio = w / h
+                # 矩形应该有合理的宽高比（不是太细长）
+                if 0.3 < aspect_ratio < 3.0:
+                    valid_rectangles.append((contour, (x, y, w, h), approx))
+    
+    return valid_rectangles
+
+def analyze_white_region(roi, original_roi):
+    """
+    分析白色区域的特征
+    """
+    # 计算白色像素比例
+    total_pixels = roi.shape[0] * roi.shape[1]
+    white_pixels = np.sum(roi == 255)
+    white_ratio = white_pixels / total_pixels
+    
+    # 计算区域的形状特征
+    height, width = roi.shape
+    aspect_ratio = width / height
+    
+    # 计算边缘强度（白色区域应该有清晰的边缘）
+    edges = cv2.Canny(original_roi, 50, 150)
+    edge_density = np.sum(edges > 0) / total_pixels
+    
+    return {
+        'white_ratio': white_ratio,
+        'aspect_ratio': aspect_ratio,
+        'edge_density': edge_density,
+        'area': total_pixels
+    }
+
+def detect_white_rectangles_in_image(image_path):
+    """
+    检测图片中的白色矩形区域
+    """
+    # 读取图像
+    image = cv2.imread(image_path)
+    if image is None:
+        print(f"无法读取图像: {image_path}")
+        return []
+    
+    # 预处理
+    white_mask = preprocess_image(image)
+    
+    # 查找白色矩形
+    valid_rectangles = find_white_rectangles(white_mask)
+    
+    results = []
+    
+    for contour, (x, y, w, h), approx in valid_rectangles:
+        # 提取ROI
+        roi_mask = white_mask[y:y+h, x:x+w]
+        roi_original = image[y:y+h, x:x+w]
+        
+        # 分析特征
+        features = analyze_white_region(roi_mask, roi_original)
+        
+        # 判断是否为有效的白色矩形
+        is_valid_white_rectangle = (
+            features['white_ratio'] > 0.4 and  # 白色像素比例要求降低，从0.6降到0.4
+            features['edge_density'] > 0.005    # 边缘密度要求降低，从0.01降到0.005
+        )
+        
+        if is_valid_white_rectangle:
+            results.append({
+                'type': 'white_rectangle',
+                'position': (x, y, w, h),
+                'features': features,
+                'confidence': 'high' if features['white_ratio'] > 0.8 else 'medium'
+            })
+            
+            # 在图像上绘制结果
+            cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 0), 2)
+            cv2.putText(image, f"白色矩形", (x, y - 10), 
+                       cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
+            
+            # 绘制轮廓点
+            cv2.drawContours(image, [approx], -1, (255, 0, 0), 2)
+    
+    return image, results
+
+def main():
+    """
+    主函数：测试白色矩形检测
+    """
+    # 图片路径
+    img_dir = Path("imgs")
+    img1_path = img_dir / "1.jpg"
+    img2_path = img_dir / "2.jpg"
+    
+    print("开始检测图片中的白色矩形区域...")
+    
+    # 检测第一张图片
+    if img1_path.exists():
+        print(f"\n检测图片: {img1_path}")
+        result_img1, results1 = detect_white_rectangles_in_image(str(img1_path))
+        
+        print("检测结果:")
+        for result in results1:
+            print(f"  类型: {result['type']}, 位置: {result['position']}, 置信度: {result['confidence']}")
+            print(f"  特征: 白色比例={result['features']['white_ratio']:.2f}, "
+                  f"宽高比={result['features']['aspect_ratio']:.2f}")
+        
+        # 保存结果图片
+        output_path1 = img_dir / "result_1.jpg"
+        cv2.imwrite(str(output_path1), result_img1)
+        print(f"结果图片已保存到: {output_path1}")
+    
+    # 检测第二张图片
+    if img2_path.exists():
+        print(f"\n检测图片: {img2_path}")
+        result_img2, results2 = detect_white_rectangles_in_image(str(img2_path))
+        
+        print("检测结果:")
+        for result in results2:
+            print(f"  类型: {result['type']}, 位置: {result['position']}, 置信度: {result['confidence']}")
+            print(f"  特征: 白色比例={result['features']['white_ratio']:.2f}, "
+                  f"宽高比={result['features']['aspect_ratio']:.2f}")
+        
+        # 保存结果图片
+        output_path2 = img_dir / "result_2.jpg"
+        cv2.imwrite(str(output_path2), result_img2)
+        print(f"结果图片已保存到: {output_path2}")
+    
+    print("\n检测完成！")
+
+if __name__ == "__main__":
+    main()

test/text_image.py

@@ -0,0 +1,3 @@
+
+
+

utils/shoushi_demo.py

@@ -118,58 +118,6 @@ class GestureControlNode(Node):
         elif msg.id == 7:  # 停止手势
             self.get_logger().info("检测到停止手势")
 
-    def complete_loading(self):
-        """完成配货操作"""
-        self.get_logger().info("配货完成，开始运输")
-        
-        # 调用开始运输服务
-        if self.start_transport_cli.wait_for_service(timeout_sec=1.0):
-            req = Trigger.Request()
-            future = self.start_transport_cli.call_async(req)
-            future.add_done_callback(self.transport_start_callback)
-        else:
-            self.get_logger().warn("开始运输服务不可用")
-        
-        # 更新状态
-        self.current_state = "transporting"
-
-    def transport_start_callback(self, future):
-        """运输开始服务回调"""
-        try:
-            response = future.result()
-            if response.success:
-                self.get_logger().info("运输已开始")
-            else:
-                self.get_logger().warn("运输启动失败")
-        except Exception as e:
-            self.get_logger().error(f"运输服务调用失败: {e}")
-
-    def complete_unloading(self):
-        """完成卸货操作"""
-        self.get_logger().info("卸货完成，准备新的配货")
-        
-        # 调用完成卸货服务
-        if self.complete_unloading_cli.wait_for_service(timeout_sec=1.0):
-            req = Trigger.Request()
-            future = self.complete_unloading_cli.call_async(req)
-            future.add_done_callback(self.unloading_complete_callback)
-        else:
-            self.get_logger().warn("完成卸货服务不可用")
-        
-        # 更新状态
-        self.current_state = "waiting_for_loading"
-
-    def unloading_complete_callback(self, future):
-        """卸货完成服务回调"""
-        try:
-            response = future.result()
-            if response.success:
-                self.get_logger().info("卸货已完成确认")
-            else:
-                self.get_logger().warn("卸货完成确认失败")
-        except Exception as e:
-            self.get_logger().error(f"卸货完成服务调用失败: {e}")
-
     def timer_callback(self):
         """定时器回调，用于状态检查和超时处理"""
         # 检查手势识别是否激活且超时
@@ -181,7 +129,6 @@ class GestureControlNode(Node):
         # 这里可以添加状态机逻辑，根据实际需求更新current_state
         # 例如，当机器狗到达配货区域时，设置self.current_state = "loading"
         # 当机器狗到达卸货区域时，设置self.current_state = "unloading"
-
     def update_state(self, new_state):
         """更新机器狗状态"""
         old_state = self.current_state