🎏 success - 1

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,7 +51,7 @@ 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
@@ -68,13 +68,13 @@ def main():
         run_task_5(Ctrl, msg, direction=arrow_direction, time_sleep=TIME_SLEEP)
 
         if arrow_direction == 'left': 
-            run_task_3_back(Ctrl, msg)
+            run_task_3_back(Ctrl, msg, time_sleep=TIME_SLEEP)
         else:
             run_task_4_back(Ctrl, msg)
 
         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)
         
@@ -131,6 +131,9 @@ class Robot_Ctrl(object):
             self.mode_ok = self.rec_msg.mode
         else:
             self.mode_ok = 0
+    
+    def reset_offset(self):
+        self.is_calibrated = False
 
     def msg_handler_o(self, channel, data):
         self.odo_msg = localization_lcmt().decode(data)

task_1/task_1.py

@@ -80,7 +80,6 @@ def run_task_1(ctrl, msg, time_sleep=5000):
     move_distance = 0.3
     move_speed = 0.5
     if not direction:
-        # TODO to check
         # go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
         turn_degree_v2(ctrl, msg, degree=90, absolute=True, precision=True)
         go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
@@ -136,7 +135,6 @@ def run_task_1(ctrl, msg, time_sleep=5000):
 
 def run_task_1_back(ctrl, msg, time_sleep=5000):
     section('任务1-11: 返回', "移动")
-    go_straight(ctrl, msg, distance=0.2, observe=observe)
     turn_degree_v2(ctrl, msg, -90, absolute=True)
 
     section('任务1-11: 直线移动', "移动")
@@ -184,7 +182,9 @@ def run_task_1_back(ctrl, msg, time_sleep=5000):
         radius=res['radius'] * 2,
         pass_align=True,
         observe=observe,
+        no_end_reset=True,
     )
+    turn_degree_v2(ctrl, msg, degree=90, absolute=True)
 
     section('任务1-18: 直线移动', "移动")
     move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
@@ -192,6 +192,6 @@ def run_task_1_back(ctrl, msg, time_sleep=5000):
     section('任务1-19: 90度旋转', "旋转")
     turn_degree_v2(ctrl, msg, degree=0, absolute=True)
 
-    go_to_xy(ctrl, msg, target_x=-0.2, target_y=0, observe=observe)
+    go_straight(ctrl, msg, distance=-0.8, observe=observe)
 
     success("任务1-back完成", "完成")

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,
@@ -138,6 +140,8 @@ class AsyncArrowDetector:
             return self.left_count, self.right_count
 
 def run_task_2(ctrl, msg, xy_flag=False):
+    # go_to_xy(ctrl, msg, 0.8, 5, speed=0.5, observe=True)
+    # return 'right'
     # 微调 xy 和角度
     go_to_xy_v2(ctrl, msg, 0.9, 0.25, speed=0.5, observe=True)
     turn_degree_v2(ctrl, msg, 0.8, absolute=True)
@@ -289,8 +293,13 @@ 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.7, 5, speed=0.5, observe=True)
+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])
@@ -322,6 +331,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
@@ -332,6 +354,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_2_5/task_2_5.py

@@ -14,10 +14,8 @@ observe = True
 
 def run_task_2_5(Ctrl, msg, direction='left'):
     section('任务2.5：预备进入任务3', "启动")
-    go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe)
-
-    # TEST
-    turn_degree_v2(Ctrl, msg, 90, absolute=observe)
+    turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
+    # go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe)
 
     section('任务2.5-1：第一次旋转', "移动")
 
@@ -39,5 +37,5 @@ 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=0, absolute=True)
-    go_to_x_v2(Ctrl, msg, target_x=1, observe=observe)
-    go_to_y_v2(Ctrl, msg, target_y=5.5, speed=0.5, observe=True)
+    # go_to_x_v2(Ctrl, msg, target_x=1, observe=observe)
+    # go_to_y_v2(Ctrl, msg, target_y=5.2, speed=0.5, observe=True)

task_3/task_3.py

@@ -42,7 +42,6 @@ robot_cmd = {
 
 def pass_up_down(ctrl, msg):
     usergait_msg = file_send_lcmt()
-    # lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
     
     try:
         steps = toml.load("./task_3/Gait_Params_up.toml")
@@ -96,29 +95,17 @@ def pass_up_down(ctrl, msg):
         msg.duration = 1000
         msg.life_count += 1
         
-        # 参数设置
-        stable_count = 0  # 用于计数z轴稳定的次数
-        stable_threshold = 8  # 连续15次检测z轴不再增加则认为已经停止
-        z_speed_threshold = 0.01  # z轴速度阈值，小于这个值认为已经停止爬升
         climb_speed_threshold = 0.05  # 检测到开始爬坡的速度阈值
-        max_iterations = 230  # 最大循环次数，作为安全保障
-        min_iterations = 170  # 最小循环次数，作为安全保障
-
-        # 姿态判断参数
-        pitch_threshold = 0.05  # 俯仰角阈值（弧度）
-        angular_rate_threshold = 0.03  # 角速度阈值（弧度/秒）
+        max_iterations = 450  # 最大循环次数，作为安全保障
+        y_target = 9.3  # 目标y坐标，达到此值时停止
 
         # 阶段控制
         climbing_detected = False  # 是否检测到正在爬坡
         
-        # 高度变化记录
-        height_window = []
-        pitch_window = []
-        window_size = 8
-        
         # 记录起始姿态和高度
         start_height = ctrl.odo_msg.xyz[2]
-        info(f"开始监测爬坡过程，初始高度: {start_height:.3f}", "监测")
+        start_y = ctrl.odo_msg.xyz[1]
+        info(f"开始监测爬坡过程，初始高度: {start_height:.3f}, 初始Y坐标: {start_y:.3f}", "监测")
         
         for i in range(max_iterations):
             # 发送控制命令维持心跳
@@ -127,51 +114,22 @@ def pass_up_down(ctrl, msg):
             # 获取当前状态数据
             vz = ctrl.odo_msg.vxyz[2]  # Z轴速度
             current_height = ctrl.odo_msg.xyz[2]  # 当前高度
+            current_y = ctrl.odo_msg.xyz[1]  # 当前y坐标
             current_pitch = ctrl.odo_msg.rpy[1]  # 当前俯仰角
-            pitch_rate = ctrl.odo_msg.omegaBody[1]  # 俯仰角速度
-            vbody_z = ctrl.odo_msg.vBody[2]  # 机体坐标系Z速度
-            
-            # 更新滑动窗口数据
-            height_window.append(current_height)
-            pitch_window.append(current_pitch)
-            if len(height_window) > window_size:
-                height_window.pop(0)
-                pitch_window.pop(0)
             
             # 每10次迭代打印一次当前信息
             if i % 10 == 0:
-                info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
+                info(f"Step:{i} 当前Y坐标={current_y:.3f}, 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}", "监测")
             
             # 检测是否开始爬坡阶段
             if not climbing_detected and vz > climb_speed_threshold:
                 climbing_detected = True
                 info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
             
-            # 多条件判断是否完成爬坡
-            if i > min_iterations and climbing_detected and len(height_window) == window_size:
-                # 计算高度和俯仰角的稳定性
-                height_std = np.std(height_window)  # 高度标准差
-                pitch_std = np.std(pitch_window)    # 俯仰角标准差
-                
-                # 多条件综合判断
-                position_stable = abs(vz) < z_speed_threshold  # 垂直速度稳定
-                attitude_stable = abs(current_pitch) < pitch_threshold  # 俯仰角接近水平
-                angular_rate_stable = abs(pitch_rate) < angular_rate_threshold  # 角速度稳定
-                height_stable = height_std < 0.01  # 高度变化小
-                pitch_stable = pitch_std < 0.01  # 俯仰角变化小
-                vbody_stable = abs(vbody_z) < 0.01  # 机体Z方向速度稳定
-                
-                # 综合判断条件
-                if (position_stable and attitude_stable and angular_rate_stable) or \
-                   (position_stable and height_stable and pitch_stable) or \
-                   (vbody_stable and attitude_stable and height_stable):
-                    stable_count += 1
-                    if stable_count >= stable_threshold:
-                        info(f"检测到已完成爬坡：\n  - Z轴速度: {vz:.3f}\n  - 俯仰角: {current_pitch:.3f}\n  - 角速度: {pitch_rate:.3f}\n  - 当前高度: {current_height:.3f}\n  - 上升了: {current_height - start_height:.3f}米", "监测")
-                        break
-                else:
-                    # 重置稳定计数
-                    stable_count = 0
+            # 检查是否达到目标y坐标
+            if current_y >= y_target:
+                info(f"已达到目标Y坐标: {current_y:.3f}，停止爬坡", "监测")
+                break
             
             time.sleep(0.2)
     except KeyboardInterrupt:
@@ -184,8 +142,9 @@ def pass_up_down(ctrl, msg):
 
     section('任务3-2：x = 2', "开始")
     turn_degree_v2(ctrl, msg, 90, absolute=True)
+    go_lateral(ctrl, msg, distance=0.1, speed=0.5, observe=True)
     center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3)
-    go_straight(ctrl, msg, distance=0.5, speed=0.5, observe=True)
+    go_straight(ctrl, msg, distance=0.6, speed=0.5, observe=True)
     time.sleep(1)
 
     section('任务3-3：down', "完成")
@@ -242,28 +201,17 @@ def pass_up_down(ctrl, msg):
         msg.life_count += 1
         
         # 参数设置
-        stable_count = 0  # 用于计数z轴稳定的次数
-        stable_threshold = 10  # 连续10次检测z轴速度接近零则认为已经到达平地
-        z_speed_threshold = 0.005  # z轴速度阈值，小于这个值认为已经停止下降
         descent_speed_threshold = -0.05  # 检测到开始下坡的速度阈值（负值表示下降）
-        max_iterations = 110  # 最大循环次数，作为安全保障
-        min_iterations = 80  # 最小循环次数，确保有足够的时间开始动作
+        max_iterations = 200  # 最大循环次数，作为安全保障
         start_height = ctrl.odo_msg.xyz[2]  # 记录起始高度
-        
-        # 姿态判断参数
-        pitch_threshold = 0.05  # 俯仰角阈值（弧度）
-        angular_rate_threshold = 0.03  # 角速度阈值（弧度/秒）
+        start_y = ctrl.odo_msg.xyz[1]  # 记录起始y坐标
+        y_target = 11.6  # 目标y坐标，达到此值时停止
         
         # 阶段控制
         descending_detected = False  # 是否检测到正在下坡
         flat_ground_detected = False  # 是否检测到已到达平地
         
-        # 高度变化记录
-        height_window = []
-        pitch_window = []
-        window_size = 8
-        
-        info(f"开始监测下坡过程，初始高度: {start_height}", "监测")
+        info(f"开始监测下坡过程，初始高度: {start_height}, 初始Y坐标: {start_y:.3f}", "监测")
         
         for i in range(max_iterations):
             # 发送控制命令维持心跳
@@ -272,57 +220,28 @@ def pass_up_down(ctrl, msg):
             # 获取当前状态数据
             vz = ctrl.odo_msg.vxyz[2]  # Z轴速度
             current_height = ctrl.odo_msg.xyz[2]  # 当前高度
+            current_y = ctrl.odo_msg.xyz[1]  # 当前y坐标
             current_pitch = ctrl.odo_msg.rpy[1]  # 当前俯仰角
-            pitch_rate = ctrl.odo_msg.omegaBody[1]  # 俯仰角速度
-            vbody_z = ctrl.odo_msg.vBody[2]  # 机体坐标系Z速度
-            
-            # 更新滑动窗口数据
-            height_window.append(current_height)
-            pitch_window.append(current_pitch)
-            if len(height_window) > window_size:
-                height_window.pop(0)
-                pitch_window.pop(0)
-            
+
             # 每10次迭代打印一次当前信息
             if observe and i % 10 == 0:
-                info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
+                info(f"Step:{i} 当前Y坐标={current_y:.3f}, 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}", "监测")
             
             # 检测是否开始下坡阶段
             if not descending_detected and vz < descent_speed_threshold:
                 descending_detected = True
                 info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
             
-            # 多条件判断是否到达平地
-            if i > min_iterations and descending_detected and len(height_window) == window_size:
-                # 计算高度和俯仰角的稳定性
-                height_std = np.std(height_window)  # 高度标准差
-                pitch_std = np.std(pitch_window)    # 俯仰角标准差
-                
-                # 多条件综合判断
-                position_stable = abs(vz) < z_speed_threshold  # 垂直速度稳定
-                attitude_stable = abs(current_pitch) < pitch_threshold  # 俯仰角接近水平
-                angular_rate_stable = abs(pitch_rate) < angular_rate_threshold  # 角速度稳定
-                height_stable = height_std < 0.01  # 高度变化小
-                pitch_stable = pitch_std < 0.01  # 俯仰角变化小
-                vbody_stable = abs(vbody_z) < 0.01  # 机体Z方向速度稳定
-                
-                # 综合判断条件
-                if (position_stable and attitude_stable and angular_rate_stable) or \
-                   (position_stable and height_stable and pitch_stable) or \
-                   (vbody_stable and attitude_stable and height_stable):
-                    stable_count += 1
-                    if stable_count >= stable_threshold:
-                        info(f"检测到已到达平地：\n  - Z轴速度: {vz:.3f}\n  - 俯仰角: {current_pitch:.3f}\n  - 角速度: {pitch_rate:.3f}\n  - 高度: {current_height:.3f}\n  - 下降了: {start_height - current_height:.3f}米", "监测")
-                        flat_ground_detected = True
-                        break
-                else:
-                    # 重置稳定计数
-                    stable_count = 0
+            # 检查是否达到目标y坐标
+            if current_y >= y_target:
+                info(f"已达到目标Y坐标: {current_y:.3f}，停止下坡", "监测")
+                flat_ground_detected = True
+                break
             
             time.sleep(0.2)
         
         if not flat_ground_detected:
-            info(f"达到最大循环次数，未能明确检测到到达平地。当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "警告")
+            info(f"达到最大循环次数，未能到达目标Y坐标。当前Y坐标: {ctrl.odo_msg.xyz[1]:.3f}", "警告")
     except KeyboardInterrupt:
         msg.mode = 7 #PureDamper before KeyboardInterrupt:
         msg.gait_id = 0
@@ -495,18 +414,20 @@ def run_task_3(ctrl, msg, time_sleep=5000):
     info('开始执行任务3...', "启动")
 
     turn_degree_v2(ctrl, msg, 90, absolute=True)
+    go_straight(ctrl, msg, distance=0.1, speed=0.5, observe=True)
 
     section('任务3-1：up', "开始")
     pass_up_down(ctrl, msg)
 
     turn_degree_v2(ctrl, msg, 90, absolute=True)
+    go_lateral(ctrl, msg, distance=0.2, speed=0.5, observe=True)
     center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)
 
     section('任务3-2：yellow stop', "开始")
     go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
     
     # 原地站立3秒
-    section("原地站立3秒", "站立")
+    section("原地站立5秒", "站立")
     msg.mode = 12
     msg.gait_id = 0
     msg.duration = 0
@@ -515,10 +436,28 @@ def run_task_3(ctrl, msg, time_sleep=5000):
     msg.life_count += 1
     ctrl.Send_cmd(msg)
     
-    info("开始原地站立3秒", "站立")
+    info("开始原地站立5秒", "站立")
     time.sleep(time_sleep / 1000)
     info("完成原地站立", "站立")
     
 
-def run_task_3_back(ctrl, msg):
-    return
+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("原地站立5秒", "站立")
+    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("开始原地站立5秒", "站立")
+    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")
@@ -51,7 +52,10 @@ def run_task_4_back(ctrl, msg):
     turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
     center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)
 
-    go_straight(ctrl, msg, distance=2, speed=1, observe=True)
+    ctrl.reset_offset()
+    time.sleep(0.5)
+
+    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秒
@@ -61,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.2)
+    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)
@@ -69,12 +73,12 @@ def run_task_4_back(ctrl, msg):
 
     turn_degree_v2(ctrl, msg, degree=-90, absolute=True) 
     section('任务4-3：stone', "移动")
-    go_straight(ctrl, msg, distance=1, speed=2)
+    go_straight(ctrl, msg, distance=1, speed=1)
 
     turn_degree_v2(ctrl, msg, degree=-92, absolute=True) 
 
     # Use enhanced calibration for better Y-axis correction on stone path
-    go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
+    go_straight(ctrl, msg, distance=4.8, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
     # go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35, 
     #                                      mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
 
@@ -84,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

@@ -247,7 +247,7 @@ def run_task_5(ctrl, msg, direction='left', observe=False, time_sleep=5000):
 
     section('任务5-5：转弯', "转弯")
     turn_degree_v2(ctrl, msg, degree=179, absolute=True)
-    go_straight(ctrl, msg, distance=1.2, speed=0.6, observe=observe)
+    go_straight(ctrl, msg, distance=1.4, speed=0.6, observe=observe)
 
     section('任务5-6：转弯', "转弯")
     arc_turn_around_hori_line(ctrl, msg, angle_deg=-85, target_distance=0.3, observe=observe, no_end_reset=True)

utils/detect_dual_track_lines.py

@@ -11,6 +11,7 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
                             min_line_length=0.05,
                             max_line_gap=40,
                             detect_height=0.4):
+    save_log = False
     """
     检测左右两条平行的黄色轨道线，优化后能够更准确处理各种路况