diff --git a/main.py b/main.py index 39765b6..ff5df75 100755 --- a/main.py +++ b/main.py @@ -39,7 +39,7 @@ from Circle1.circle1 import circle1 from Circle2.circle2 import circle2 pass_marker = True -TIME_SLEEP = 5000 # TODO 比赛时改成 5000 +# TIME_SLEEP = 5000 # TODO 比赛时改成 5000 from enum import Enum, auto @@ -66,99 +66,99 @@ def main(): Ctrl.voice.play_audio('ok') demo.play_audio(text) - # try: - # # INFO Real Task - # # TAG task - 0 - # info("Recovery stand", "info") - # Ctrl.base_msg.stand_up() - # Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # INFO 但是正式比赛或许也有用 + try: + # INFO Real Task + # TAG task - 0 + info("Recovery stand", "info") + Ctrl.base_msg.stand_up() + Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # INFO 但是正式比赛或许也有用 - # 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, yellow_ratio_threshold=0.012, speed=0.3, max_time=30, observe=True) - # elif TASK == TaskType.RED_BAR: - # from task_4.task_4 import go_straight_until_red_bar - # 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]) - # elif TASK == TaskType.STONE_ROAD: - # from task_3.task_3 import pass_stone - # pass_stone(Ctrl, msg, distance = 4.5, observe=False) - # elif TASK == TaskType.MOVE_TO_LINE: - # from base_move.move_base_hori_line import move_to_hori_line - # move_to_hori_line(Ctrl, msg, target_distance = 1.1, observe=False) - # 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) - # else: - # pass + 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, yellow_ratio_threshold=0.012, speed=0.3, max_time=30, observe=True) + elif TASK == TaskType.RED_BAR: + from task_4.task_4 import go_straight_until_red_bar + 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]) + elif TASK == TaskType.STONE_ROAD: + from task_3.task_3 import pass_stone + pass_stone(Ctrl, msg, distance = 4.5, observe=False) + elif TASK == TaskType.MOVE_TO_LINE: + from base_move.move_base_hori_line import move_to_hori_line + move_to_hori_line(Ctrl, msg, target_distance = 1.1, observe=False) + 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) + else: + pass - # if TASK != TaskType.TASK: - # # 如果不是 task 类型，直接返回 - # pass + if TASK != TaskType.TASK: + # 如果不是 task 类型，直接返回 + pass - # # TAG task - 1 - # run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP) + # TAG task - 1 + # run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP) - # # TAG task - 2 - # arrow_direction = xiesi2(Ctrl, msg) + # TAG task - 2 + arrow_direction = xiesi2(Ctrl, msg) - # # TAG task - 3 / 4 - part I - # # arrow_direction = 'left' - # if arrow_direction == 'left': - # circle1(Ctrl, msg) - # else: - # circle2(Ctrl, msg) + # TAG task - 3 / 4 - part I + # arrow_direction = 'left' + if arrow_direction == 'left': + circle1(Ctrl, msg) + else: + circle2(Ctrl, msg) - # # TAG task - 5 - # # Ctrl.base_msg.stand_up() - # # turn_degree_v2(Ctrl, msg, degree=90, absolute=True) - # # run_task_5(Ctrl, msg, direction=arrow_direction, observe=True, time_sleep=TIME_SLEEP) # B区任务 - # # Ctrl.base_msg.lie_down(wait_time=10000) - # # print('wait 30s') + # TAG task - 5 + # Ctrl.base_msg.stand_up() + # turn_degree_v2(Ctrl, msg, degree=90, absolute=True) + # run_task_5(Ctrl, msg, direction=arrow_direction, observe=True, time_sleep=TIME_SLEEP) # B区任务 + # Ctrl.base_msg.lie_down(wait_time=10000) + # print('wait 30s') - # # TAG task - 3 / 4 - part II - # # Ctrl.base_msg.stand_up() - # # if arrow_direction == 'left': - # # 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]) - # # else: - # # from task_3.task_3 import pass_stone - # # pass_stone(Ctrl, msg, distance = 4.5, observe=False) + # TAG task - 3 / 4 - part II + # Ctrl.base_msg.stand_up() + # if arrow_direction == 'left': + # 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]) + # else: + # from task_3.task_3 import pass_stone + # pass_stone(Ctrl, msg, distance = 4.5, observe=False) - # # TAG task - 2.5 - back - # #nrun_task_2_5_back(Ctrl, msg, direction=arrow_direction) - # # Ctrl.base_msg.lie_down(wait_time=10000) - # # print('wait 30s') + # TAG task - 2.5 - back + #nrun_task_2_5_back(Ctrl, msg, direction=arrow_direction) + # Ctrl.base_msg.lie_down(wait_time=10000) + # print('wait 30s') - # # Ctrl.base_msg.stand_up() - # # TAG task - 2 - back - # # run_task_2_back(Ctrl, msg) - # xiesi2_back(Ctrl, msg) - # # Ctrl.base_msg.lie_down(wait_time=10000) - # # print('wait 30s') + # Ctrl.base_msg.stand_up() + # TAG task - 2 - back + # run_task_2_back(Ctrl, msg) + xiesi2_back(Ctrl, msg) + # Ctrl.base_msg.lie_down(wait_time=10000) + # print('wait 30s') - # # Ctrl.base_msg.stand_up() - # # TAG task - 1 - back - # # run_task_1_back(Ctrl, msg, time_sleep=TIME_SLEEP) + # Ctrl.base_msg.stand_up() + # TAG task - 1 - back + # run_task_1_back(Ctrl, msg, time_sleep=TIME_SLEEP) - # print('🏁 Task finished.') - # try: - # while rclpy.ok(): - # executor.spin_once(timeout_sec=0.1) - # if getattr(node, '_shutdown_flag', False): - # break - # except KeyboardInterrupt: - # pass + print('🏁 Task finished.') + try: + while rclpy.ok(): + executor.spin_once(timeout_sec=0.1) + if getattr(node, '_shutdown_flag', False): + break + except KeyboardInterrupt: + pass except KeyboardInterrupt: print("\n程序被用户中断")