feat: add TaskType enumeration and integrate task handling in main program; adjust STONE_DISTANCE in task_4

main.py

@@ -32,11 +32,22 @@ from task_5.task_5 import run_task_5
 from base_move.turn_degree import turn_degree_v2
 from base_move.go_to_xy import go_to_x_v2, go_to_y_v2
 from utils.log_helper import info
-from task_4.pass_bar import pass_bar
 
 pass_marker = True
 TIME_SLEEP = 3000  # TODO 比赛时改成 5000
 
+from enum import Enum, auto
+
+class TaskType(Enum):
+    TASK = auto()           # 默认任务
+    PASS_BAR = auto()       # TODO 测试低头高度；可以修改代码，和 RED-BAR 一块测试。
+    YELLOW_LIGHT = auto()   # TODO 设定 yellow threshold
+    RED_BAR = auto()        # TODO 设定 red threshold
+    UP_AND_DOWN = auto()    # TODO 调整其完成从上坡到下坡
+    STONE_ROAD = auto()     # TODO 调整其完成从石板路到石板路
+
+TASK = TaskType.STONE_ROAD
+
 def main():
     rclpy.init()  # 新增：在主程序中统一初始化 ROS 2 上下文
     Ctrl = Robot_Ctrl()
@@ -65,13 +76,32 @@ def main():
         Ctrl.base_msg.stand_up()
         Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat # INFO 但是正式比赛或许也有用
 
-        # TEST #1: pass-bar
-        pass_bar(Ctrl, msg)
-        Ctrl.base_msg.stand_up()
-        Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat # INFO 但是正式比赛或许也有用
-
-        return
-
+        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
+            turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)
+            go_until_yellow_area(Ctrl, msg)
+        elif TASK == TaskType.RED_BAR:
+            from task_3.task_3 import go_until_red_area
+            go_until_red_area(Ctrl, msg)
+        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])
+        elif TASK == TaskType.STONE_ROAD:
+            from task_3.task_3 import pass_stone
+            turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)
+            Ctrl.base_msg.stand_up()
+            Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat # INFO 但是正式比赛或许也有用
+            pass_stone(Ctrl, msg, distance = 4, observe=False)
+        else:
+            pass
+        
+        if TASK != TaskType.TASK:
+            # 如果不是 task 类型，直接返回
+            return
         
         # TAG task - 1
         run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)

task_3/task_3.py

@@ -247,6 +247,7 @@ def go_straight_with_enhanced_calibration(ctrl, msg, distance, speed=0.5, observ
                                          mode=11, gait_id=3, step_height=[0.21, 0.21]):
     """
     控制机器人在石板路上沿直线行走，使用视觉校准和姿态传感器融合来保持直线
+    现在用于 task-3 的上下坡
     
     参数:
     ctrl: Robot_Ctrl 对象

task_4/task_4.py

@@ -24,7 +24,7 @@ logger = get_logger("任务4")
 
 observe = True
 
-STONE_DISTANCE = 5.0  # TODO 距离参数需要微调
+STONE_DISTANCE = 4.0  # TODO 距离参数需要微调
 RED_RATIO_THRESHOLD = 0.35  # TODO 红色区域比例阈值需要微调
 
 def run_task_4(ctrl, msg):