From 68cae0c85a31671c6ff1e5c0ea2b4882e317d167 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Sun, 1 Jun 2025 04:15:09 +0000 Subject: [PATCH] Update TIME_SLEEP in main.py for longer pauses, enhance task 2 with new movement commands and logging, refine task 3 to include standing duration, and adjust task 4's movement distance for better performance. Improve task 5 by ensuring unloading and loading sections are executed correctly. --- main.py | 22 +++++++++++----------- task_2/task_2.py | 28 +++++++++++++++++++++++++++- task_3/task_3.py | 24 +++++++++++++++++++++--- task_4/task_4.py | 14 +++++++++++--- task_5/task_5.py | 12 ++++++------ 5 files changed, 76 insertions(+), 24 deletions(-) diff --git a/main.py b/main.py index eced740..3ef8450 100644 --- a/main.py +++ b/main.py @@ -37,7 +37,7 @@ from base_move.go_to_xy import go_to_x_v2, go_to_y_v2 from utils.log_helper import info pass_marker = True -TIME_SLEEP = 3000 +TIME_SLEEP = 5000 def main(): rclpy.init() # 新增：在主程序中统一初始化 ROS 2 上下文 @@ -51,18 +51,18 @@ def main(): Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # time.sleep(100) # TEST, - # run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP) + run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP) - # arrow_direction = run_task_2(Ctrl, msg, xy_flag=False) - arrow_direction = 'right' # TEST + 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, time_sleep=TIME_SLEEP) + if arrow_direction == 'left': + run_task_4(Ctrl, msg) + else: + run_task_3(Ctrl, msg, time_sleep=TIME_SLEEP) turn_degree_v2(Ctrl, msg, degree=90, absolute=True) run_task_5(Ctrl, msg, direction=arrow_direction, time_sleep=TIME_SLEEP) @@ -74,7 +74,7 @@ def main(): run_task_2_5_back(Ctrl, msg, direction=arrow_direction) - run_task_2_back(Ctrl, msg) + run_task_2_back(Ctrl, msg, xy_flag=False) run_task_1_back(Ctrl, msg, time_sleep=TIME_SLEEP) diff --git a/task_2/task_2.py b/task_2/task_2.py index 4d45b39..2ca0bbf 100644 --- a/task_2/task_2.py +++ b/task_2/task_2.py @@ -13,6 +13,8 @@ from base_move.turn_degree import turn_degree, turn_degree_twice, turn_degree_v2 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection +from base_move.go_straight import go_straight, go_lateral + from base_move.move_base_hori_line import ( arc_turn_around_hori_line, go_straight_by_hori_line, @@ -292,8 +294,13 @@ def run_task_2(ctrl, msg, xy_flag=False): # 返回检测到的箭头方向 return arrow_direction -def run_task_2_back(ctrl, msg): +def run_task_2_back(ctrl, msg,xy_flag=False): + turn_degree_v2(ctrl, msg, 0, absolute=True) + go_straight(ctrl, msg, distance=0.9, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21]) go_to_xy(ctrl, msg, 0.72, -11.1, speed=0.5, observe=True) + print(ctrl.odo_msg.xyz[0], 'ctrl.odo_msg.xyz[0]') + print(ctrl.odo_msg.xyz[1], 'ctrl.odo_msg.xyz[1]') + print(ctrl.odo_msg.rpy[2], 'ctrl.odo_msg.rpy[2]') turn_degree_v2(ctrl, msg, -106.5, absolute=True) print('角度为',ctrl.odo_msg.rpy[2]) @@ -325,6 +332,19 @@ def run_task_2_back(ctrl, msg): (0.4,0,0,0,0,0,1.5), ] # [vel_x, vel_z] 对应 [左转, 右转, 左转] + gotoxy=[ + (0.76,-11), + (0.71,-11), + (1.12,-12), + (1.49,-12.1), + (1.1,-13.5), + (0.74,-13.3), + (0.68,-14.5), + (0.97,-14.3), + (1.69,-15.5), + (1.59,-15.6), + ] + for i, (vel_x, vel_y, vel_z ,rpy_x,rpy_y,rpy_z,t) in enumerate(directions): # 设置转向命令 msg.mode = 11 # Locomotion @@ -335,6 +355,12 @@ def run_task_2_back(ctrl, msg): msg.step_height = [0.03, 0.03] msg.life_count += 1 ctrl.Send_cmd(msg) + if xy_flag: + go_to_xy(ctrl, msg, gotoxy[i][0], gotoxy[i][1], speed=0.5, observe=True) + print('角度为',ctrl.odo_msg.rpy[2]) + print('x为',ctrl.odo_msg.xyz[0]) + print('y为',ctrl.odo_msg.xyz[1]) + print('z为',ctrl.odo_msg.xyz[2]) # 打印当前方向 print(f"开始 {text[i]} ") diff --git a/task_3/task_3.py b/task_3/task_3.py index 1097fe0..4ed45a3 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -437,9 +437,27 @@ def run_task_3(ctrl, msg, time_sleep=5000): ctrl.Send_cmd(msg) info("开始原地站立3秒", "站立") - # time.sleep(time_sleep / 1000) + time.sleep(time_sleep / 1000) info("完成原地站立", "站立") -def run_task_3_back(ctrl, msg): - return \ No newline at end of file +def run_task_3_back(ctrl, msg, time_sleep=5000): + go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3) + + # 原地站立3秒 + section("原地站立3秒", "站立") + msg.mode = 12 + msg.gait_id = 0 + msg.duration = 0 + msg.step_height = [0.06, 0.06] + msg.vel_des = [0, 0, 0] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + info("开始原地站立3秒", "站立") + time.sleep(time_sleep / 1000) + info("完成原地站立", "站立") + + section('任务3-1：up', "开始") + pass_up_down(ctrl, msg) + \ No newline at end of file diff --git a/task_4/task_4.py b/task_4/task_4.py index 9817097..8a3234e 100644 --- a/task_4/task_4.py +++ b/task_4/task_4.py @@ -17,6 +17,7 @@ from utils.detect_dual_track_lines import detect_dual_track_lines from base_move.move_base_hori_line import calculate_distance_to_line from task_4.pass_bar import pass_bar from base_move.center_on_dual_tracks import center_on_dual_tracks +from base_move.go_to_xy import go_to_xy, go_to_xy_v2 # 创建本模块特定的日志记录器 logger = get_logger("任务4") @@ -54,7 +55,7 @@ def run_task_4_back(ctrl, msg): ctrl.reset_offset() time.sleep(0.5) - go_straight(ctrl, msg, distance=3, speed=1, observe=True) + go_straight(ctrl, msg, distance=2.5, speed=1, observe=True) center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False) # 向右移动0.5秒 @@ -64,7 +65,7 @@ def run_task_4_back(ctrl, msg): turn_degree_v2(ctrl, msg, degree=-90, absolute=True) section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测") - go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.3) + go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.3, max_distance=1) section('任务4-2：通过栏杆', "移动") turn_degree_v2(ctrl, msg, degree=-90, absolute=True) @@ -87,13 +88,20 @@ def run_task_4_back(ctrl, msg): camera_height = 0.355 # 单位: 米 # INFO from TF-tree edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=True, save_log=True) current_distance = calculate_distance_to_line(edge_info, camera_height, observe=True) + # if current_distance < 1.5: + # go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21]) + # else: + # go_to_xy(ctrl, msg, 0, -10, speed=0.5, observe=True) go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21]) + print(ctrl.odo_msg.xyz[0], 'ctrl.odo_msg.xyz[0]') + print(ctrl.odo_msg.xyz[1], 'ctrl.odo_msg.xyz[1]') + print(ctrl.odo_msg.rpy[2], 'ctrl.odo_msg.rpy[2]') def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, - max_distance=5, + max_distance=3, speed=0.3 ): """ diff --git a/task_5/task_5.py b/task_5/task_5.py index 94119f7..a00a017 100644 --- a/task_5/task_5.py +++ b/task_5/task_5.py @@ -237,9 +237,9 @@ def run_task_5(ctrl, msg, direction='left', observe=False, time_sleep=5000): else: move_to_hori_line(ctrl, msg, target_distance=2, observe=observe, detect_func_version=4) - # section('任务5-3：卸货', "卸货") - # ctrl.base_msg.lie_down(wait_time=time_sleep) - # ctrl.base_msg.stand_up() + section('任务5-3：卸货', "卸货") + ctrl.base_msg.lie_down(wait_time=time_sleep) + ctrl.base_msg.stand_up() section('任务5-4：返回', "移动") go_straight(ctrl, msg, distance=-1, speed=0.5, observe=observe) @@ -252,9 +252,9 @@ def run_task_5(ctrl, msg, direction='left', observe=False, time_sleep=5000): section('任务5-6：转弯', "转弯") arc_turn_around_hori_line(ctrl, msg, angle_deg=-85, target_distance=0.3, observe=observe, no_end_reset=True) - # section('任务5-5：上货', "卸货") - # ctrl.base_msg.lie_down(wait_time=SLEEP_TIME) - # ctrl.base_msg.stand_up() + section('任务5-5：上货', "卸货") + ctrl.base_msg.lie_down(wait_time=SLEEP_TIME) + ctrl.base_msg.stand_up() section('任务5-7：返回', "移动") turn_degree_v2(ctrl, msg, degree=-90, absolute=True)