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

@@ -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 = run_task_2(Ctrl, msg, xy_flag=False)
-        arrow_direction = 'right' # TEST
+        # arrow_direction = 'right' # TEST
 
-        # info(f"识别到箭头方向: {arrow_direction}", "info")
+        info(f"识别到箭头方向: {arrow_direction}", "info")
-        # run_task_2_5(Ctrl, msg, direction=arrow_direction)
+        run_task_2_5(Ctrl, msg, direction=arrow_direction)
 
-        # if arrow_direction == 'left':
+        if arrow_direction == 'left':
-        #     run_task_4(Ctrl, msg)
+            run_task_4(Ctrl, msg)
-        # else:
+        else:
-        #     run_task_3(Ctrl, msg, time_sleep=TIME_SLEEP)
+            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)
         

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]} ")

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):
+def run_task_3_back(ctrl, msg, time_sleep=5000):
-    return
+    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)
+    

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
                                       ):
     """

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：卸货', "卸货")
+    section('任务5-3：卸货', "卸货")
-    # ctrl.base_msg.lie_down(wait_time=time_sleep)
+    ctrl.base_msg.lie_down(wait_time=time_sleep)
-    # ctrl.base_msg.stand_up()
+    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：上货', "卸货")
+    section('任务5-5：上货', "卸货")
-    # ctrl.base_msg.lie_down(wait_time=SLEEP_TIME)
+    ctrl.base_msg.lie_down(wait_time=SLEEP_TIME)
-    # ctrl.base_msg.stand_up()
+    ctrl.base_msg.stand_up()
 
     section('任务5-7：返回', "移动")
     turn_degree_v2(ctrl, msg, degree=-90, absolute=True)