Refactor main execution flow by adding support for task 3 back functionality and adjusting task 1 and task 2 parameters. Update task 2.5 back execution logic and improve movement commands for better control. Set observe flag in track detection functions for consistency.

main.py

@@ -22,7 +22,7 @@ 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_3.task_3 import run_task_3
+from task_3.task_3 import run_task_3, run_task_3_back
 from task_1.task_1 import run_task_1, run_task_1_back
 from task_2.task_2 import run_task_2, run_task_2_back
 from task_2_5.task_2_5 import run_task_2_5, run_task_2_5_back
@@ -43,37 +43,33 @@ def main():
     msg = robot_control_cmd_lcmt()
 
     try:
-        print("Recovery stand")
+        info("Recovery stand", "info")
         Ctrl.base_msg.stand_up()
         Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat
         
         # time.sleep(100) # TEST,
-        run_task_1(Ctrl, msg)
+        # run_task_1(Ctrl, msg)
 
-        arrow_direction = run_task_2(Ctrl, msg, xy_flag=False)
+        # arrow_direction = run_task_2(Ctrl, msg, xy_flag=False)
         # arrow_direction = 'right' # TEST
 
-        info(f"识别到箭头方向: {arrow_direction}", "info")
+        # info(f"识别到箭头方向: {arrow_direction}", "info")
-        run_task_2_5(Ctrl, msg, direction=arrow_direction)
+        # run_task_2_5(Ctrl, msg, direction=arrow_direction)
 
-        if arrow_direction == 'left':
+        # if arrow_direction == 'left':
-            run_task_4(Ctrl, msg)
+        #     run_task_4(Ctrl, msg)
-        else:
+        # else:
-            run_task_3(Ctrl, msg)
+        #     run_task_3(Ctrl, msg)
 
-        turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
+        # turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
-        run_task_5(Ctrl, msg, direction=arrow_direction)
+        # run_task_5(Ctrl, msg, direction=arrow_direction)
 
-        if arrow_direction == 'left':
+        # if arrow_direction == 'left':
-            run_task_3_back(Ctrl, msg)
+        #     run_task_3_back(Ctrl, msg)
-        else:
+        # else:
-            run_task_4_back(Ctrl, msg)
+        #     run_task_4_back(Ctrl, msg)
 
-        return
+        # run_task_2_5_back(Ctrl, msg, direction=arrow_direction)
-
-        run_task_5(Ctrl, msg)
-
-        run_task_2_5_back(Ctrl, msg, direction=arrow_direction)
 
         run_task_2_back(Ctrl, msg)
 

task_1/task_1.py

@@ -13,6 +13,8 @@ from base_move.move_base_hori_line import (
 )
 from base_move.go_straight import go_straight
 from base_move.turn_degree import turn_degree, turn_degree_twice, turn_degree_v2
+from base_move.center_on_dual_tracks import center_on_dual_tracks
+from base_move.go_to_xy import go_to_xy
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 
 # 创建本模块特定的日志记录器
@@ -134,11 +136,11 @@ def run_task_1(ctrl, msg):
 
 def run_task_1_back(ctrl, msg):
     section('任务1-11: 返回', "移动")
-
+    go_straight(ctrl, msg, distance=0.2, observe=observe)
     turn_degree_v2(ctrl, msg, -90, absolute=True)
 
     section('任务1-11: 直线移动', "移动")
-    move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
+    move_to_hori_line(ctrl, msg, target_distance=0.2, observe=observe)
 
     section('任务1-12: 90度旋转', "旋转")
     turn_success, res = arc_turn_around_hori_line(
@@ -150,13 +152,14 @@ def run_task_1_back(ctrl, msg):
         max_radius=0.4,
         pass_align=True,
         observe=observe,
+        no_end_reset=True,
     )
 
-    return
+    turn_degree_v2(ctrl, msg, degree=90, absolute=True)
 
     section('任务1-13: 直线移动', "移动")
-    move_distance = 0.35
+    move_distance = 0.5
-    go_straight_by_hori_line(ctrl, msg, distance=move_distance, observe=observe)
+    go_straight(ctrl, msg, distance=move_distance, observe=observe)
 
     section('任务1-14: 模拟装货', "停止")
     info('机器人躺下，模拟装货过程', "信息")
@@ -169,27 +172,26 @@ def run_task_1_back(ctrl, msg):
     ctrl.base_msg.stand_up()
 
     section('任务1-16: 返回', "移动")
-    go_straight(ctrl, msg, distance=-(move_distance + res['radius'] + 0.3), observe=observe)
+    go_straight(ctrl, msg, distance=-(move_distance + res['radius']), observe=observe)
+
+    turn_degree_v2(ctrl, msg, degree=179, absolute=True)
 
     section('任务1-17: 90度转弯', "旋转")
     turn_success, res = arc_turn_around_hori_line(
         ctrl=ctrl,
         msg=msg,
-        angle_deg=90,
+        angle_deg=-85,
-        radius=0.4,
+        radius=res['radius'] * 2,
         pass_align=True,
         observe=observe,
     )
 
     section('任务1-18: 直线移动', "移动")
-    move_to_hori_line(ctrl, msg, target_distance=0.35, observe=observe)
+    move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
 
     section('任务1-19: 90度旋转', "旋转")
-    turn_success, res = arc_turn_around_hori_line(
+    turn_degree_v2(ctrl, msg, degree=0, absolute=True)
-        ctrl=ctrl,
+
-        msg=msg,
+    go_to_xy(ctrl, msg, target_x=-0.2, target_y=0, observe=observe)
-        angle_deg=90,
-        radius=0.4,
-    )
 
     success("任务1-back完成", "完成")

task_2/task_2.py

@@ -249,7 +249,6 @@ def run_task_2(ctrl, msg, xy_flag=False):
     return arrow_direction
 
 def run_task_2_back(ctrl, msg):
-    # go_to_xy(ctrl, msg, 0.2, 1, speed=0.5, observe=True)
     go_to_xy(ctrl, msg, 0.7, 5, speed=0.5, observe=True)
     turn_degree_v2(ctrl, msg, -106.5, absolute=True)
 

task_2_5/task_2_5.py

@@ -6,7 +6,7 @@ import os
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 
 from base_move.turn_degree import turn_degree, turn_degree_v2
-from base_move.go_to_xy import go_straight
+from base_move.go_to_xy import go_straight, go_to_x_v2, go_to_y_v2
 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
 
@@ -50,23 +50,6 @@ def run_task_2_5(Ctrl, msg, direction='left'):
 
 def run_task_2_5_back(Ctrl, msg, direction='left'):
     section('任务2.5-back：预备进入任务3', "启动")
+    turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)
 
-    arc_turn_around_hori_line(
+    go_to_x_v2(Ctrl, msg, target_x=0.8, observe=observe)
-        Ctrl,
-        msg,
-        angle_deg=90 if direction == 'left' else -90,
-        target_distance=0.3,
-        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,
-    # )
-    

utils/detect_track.py

@@ -362,7 +362,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)