diff --git a/main.py b/main.py index 6b533b4..9f70f0e 100644 --- a/main.py +++ b/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") - run_task_2_5(Ctrl, msg, direction=arrow_direction) + # info(f"识别到箭头方向: {arrow_direction}", "info") + # run_task_2_5(Ctrl, msg, direction=arrow_direction) - if arrow_direction == 'left': - run_task_4(Ctrl, msg) - else: - run_task_3(Ctrl, msg) + # if arrow_direction == 'left': + # run_task_4(Ctrl, msg) + # else: + # run_task_3(Ctrl, msg) - turn_degree_v2(Ctrl, msg, degree=90, absolute=True) - run_task_5(Ctrl, msg, direction=arrow_direction) + # turn_degree_v2(Ctrl, msg, degree=90, absolute=True) + # run_task_5(Ctrl, msg, direction=arrow_direction) - if arrow_direction == 'left': - run_task_3_back(Ctrl, msg) - else: - run_task_4_back(Ctrl, msg) + # if arrow_direction == 'left': + # run_task_3_back(Ctrl, msg) + # else: + # run_task_4_back(Ctrl, msg) - return - - run_task_5(Ctrl, msg) - - run_task_2_5_back(Ctrl, msg, direction=arrow_direction) + # run_task_2_5_back(Ctrl, msg, direction=arrow_direction) run_task_2_back(Ctrl, msg) diff --git a/task_1/task_1.py b/task_1/task_1.py index f02c64a..7d92b33 100644 --- a/task_1/task_1.py +++ b/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 - go_straight_by_hori_line(ctrl, msg, distance=move_distance, observe=observe) + move_distance = 0.5 + 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, - radius=0.4, + angle_deg=-85, + 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( - ctrl=ctrl, - msg=msg, - angle_deg=90, - radius=0.4, - ) + turn_degree_v2(ctrl, msg, degree=0, absolute=True) + + go_to_xy(ctrl, msg, target_x=-0.2, target_y=0, observe=observe) success("任务1-back完成", "完成") \ No newline at end of file diff --git a/task_2/task_2.py b/task_2/task_2.py index 5ca3b0e..a8d174c 100644 --- a/task_2/task_2.py +++ b/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) diff --git a/task_2_5/task_2_5.py b/task_2_5/task_2_5.py index 9c7d114..d55948e 100644 --- a/task_2_5/task_2_5.py +++ b/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( - 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, - # ) - \ No newline at end of file + go_to_x_v2(Ctrl, msg, target_x=0.8, observe=observe) diff --git a/utils/detect_track.py b/utils/detect_track.py index d60ef8f..579005d 100644 --- a/utils/detect_track.py +++ b/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)