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

base_move/turn_degree.py

@@ -43,7 +43,7 @@ def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False):
     
     # 定义允许的误差范围（弧度）
     # 如果是精确模式，使用更小的误差阈值
-    limit = 0.03 if precision else 0.04  # 约1.7度或2.3度
+    limit = 0.05
     
     # 计算最短旋转方向和距离
     def circle_dist(target, location):
@@ -94,7 +94,7 @@ def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False):
         debug(f"等待旋转完成: {wait_time:.2f}秒", "时间")
         
         # 精确模式下使用实时监控而不是固定等待时间
-        if precision and abs(dist) > 0.1:  # 对于较大角度
+        if abs(dist) > 0.1:  # 对于较大角度
             start_time = time.time()
             last_yaw = current_yaw
             stable_count = 0

main.py

@@ -22,9 +22,8 @@ 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
+from task_4.task_4 import run_task_4
+from task_test.task_left_line import run_task_test
 
 pass_marker = True
 
@@ -42,11 +41,11 @@ def main():
         # time.sleep(100) # TEST 
         # run_task_1(Ctrl, msg)
 
-        run_task_2_5(Ctrl, msg)
+        # run_task_2_5(Ctrl, msg)
 
-        # run_task_5(Ctrl, msg)
+        # run_task_4(Ctrl, msg)
 
-        # run_task_test(Ctrl, msg)
+        run_task_test(Ctrl, msg)
 
         # time.sleep(100)
 

task_2_5/task_2_5.py

@@ -11,41 +11,36 @@ from utils.log_helper import LogHelper, get_logger, section, info, debug, warnin
 
 observe = True
 
-def run_task_2_5(Ctrl, msg):
+def run_task_2_5(Ctrl, msg, direction='left'):
     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(
+    turn_success, res = arc_turn_around_hori_line(
         Ctrl,
         msg,
-        angle_deg=-90,
-        target_distance=0.5,
+        angle_deg=90 if direction == 'left' else -90,
+        target_distance=0.3,
         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,
-    )
+    turn_degree(Ctrl, msg, degree=90, absolute=True)
+
+    # arc_turn_around_hori_line(
+    #     Ctrl,
+    #     msg,
+    #     angle_deg=70,
+    #     # target_distance=0.5,
+    #     pass_align=True,
+    #     radius=res['radius'],
+    #     detect_func_version=2,
+    #     observe=observe,
+    # )
 
     

task_4/task_4.py

@@ -0,0 +1,28 @@
+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.go_straight import go_straight
+from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
+
+# 创建本模块特定的日志记录器
+logger = get_logger("任务5")
+
+def run_task_4(ctrl, msg):
+    """
+    参数:
+        ctrl: Robot_Ctrl对象
+        msg: 控制消息对象
+        image_processor: 可选的图像处理器实例
+    """
+
+    turn_degree(ctrl, msg, 90, absolute=90)
+
+    time.sleep(100)
+
+    # go_straight(ctrl, msg, distance=6)
+

task_test/task_arrow.py

@@ -5,7 +5,7 @@ import os
 # 添加父目录到路径，以便能够导入utils
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 
-from task_5.detect_arrow_direction import ArrowDetector
+from task_test.detect_arrow_direction import ArrowDetector
 from base_move.turn_degree import turn_degree
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 

task_test/task_left_line.py

@@ -0,0 +1,17 @@
+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 follow_left_side_track
+from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
+
+# 创建本模块特定的日志记录器
+logger = get_logger("任务-test")
+
+def run_task_test(ctrl, msg):
+    # follow_left_side_track(ctrl, msg)
+    time.sleep(100)

task_test/task_test.py

@@ -1,15 +0,0 @@
-import time
-import sys
-import os
-
-# 添加父目录到路径，以便能够导入utils
-sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
-
-from task_5.detect_arrow_direction import ArrowDetector
-from base_move.turn_degree import turn_degree
-
-def run_task_test(ctrl, msg):
-    ctrl.base_msg.stop()
-    turn_degree(ctrl, msg, 90)
-    turn_degree(ctrl, msg, 90)
-    

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/path/image_{timestamp}.png"
+            filename = f"./captured_images/left_path/image_{timestamp}.png"
             
             cv2.imwrite(filename, self.cv_image)
             self.get_logger().info(f"Saved image as {filename}")

utils/detect_track.py

@@ -19,7 +19,7 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True
         edge_point: 赛道前方边缘点的坐标 (x, y)
         edge_info: 边缘信息字典
     """
-    # observe = False  # TEST
+    observe = False  # TEST
     # 如果输入是字符串（文件路径），则加载图像
     if isinstance(image, str):
         img = cv2.imread(image)
@@ -361,7 +361,7 @@ def detect_horizontal_track_edge_v2(image, observe=False, delay=1000, save_log=T
         edge_point: 赛道前方边缘点的坐标 (x, y)
         edge_info: 边缘信息字典
     """
-    # observe = False  # TEST
+    observe = False  # TEST
     # 如果输入是字符串（文件路径），则加载图像
     if isinstance(image, str):
         img = cv2.imread(image)