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

base_move/move_base_hori_line.py

@@ -59,7 +59,7 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3, detect_fu
             error("未检测到横向线，无法进行校准", "失败")
             
             # 尝试小幅度摇头寻找横线
-            # TODO 改成上下低头；
+            # TODO 或者改成上下低头?
             if attempts < max_attempts - 1:
                 small_angle = 5 * (1 if attempts % 2 == 0 else -1)
                 info(f"尝试摇头 {small_angle}度 寻找横线", "校准")
@@ -125,12 +125,20 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3, detect_fu
         attempts += 1
         
         # 检查校准结果
-        edge_point_after, edge_info_after = detect_horizontal_track_edge(
+        if detect_func_version == 1:
-            ctrl.image_processor.get_current_image(), 
+            edge_point_after, edge_info_after = detect_horizontal_track_edge(
-            observe=observe, 
+                ctrl.image_processor.get_current_image(), 
-            delay=1000 if observe else 0,
+                observe=observe, 
-            save_log=True
+                delay=1000 if observe else 0,
-        )
+                save_log=True
+            )
+        elif detect_func_version == 2:
+            edge_point_after, edge_info_after = detect_horizontal_track_edge_v2(
+                ctrl.image_processor.get_current_image(), 
+                observe=observe, 
+                delay=1000 if observe else 0,
+                save_log=True
+            )
         
         if edge_info_after and "slope" in edge_info_after:
             current_slope = edge_info_after["slope"]
@@ -437,11 +445,16 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, absolute_deg=200, observe=
 
 def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2, 
                               pass_align=False,
+                              # radius
                               radius=None,
                               min_radius=0.1,
                               max_radius=5,
+                              # mode
+                              detect_func_version=1,
+                              # Qrcode
                               scan_qrcode=False,
                               qr_check_interval=0.3, 
+                              # additional
                               observe=False,
                               bad_big_angle_corret=False,
                               no_end_reset=False,):
@@ -486,7 +499,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2,
     # 1. 对准横线
     if not pass_align:
         section("校准到横向线水平", "校准")
-        aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
+        aligned = align_to_horizontal_line(ctrl, msg, detect_func_version=detect_func_version, observe=observe)
         if not aligned:
             error("无法校准到横向线水平，停止动作", "停止")
             if scan_qrcode:

main.py

@@ -21,6 +21,7 @@ from utils.marker_client import MarkerRunner
 from utils.base_msg import BaseMsg
 
 from task_1.task_1 import run_task_1
+from task_2_5.task_2_5 import run_task_2_5
 from task_5.task_5 import run_task_5
 
 from task_test.task_test import run_task_test
@@ -39,7 +40,9 @@ def main():
         Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat
         
         # time.sleep(100) # TEST 
-        run_task_1(Ctrl, msg)
+        # run_task_1(Ctrl, msg)
+
+        run_task_2_5(Ctrl, msg)
 
         # run_task_5(Ctrl, msg)
 

task_2_5/task_2_5.py

@@ -0,0 +1,51 @@
+import time
+import sys
+import os
+
+# 添加父目录到路径，以便能够导入utils
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from base_move.turn_degree import turn_degree
+from base_move.move_base_hori_line import arc_turn_around_hori_line, align_to_horizontal_line
+from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
+
+observe = True
+
+def run_task_2_5(Ctrl, msg):
+    section('任务2.5：预备进入任务3', "启动")
+
+    # TEST
+    turn_degree(Ctrl, msg, 90, absolute=observe)
+
+    align_to_horizontal_line(
+        Ctrl,
+        msg,
+        detect_func_version=2,
+        observe=observe,
+    )
+
+    return
+
+    section('任务2.5-1：第一次旋转', "移动")
+
+    arc_turn_around_hori_line(
+        Ctrl,
+        msg,
+        angle_deg=-90,
+        target_distance=0.5,
+        detect_func_version=2,
+        observe=observe,
+    )
+
+    section('任务2.5-2：第二次旋转', "移动")
+
+    arc_turn_around_hori_line(
+        Ctrl,
+        msg,
+        angle_deg=90,
+        target_distance=0.5,
+        detect_func_version=2,
+        observe=observe,
+    )
+
+    

test/ros2/rgb-camera/img-raw-get.py

@@ -51,7 +51,7 @@ class ImageSubscriber(Node):
         if self.cv_image is not None:
             # Generate a timestamped filename
             timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
-            filename = f"captured_images/arrow-right/image_{timestamp}.png"
+            filename = f"captured_images/path/image_{timestamp}.png"
             
             cv2.imwrite(filename, self.cv_image)
             self.get_logger().info(f"Saved image as {filename}")

utils/detect_track.py

@@ -372,6 +372,12 @@ def detect_horizontal_track_edge_v2(image, observe=False, delay=1000, save_log=T
         error("无法加载图像", "失败")
         return None, None
     
+    if save_log:
+        timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f")
+        origin_image_path = os.path.join("logs/image", f"origin_horizontal_edge_{timestamp}.jpg")
+        cv2.imwrite(origin_image_path, img)
+        info(f"保存原始图像到: {origin_image_path}", "日志")
+    
     # 获取图像尺寸
     height, width = img.shape[:2]
     
@@ -890,7 +896,7 @@ def detect_horizontal_track_edge_v2(image, observe=False, delay=1000, save_log=T
         result_img = slope_img
     
     # 保存日志图像
-    if save_log and result_img is not None:
+    if save_log:
         timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f")
         log_dir = "logs/image"
         os.makedirs(log_dir, exist_ok=True)
@@ -900,24 +906,25 @@ def detect_horizontal_track_edge_v2(image, observe=False, delay=1000, save_log=T
         cv2.imwrite(origin_image_path, img)
         info(f"保存原始图像到: {origin_image_path}", "日志")
 
-        img_path = os.path.join(log_dir, f"horizontal_edge_{timestamp}.jpg")
+        if result_img is not None:
-        cv2.imwrite(img_path, result_img)
+            img_path = os.path.join(log_dir, f"horizontal_edge_{timestamp}.jpg")
-        info(f"保存横向边缘检测结果图像到: {img_path}", "日志")
+            cv2.imwrite(img_path, result_img)
-        
+            info(f"保存横向边缘检测结果图像到: {img_path}", "日志")
-        # 保存文本日志信息
+            
-        log_info = {
+            # 保存文本日志信息
-            "timestamp": timestamp,
+            log_info = {
-            "edge_point": bottom_edge_point,
+                "timestamp": timestamp,
-            "distance_to_center": distance_to_center,
+                "edge_point": bottom_edge_point,
-            "slope": selected_slope,
+                "distance_to_center": distance_to_center,
-            "distance_to_bottom": distance_to_bottom,
+                "slope": selected_slope,
-            "intersection_point": intersection_point,
+                "distance_to_bottom": distance_to_bottom,
-            "score": selected_score,
+                "intersection_point": intersection_point,
-            "valid": valid_result,
+                "score": selected_score,
-            "reason": reason if not valid_result else "",
+                "valid": valid_result,
-            "is_upper_line": len(upper_horizontal_lines) > 0 and selected_line == upper_horizontal_lines[0][0]
+                "reason": reason if not valid_result else "",
-        }
+                "is_upper_line": len(upper_horizontal_lines) > 0 and selected_line == upper_horizontal_lines[0][0]
-        info(f"横向边缘检测结果: {log_info}", "日志")
+            }
+            info(f"横向边缘检测结果: {log_info}", "日志")
     
     # 如果结果无效，但检测到了一些线，仍然返回结果，不拒绝太靠近底部的线
     if not valid_result and "边缘点y坐标超出有效范围" in reason and bottom_edge_point[1] > height * 0.8: