From 0979d104151a3eeaf8807af7fe9766c033c84a1c Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Sat, 31 May 2025 13:53:54 +0000 Subject: [PATCH] Refactor main execution flow by enabling task execution based on arrow direction detection. Adjust max_time parameter in centering function for improved responsiveness. Update task 5's movement commands and section handling for clarity and consistency. --- base_move/center_on_dual_tracks.py | 4 ++-- main.py | 28 ++++++++++++++-------------- task_3/task_3.py | 10 +++++----- task_4/task_4.py | 2 +- task_5/task_5.py | 12 +++++++++--- 5 files changed, 31 insertions(+), 25 deletions(-) diff --git a/base_move/center_on_dual_tracks.py b/base_move/center_on_dual_tracks.py index b783df3..a32e2e4 100644 --- a/base_move/center_on_dual_tracks.py +++ b/base_move/center_on_dual_tracks.py @@ -7,7 +7,7 @@ sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing from utils.detect_dual_track_lines import detect_dual_track_lines -def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False, +def center_on_dual_tracks(ctrl, msg, max_time=3, max_deviation=8.0, observe=False, mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0, detect_height=0.4, @@ -18,7 +18,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 - max_time: 最大调整时间(秒)，默认为15秒 + max_time: 最大调整时间(秒)，默认为3秒 max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中，默认为8像素 observe: 是否输出中间状态信息和可视化结果，默认为False mode: 控制模式，默认为11 diff --git a/main.py b/main.py index 9f70f0e..8f23702 100644 --- a/main.py +++ b/main.py @@ -51,29 +51,29 @@ def main(): # run_task_1(Ctrl, msg) # arrow_direction = run_task_2(Ctrl, msg, xy_flag=False) - # arrow_direction = 'right' # TEST + arrow_direction = 'right' # TEST # 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) - # 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) + # run_task_2_back(Ctrl, msg) - run_task_1_back(Ctrl, msg) + # run_task_1_back(Ctrl, msg) except KeyboardInterrupt: print("\n程序被用户中断") diff --git a/task_3/task_3.py b/task_3/task_3.py index 9be74aa..f4f01ba 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -494,16 +494,16 @@ def run_task_3(ctrl, msg): section('任务3：上下坡', "启动") info('开始执行任务3...', "启动") - turn_degree_v2(ctrl, msg, 90, absolute=True) - # go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=True) + # turn_degree_v2(ctrl, msg, 90, absolute=True) + # go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=True) # TEST - section('任务3-1：up', "开始") - pass_up_down(ctrl, msg) + # section('任务3-1：up', "开始") + # pass_up_down(ctrl, msg) turn_degree_v2(ctrl, msg, 90, absolute=True) center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False) - section('任务3-2：yellow stop', "开始") + # section('任务3-2：yellow stop', "开始") go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3) # 原地站立3秒 diff --git a/task_4/task_4.py b/task_4/task_4.py index f63477e..9388bed 100644 --- a/task_4/task_4.py +++ b/task_4/task_4.py @@ -81,7 +81,7 @@ def run_task_4_back(ctrl, msg): def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, - max_distance=2, + max_distance=5, speed=0.3 ): """ diff --git a/task_5/task_5.py b/task_5/task_5.py index 2d70185..368dab5 100644 --- a/task_5/task_5.py +++ b/task_5/task_5.py @@ -210,7 +210,6 @@ def run_task_5(ctrl, msg, direction='left', observe=False): """ 走向卸货 """ - center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False) section('任务5-1：直线移动并扫描二维码', "移动") @@ -242,7 +241,7 @@ def run_task_5(ctrl, msg, direction='left', observe=False): ctrl.base_msg.lie_down(wait_time=3000) ctrl.base_msg.stand_up() - # section('任务5-4：返回', "移动") + section('任务5-4：返回', "移动") go_straight(ctrl, msg, distance=-1, speed=0.5, observe=observe) move_to_hori_line(ctrl, msg, target_distance=1.3, observe=observe) @@ -251,7 +250,14 @@ def run_task_5(ctrl, msg, direction='left', observe=False): go_straight(ctrl, msg, distance=1.2, speed=0.5, observe=observe) section('任务5-6：转弯', "转弯") - arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.3, observe=observe) + arc_turn_around_hori_line(ctrl, msg, angle_deg=-90, target_distance=0.3, observe=observe) + + section('任务5-5：上货', "卸货") + ctrl.base_msg.lie_down(wait_time=3000) + ctrl.base_msg.stand_up() + + section('任务5-7：返回', "移动") + turn_degree_v2(ctrl, msg, degree=-90, absolute=True) # 返回移动和扫描结果 return go_success, res['qr_result']