Refactor main.py and task_3.py: Re-enable task execution logic by restoring arrow direction handling and task function calls. Update max_iterations in pass_up_down function for improved safety. Add enhanced go_straight_with_enhanced_calibration function in task_4.py for better navigation on stone paths.

base_move/center_on_dual_tracks.py

@@ -200,6 +200,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=Fa
     
     return success
 
+
 def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_time=15, observe=False,
                                 mode=11, gait_id=26, step_height=[0.06, 0.06],
                                 stone_path_mode=None):

main.py

@@ -59,13 +59,15 @@ def main():
 
         arrow_direction = 'right' # TEST
 
-        # 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)
+        return
+
+        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)

task_3/task_3.py

@@ -253,7 +253,7 @@ def pass_up_down(ctrl, msg):
         stable_threshold = 10  # 连续10次检测z轴速度接近零则认为已经到达平地
         z_speed_threshold = 0.005  # z轴速度阈值，小于这个值认为已经停止下降
         descent_speed_threshold = -0.05  # 检测到开始下坡的速度阈值（负值表示下降）
-        max_iterations = 120  # 最大循环次数，作为安全保障
+        max_iterations = 100  # 最大循环次数，作为安全保障
         min_iterations = 80  # 最小循环次数，确保有足够的时间开始动作
         start_height = ctrl.odo_msg.xyz[2]  # 记录起始高度
         
@@ -501,16 +501,17 @@ 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)
 
-    section('任务3-1：up', "开始")
-    pass_up_down(ctrl, msg)
+    # section('任务3-1：up', "开始")
+    # pass_up_down(ctrl, msg)
 
-    section('任务3-2：center on dual tracks', "开始")
     turn_degree_v2(ctrl, msg, 90, absolute=True)
     center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
 
+    return
+
     section('任务3-2：yellow stop', "开始")
     go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
     

task_4/task_4.py

@@ -3,6 +3,7 @@ import sys
 import os
 import cv2
 import numpy as np
+import math
 
 # 添加父目录到路径，以便能够导入utils
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
@@ -12,6 +13,7 @@ from base_move.go_straight import go_straight, go_lateral
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 from utils.gray_sky_analyzer import analyze_gray_sky_ratio
 from utils.detect_track import detect_horizontal_track_edge
+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
 
@@ -50,18 +52,21 @@ def run_task_4_back(ctrl, msg):
 
     # 向右移动0.5秒
     section('任务4-回程：向右移动', "移动")
-    go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True)  # DEBUG
+    # go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True)  # DEBUG
 
     section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
     go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.5)
 
     section('任务4-2：通过栏杆', "移动")
+    turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
     pass_bar(ctrl, msg)
 
     section('任务4-3：stone', "移动")
     go_straight(ctrl, msg, distance=1, speed=2)
 
-    go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21])
+    # Use enhanced calibration for better Y-axis correction on stone path
+    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)
 
     section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
     # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
@@ -155,3 +160,304 @@ def go_straight_until_sky_ratio_below(ctrl, msg,
         warning(f"已达到最大移动距离 {max_distance} 米，但未找到天空比例小于 {sky_ratio_threshold:.2%} 的位置", "超时")
     
     return success_flag
+
+def go_straight_with_enhanced_calibration(ctrl, msg, distance, speed=0.5, observe=False,
+                                         mode=11, gait_id=3, step_height=[0.21, 0.21]):
+    """
+    控制机器人在石板路上沿直线行走，使用视觉校准和姿态传感器融合来保持直线
+    
+    参数:
+    ctrl: Robot_Ctrl 对象
+    msg: 控制消息对象
+    distance: 要行走的距离(米)
+    speed: 行走速度(米/秒)
+    observe: 是否输出调试信息
+    mode: 运动模式
+    gait_id: 步态ID
+    step_height: 摆动腿高度
+    
+    返回:
+    bool: 是否成功完成
+    """
+    section("开始石板路增强直线移动", "石板路移动")
+    
+    # 参数验证
+    if abs(distance) < 0.01:
+        info("距离太短，无需移动", "信息")
+        return True
+    
+    # 检查相机是否可用
+    if not hasattr(ctrl, 'image_processor') or not hasattr(ctrl.image_processor, 'get_current_image'):
+        warning("机器人控制器没有提供图像处理器，将使用姿态传感器辅助校准", "警告")
+    
+    # 限制速度范围
+    speed = min(max(abs(speed), 0.1), 1.0)
+    
+    # 确定前进或后退方向
+    forward = distance > 0
+    move_speed = speed if forward else -speed
+    abs_distance = abs(distance)
+    
+    # 获取起始位置和姿态
+    start_position = list(ctrl.odo_msg.xyz)
+    start_yaw = ctrl.odo_msg.rpy[2]  # 记录起始朝向
+    
+    if observe:
+        debug(f"起始位置: {start_position}", "位置")
+        info(f"开始石板路增强直线移动，距离: {abs_distance:.3f}米，速度: {abs(move_speed):.2f}米/秒", "移动")
+    
+    # 设置移动命令
+    msg.mode = mode
+    msg.gait_id = gait_id
+    msg.step_height = step_height
+    msg.duration = 0  # wait next cmd
+    
+    # 根据需要移动的距离动态调整移动速度
+    if abs_distance > 1.0:
+        actual_speed = move_speed  # 距离较远时用设定速度
+    else:
+        actual_speed = move_speed * 0.8  # 较近距离略微降速
+    
+    # 设置移动速度和方向
+    msg.vel_des = [actual_speed, 0, 0]  # [前进速度, 侧向速度, 角速度]
+    msg.life_count += 1
+    
+    # 发送命令
+    ctrl.Send_cmd(msg)
+    
+    # 估算移动时间，但实际上会通过里程计控制
+    estimated_time = abs_distance / abs(actual_speed)
+    timeout = estimated_time + 5  # 增加超时时间
+    
+    # 使用里程计进行实时监控移动距离
+    distance_moved = 0
+    start_time = time.time()
+    last_check_time = start_time
+    position_check_interval = 0.1  # 位置检查间隔(秒)
+    
+    # 计算移动方向单位向量（世界坐标系下）
+    direction_vector = [math.cos(start_yaw), math.sin(start_yaw)]
+    if not forward:
+        direction_vector = [-direction_vector[0], -direction_vector[1]]
+    
+    # 视觉跟踪相关变量
+    vision_check_interval = 0.2  # 视觉检查间隔(秒)
+    last_vision_check = start_time
+    vision_correction_gain = 0.006  # 视觉修正增益系数
+    
+    # 用于滤波的队列
+    deviation_queue = []
+    filter_size = 5
+    last_valid_deviation = 0
+    
+    # PID控制参数 - 用于角度修正
+    kp_angle = 0.6  # 比例系数
+    ki_angle = 0.02  # 积分系数
+    kd_angle = 0.1  # 微分系数
+    
+    # PID控制变量
+    angle_error_integral = 0
+    last_angle_error = 0
+    
+    # 偏移量累计 - 用于检测持续偏移
+    y_offset_accumulator = 0
+    
+    # 动态调整参数
+    slow_down_ratio = 0.85  # 当移动到目标距离的85%时开始减速
+    completion_threshold = 0.95  # 当移动到目标距离的95%时停止
+    
+    # 监控移动过程
+    while distance_moved < abs_distance * completion_threshold and time.time() - start_time < timeout:
+        current_time = time.time()
+        
+        # 按固定间隔检查位置
+        if current_time - last_check_time >= position_check_interval:
+            # 获取当前位置和朝向
+            current_position = ctrl.odo_msg.xyz
+            current_yaw = ctrl.odo_msg.rpy[2]
+            
+            # 计算在移动方向上的位移
+            dx = current_position[0] - start_position[0]
+            dy = current_position[1] - start_position[1]
+            
+            # 计算在初始方向上的投影距离（实际前进距离）
+            distance_moved = dx * direction_vector[0] + dy * direction_vector[1]
+            distance_moved = abs(distance_moved)  # 确保距离为正值
+            
+            # 计算垂直于移动方向的偏移量（y方向偏移）
+            y_offset = -dx * direction_vector[1] + dy * direction_vector[0]
+            
+            # 累积y方向偏移量，检测持续偏移趋势
+            y_offset_accumulator = y_offset_accumulator * 0.8 + y_offset * 0.2
+            
+            # 根据前进或后退确定期望方向
+            expected_direction = start_yaw if forward else (start_yaw + math.pi) % (2 * math.pi)
+            
+            # 使用IMU朝向数据计算角度偏差
+            yaw_error = current_yaw - expected_direction
+            # 角度归一化
+            while yaw_error > math.pi:
+                yaw_error -= 2 * math.pi
+            while yaw_error < -math.pi:
+                yaw_error += 2 * math.pi
+            
+            # 使用PID控制计算角速度修正
+            # 比例项
+            p_control = kp_angle * yaw_error
+            
+            # 积分项 (带衰减)
+            angle_error_integral = angle_error_integral * 0.9 + yaw_error
+            angle_error_integral = max(-1.0, min(1.0, angle_error_integral))  # 限制积分范围
+            i_control = ki_angle * angle_error_integral
+            
+            # 微分项
+            d_control = kd_angle * (yaw_error - last_angle_error)
+            last_angle_error = yaw_error
+            
+            # 计算总的角速度控制量
+            angular_correction = -(p_control + i_control + d_control)
+            # 限制最大角速度修正
+            angular_correction = max(min(angular_correction, 0.3), -0.3)
+            
+            # 根据持续的y偏移趋势增加侧向校正
+            lateral_correction = 0
+            if abs(y_offset_accumulator) > 0.05:  # 如果累积偏移超过5厘米
+                lateral_correction = -y_offset_accumulator * 0.8  # 反向校正
+                lateral_correction = max(min(lateral_correction, 0.15), -0.15)  # 限制最大侧向速度
+                
+                if observe and abs(lateral_correction) > 0.05:
+                    warning(f"累积Y偏移校正: {y_offset_accumulator:.3f}米，应用侧向速度 {lateral_correction:.3f}m/s", "偏移")
+            
+            # 计算完成比例
+            completion_ratio = distance_moved / abs_distance
+            
+            # 根据距离完成情况调整速度
+            if completion_ratio > slow_down_ratio:
+                # 计算减速系数
+                slow_factor = 1.0 - (completion_ratio - slow_down_ratio) / (1.0 - slow_down_ratio)
+                # 确保不会减速太多
+                slow_factor = max(0.3, slow_factor)
+                new_speed = actual_speed * slow_factor
+                
+                if observe and abs(new_speed - msg.vel_des[0]) > 0.05:
+                    info(f"减速: {msg.vel_des[0]:.2f} -> {new_speed:.2f} 米/秒 (完成: {completion_ratio*100:.1f}%)", "移动")
+                
+                actual_speed = new_speed
+            
+            # 应用修正 - 同时应用角速度和侧向速度修正
+            msg.vel_des = [actual_speed, lateral_correction, angular_correction]
+            msg.life_count += 1
+            ctrl.Send_cmd(msg)
+            
+            if observe and current_time % 1.0 < position_check_interval:
+                debug(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)", "距离")
+                debug(f"Y偏移: {y_offset:.3f}米, 累积偏移: {y_offset_accumulator:.3f}米", "偏移")
+                debug(f"朝向偏差: {math.degrees(yaw_error):.1f}度, 角速度修正: {angular_correction:.3f}rad/s", "角度")
+                debug(f"PID: P={p_control:.3f}, I={i_control:.3f}, D={d_control:.3f}", "控制")
+            
+            # 更新检查时间
+            last_check_time = current_time
+        
+        # 定期进行视觉检查和修正
+        if hasattr(ctrl, 'image_processor') and current_time - last_vision_check >= vision_check_interval:
+            try:
+                # 获取当前相机帧
+                frame = ctrl.image_processor.get_current_image()
+                if frame is not None:
+                    # 检测轨道线 - 使用特定的石板路模式
+                    center_info, _, _ = detect_dual_track_lines(
+                        frame, observe=False, save_log=False, stone_path_mode=True)
+                    
+                    # 如果成功检测到轨道线，使用它进行修正
+                    if center_info is not None:
+                        # 获取当前偏差
+                        current_deviation = center_info["deviation"]
+                        last_valid_deviation = current_deviation
+                        
+                        # 添加到队列进行滤波
+                        deviation_queue.append(current_deviation)
+                        if len(deviation_queue) > filter_size:
+                            deviation_queue.pop(0)
+                        
+                        # 计算滤波后的偏差值 (去除最大和最小值后的平均)
+                        if len(deviation_queue) >= 3:
+                            filtered_deviations = sorted(deviation_queue)[1:-1] if len(deviation_queue) > 2 else deviation_queue
+                            filtered_deviation = sum(filtered_deviations) / len(filtered_deviations)
+                        else:
+                            filtered_deviation = current_deviation
+                        
+                        # 计算视觉侧向修正速度
+                        vision_lateral_correction = -filtered_deviation * vision_correction_gain
+                        # 限制最大侧向速度修正
+                        vision_lateral_correction = max(min(vision_lateral_correction, 0.2), -0.2)
+                        
+                        # 与当前的侧向校正进行融合 (加权平均)
+                        if msg.vel_des[1] != 0:
+                            # 如果已经有侧向校正，与视觉校正进行融合
+                            fused_lateral = msg.vel_des[1] * 0.3 + vision_lateral_correction * 0.7
+                        else:
+                            # 如果没有侧向校正，直接使用视觉校正
+                            fused_lateral = vision_lateral_correction
+                        
+                        if observe and abs(vision_lateral_correction) > 0.05:
+                            warning(f"视觉修正: 偏差 {filtered_deviation:.1f}像素，应用侧向速度 {vision_lateral_correction:.3f}m/s", "视觉")
+                        
+                        # 应用视觉修正，保留当前前进速度和角速度
+                        msg.vel_des = [msg.vel_des[0], fused_lateral, msg.vel_des[2]]
+                        msg.life_count += 1
+                        ctrl.Send_cmd(msg)
+                        
+                        if observe and current_time % 1.0 < vision_check_interval:
+                            debug(f"视觉检测: 原始偏差 {current_deviation:.1f}像素, 滤波后 {filtered_deviation:.1f}像素", "视觉")
+                            debug(f"融合侧向速度: {fused_lateral:.3f}m/s", "视觉")
+            except Exception as e:
+                if observe:
+                    error(f"视觉检测异常: {e}", "错误")
+            
+            # 更新视觉检查时间
+            last_vision_check = current_time
+        
+        # 短暂延时
+        time.sleep(0.01)
+    
+    # 平滑停止
+    slowdown_steps = 5
+    for i in range(slowdown_steps, 0, -1):
+        slowdown_factor = i / slowdown_steps
+        msg.vel_des = [actual_speed * slowdown_factor, 0, 0]
+        msg.life_count += 1
+        ctrl.Send_cmd(msg)
+        time.sleep(0.1)
+    
+    # 最后完全停止
+    if hasattr(ctrl.base_msg, 'stop_smooth'):
+        ctrl.base_msg.stop_smooth()
+    else:
+        ctrl.base_msg.stop()
+    
+    # 获取最终位置和实际移动距离
+    final_position = ctrl.odo_msg.xyz
+    dx = final_position[0] - start_position[0]
+    dy = final_position[1] - start_position[1]
+    actual_distance = math.sqrt(dx*dx + dy*dy)
+    
+    # 计算最终y方向偏移
+    final_y_offset = -dx * direction_vector[1] + dy * direction_vector[0]
+    
+    if observe:
+        success(f"石板路增强直线移动完成，实际移动距离: {actual_distance:.3f}米", "完成")
+        info(f"最终Y方向偏移: {final_y_offset:.3f}米", "偏移")
+    
+    # 判断是否成功完成
+    distance_error = abs(actual_distance - abs_distance)
+    y_offset_error = abs(final_y_offset)
+    
+    go_success = distance_error < 0.1 and y_offset_error < 0.1  # 距离误差和y偏移都小于10厘米视为成功
+    
+    if observe:
+        if go_success:
+            success(f"移动成功", "成功")
+        else:
+            warning(f"移动不够精确，距离误差: {distance_error:.3f}米, Y偏移: {y_offset_error:.3f}米", "警告")
+    
+    return go_success