mi-task/base_move/move_base_hori_line.py

import math
import time
import cv2
import numpy as np

import sys
import os

sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from utils.detect_track import (
    detect_horizontal_track_edge, detect_horizontal_track_edge_v2,
    detect_dual_track_lines, detect_left_side_track
)
from base_move.turn_degree import turn_degree
from base_move.go_straight import go_straight
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

# 创建此模块特定的日志记录器
logger = get_logger("横线移动")

def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3, detect_func_version=1):
    """
    控制机器人旋转到横向线水平的位置
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    observe: 是否输出中间状态信息和可视化结果，默认为False
    max_attempts: 最大尝试次数，默认为3次
    
    返回:
    bool: 是否成功校准
    """
    attempts = 0
    aligned = False
    image = ctrl.image_processor.get_current_image()
    accumulated_angle = 0
    max_total_rotation = 45
    angle_accuracy_threshold = 2.0
    
    # 记录初始位置
    initial_yaw = ctrl.odo_msg.rpy[2]
    
    while attempts < max_attempts and not aligned:
        section(f"尝试校准横线 {attempts+1}/{max_attempts}", "校准")
        
        # 检测横向线边缘; 不同版本
        if detect_func_version == 1:
            edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
        elif detect_func_version == 2:
            edge_point, edge_info = detect_horizontal_track_edge_v2(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
        else:
            error("未指定检测版本，请检查参数", "失败")
            return False

        print('😺', edge_info)

        if edge_point is None or edge_info is None:
            error("未检测到横向线，无法进行校准", "失败")
            
            # 尝试小幅度摇头寻找横线
            # TODO 改成上下低头；
            if attempts < max_attempts - 1:
                small_angle = 5 * (1 if attempts % 2 == 0 else -1)
                info(f"尝试摇头 {small_angle}度 寻找横线", "校准")
                turn_degree(ctrl, msg, small_angle, absolute=False)
                time.sleep(0.5)
            
            attempts += 1
            continue
        
        slope = edge_info["slope"]
        is_horizontal = edge_info["is_horizontal"]
        
        if observe:
            debug(f"检测到横向线，斜率: {slope:.6f}", "检测")
            info(f"是否足够水平: {is_horizontal}", "检测")
        
        if is_horizontal:
            success("横向线已经水平，无需校准", "完成")
            return True
        
        # 计算需要旋转的角度
        angle_rad = math.atan(slope)
        angle_deg = math.degrees(angle_rad)
        angle_to_rotate = -angle_deg  # 取负值使旋转方向正确
        
        # 设置最小旋转阈值
        if abs(angle_to_rotate) < 0.5:  # 降低最小旋转阈值
            angle_to_rotate = math.copysign(0.5, angle_to_rotate)
        
        # 检查累积旋转
        if abs(accumulated_angle + angle_to_rotate) > max_total_rotation:
            warning(f"累积旋转角度({accumulated_angle + angle_to_rotate:.2f}°)过大，限制旋转", "警告")
            angle_to_rotate = angle_to_rotate / 2
        
        if observe:
            info(f"计算旋转角度: {angle_to_rotate:.2f}度", "角度")
            info(f"当前累积旋转: {accumulated_angle:.2f}度", "累积")
        
        # 使用turn_degree函数执行旋转，增加精度参数
        info(f"旋转角度: {angle_to_rotate:.2f}度", "旋转")
        turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False, precision=True)
        info(f"旋转结果: {turn_success}", "旋转结果")
        
        # 等待稳定
        time.sleep(0.3)
        
        # 计算实际旋转角度
        current_yaw = ctrl.odo_msg.rpy[2]
        actual_rotation = math.degrees(current_yaw - initial_yaw - math.radians(accumulated_angle))
        
        # 规范化角度
        while actual_rotation > 180:
            actual_rotation -= 360
        while actual_rotation < -180:
            actual_rotation += 360
            
        accumulated_angle += actual_rotation
        
        if observe:
            info(f"请求旋转: {angle_to_rotate:.2f}度, 实际旋转: {actual_rotation:.2f}度", "旋转")
            info(f"旋转结果: {'成功' if turn_success else '失败'}", "成功" if turn_success else "失败")
        
        attempts += 1
        
        # 检查校准结果
        edge_point_after, edge_info_after = detect_horizontal_track_edge(
            ctrl.image_processor.get_current_image(), 
            observe=observe, 
            delay=1000 if observe else 0,
            save_log=True
        )
        
        if edge_info_after and "slope" in edge_info_after:
            current_slope = edge_info_after["slope"]
            current_angle_deg = math.degrees(math.atan(current_slope))
            
            if observe:
                info(f"校准后斜率: {current_slope:.6f}, 角度: {current_angle_deg:.2f}度", "检测")
            
            if abs(current_angle_deg) < angle_accuracy_threshold:
                success(f"校准成功，当前角度: {current_angle_deg:.2f}度", "完成")
                aligned = True
            else:
                warning(f"校准后角度仍超出阈值: {abs(current_angle_deg):.2f}° > {angle_accuracy_threshold}°", "警告")
        
        # 最后一次尝试的妥协处理
        if attempts == max_attempts and not aligned and edge_info_after:
            current_slope = edge_info_after["slope"]
            current_angle_deg = math.degrees(math.atan(current_slope))
            
            if abs(current_angle_deg) < angle_accuracy_threshold * 1.5:
                warning(f"达到最大尝试次数，接受近似结果，当前角度: {current_angle_deg:.2f}度", "妥协")
                aligned = True
    
    # 恢复最佳状态的处理
    if not aligned and abs(accumulated_angle) > 15:
        warning(f"校准失败，累积旋转{accumulated_angle:.2f}度，尝试恢复到最佳状态", "恢复")
        
        best_edge_point, best_edge_info = detect_horizontal_track_edge(
            ctrl.image_processor.get_current_image(), 
            observe=observe,
            save_log=True
        )
        
        if best_edge_info and "slope" in best_edge_info:
            best_slope = best_edge_info["slope"]
            best_angle_deg = math.degrees(math.atan(best_slope))
            
            if abs(best_angle_deg) < angle_accuracy_threshold * 1.5:
                info(f"当前状态已经接近水平，角度: {best_angle_deg:.2f}度", "保持")
                return True
    
    return aligned

def calculate_distance_to_line(edge_info, camera_height, camera_tilt_angle_deg=0, observe=False):
    """
    根据相机参数和图像中横线位置计算相机到横线的实际距离
    
    几何模型说明:
    1. 相机位于高度camera_height处，向下倾斜camera_tilt_angle_deg度
    2. 图像底部对应相机视场的下边缘，横线在图像中的位置通过像素坐标确定
    3. 计算相机视线到横线的角度，然后使用三角函数计算实际距离
    
    参数:
    edge_info: 边缘信息字典，包含distance_to_bottom等信息
    camera_height: 相机高度(米)
    camera_tilt_angle_deg: 相机向下倾斜的角度（度）
    observe: 是否打印中间计算值
    
    返回:
    float: 到横向线的X轴水平距离(米)
    """
    if edge_info is None or "distance_to_bottom" not in edge_info:
        return None
    
    # 1. 获取图像中交点到底部的距离(像素)
    distance_in_pixels = edge_info["distance_to_bottom"]
    if observe:
        debug(f"图像中交点到底部的像素距离: {distance_in_pixels}", "距离")
    
    # 2. 获取相机参数
    horizontal_fov_rad = 1.46608  # 水平视场角（弧度）约84度
    image_height_px = 1080       # 图像高度（像素）
    image_width_px = 1920        # 图像宽度（像素）
    
    # 3. 计算垂直视场角
    aspect_ratio = image_width_px / image_height_px  # 宽高比
    vertical_fov_rad = horizontal_fov_rad / aspect_ratio  # 垂直视场角(弧度)
    vertical_fov_deg = math.degrees(vertical_fov_rad)  # 垂直视场角(度)
    
    if observe:
        debug(f"相机参数: 水平FOV={math.degrees(horizontal_fov_rad):.1f}°, 垂直FOV={vertical_fov_deg:.1f}°", "计算")
        debug(f"图像尺寸: {image_width_px}x{image_height_px}, 宽高比: {aspect_ratio:.2f}", "计算")
    
    # 4. 直接计算视线角度
    
    # 计算图像底部到相机视场中心的角度
    half_vfov_rad = vertical_fov_rad / 2
    
    # 计算图像底部到横线的角度比例
    # 比例 = 底部到横线的像素距离 / 图像总高度
    pixel_ratio = distance_in_pixels / image_height_px
    
    # 计算从图像底部到横线的角度
    bottom_to_line_angle_rad = pixel_ratio * vertical_fov_rad
    
    # 计算从相机视场中心到横线的角度
    # 负值表示横线在视场中心以下，正值表示在中心以上
    center_to_line_angle_rad = bottom_to_line_angle_rad - half_vfov_rad
    
    # 考虑相机倾斜角度
    # 相机向下倾斜为正值，此时视场中心相对水平线向下
    camera_tilt_rad = math.radians(camera_tilt_angle_deg)
    
    # 计算横线相对于水平面的视线角度
    # 负值表示视线向下看到横线，正值表示视线向上看到横线
    view_angle_rad = center_to_line_angle_rad - camera_tilt_rad
    
    if observe:
        debug("视场角度关系:", "计算")
        debug(f"  - 图像底部到横线角度: {math.degrees(bottom_to_line_angle_rad):.2f}°", "角度")
        debug(f"  - 视场中心到横线角度: {math.degrees(center_to_line_angle_rad):.2f}°", "角度")
        debug(f"  - 相机倾斜角度: {camera_tilt_angle_deg}°", "角度")
        debug(f"  - 最终视线角度: {math.degrees(view_angle_rad):.2f}° ({'向下' if view_angle_rad < 0 else '向上'})", "角度")
    
    # 5. 防止除零错误或异常值
    # 确保视线角度不接近于0(水平视线无法确定地面交点)
    min_angle_rad = 0.01  # 约0.57度
    if abs(view_angle_rad) < min_angle_rad:
        if observe:
            warning(f"视线角度过小({math.degrees(view_angle_rad):.2f}°)，使用最小角度: {math.degrees(min_angle_rad):.2f}°", "警告")
        view_angle_rad = -min_angle_rad  # 设为向下的最小角度
    
    # 6. 计算水平距离
    # 仅当视线向下时计算地面距离
    if view_angle_rad < 0:  # 视线向下
        # 基本几何关系: 水平距离 = 高度 / tan(视线向下的角度)
        # 注意角度为负，所以需要取负
        ground_distance = camera_height / math.tan(-view_angle_rad)
        
        if observe:
            info(f"计算公式: 距离 = 相机高度({camera_height}米) / tan(|视线角度|({abs(math.degrees(view_angle_rad)):.2f}°))", "计算")
            info(f"计算结果: 距离 = {ground_distance:.3f}米", "距离")
    else:  # 视线平行或向上，无法确定地面交点
        if observe:
            warning(f"视线向上或水平，无法计算地面距离", "警告")
        return 0.5  # 返回一个默认值
    
    # 7. 应用校正和限制
    # 可选的校正因子(通过实验校准)
    correction_factor = 1.0
    distance = ground_distance * correction_factor
    
    # 设置合理的范围限制
    min_distance = 0.1  # 最小距离(米)
    
    # 限制结果在合理范围内
    final_distance = max(min_distance, distance)
    
    if observe and final_distance != distance:
        warning(f"应用范围限制: 原始距离 {distance:.3f}米 -> 最终距离 {final_distance:.3f}米", "警告")
    elif observe:
        info(f"最终距离: {final_distance:.3f}米", "距离")
    
    return final_distance

def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False):
    """
    根据横向线位置，控制机器人移动到指定距离
    # INFO 本质就是封装了一下 go-straight，但是需要先校准到水平
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    distance: 目标位置与横向线的距离(米)，默认为0.5米
    speed: 移动速度(米/秒)，默认为0.5米/秒
    observe: 是否输出中间状态信息和可视化结果，默认为False
    """
    # 首先校准到水平
    section("校准到横向线水平", "校准")
    aligned = align_to_horizontal_line(ctrl, msg, detect_func_version=2, observe=observe)
    
    if not aligned:
        error("无法校准到横向线水平，停止移动", "停止")
        return False
    else:
        success("校准完成", "完成")
    
    go_straight(ctrl, msg, distance=distance, speed=speed, observe=observe)

def move_to_hori_line(ctrl, msg, target_distance=0.5, absolute_deg=200, observe=False):
    """
    控制机器人校准并移动到横向线前的指定距离
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    target_distance: 目标位置与横向线的距离(米)，默认为0.5米
    observe: 是否输出中间状态信息和可视化结果，默认为False
    
    返回:
    bool: 是否成功到达目标位置
    """
    # 首先校准到水平
    section("校准到横向线水平", "校准")
    if absolute_deg < 200:
        aligned = turn_degree(ctrl, msg, absolute_deg, absolute=True)
    else:
        aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
    
    if not aligned:
        error("无法校准到横向线水平，停止移动", "停止")
        return False
    
    # 检测横向线
    image = ctrl.image_processor.get_current_image()
    edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
    
    if edge_point is None or edge_info is None:
        error("无法检测到横向线，停止移动", "停止")
        return False
    
    # 获取相机高度
    camera_height = 0.355  # 单位: 米 # INFO from TF-tree
    
    # 计算当前距离
    current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
    
    if current_distance is None:
        error("无法计算到横向线的距离，停止移动", "停止")
        return False
    
    if observe:
        info(f"当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米", "距离")
    
    # 计算需要移动的距离
    distance_to_move = current_distance - target_distance
    
    if abs(distance_to_move) < 0.05:  # 如果已经很接近目标距离
        success("已经达到目标距离，无需移动", "完成")
        return True
    
    # 设置移动命令
    msg.mode = 11  # Locomotion模式
    msg.gait_id = 26  # 自变频步态
    
    # 移动方向设置
    forward = distance_to_move > 0  # 判断是前进还是后退
    
    # 设置移动速度
    # 根据需要移动的距离动态调整移动速度
    if abs(distance_to_move) > 1.0:
        move_speed = 1.0  # 距离较远时用最大速度
    elif abs(distance_to_move) > 0.5:
        move_speed = 0.6  # 中等距离用中等速度
    elif abs(distance_to_move) > 0.2:
        move_speed = 0.3  # 较近距离用较慢速度
    else:
        move_speed = 0.15  # 非常接近时用最慢速度
    if forward:
        msg.vel_des = [move_speed, 0, 0]  # 设置前进速度
    else:
        msg.vel_des = [-move_speed, 0, 0]  # 设置后退速度
    
    # 获取起始位置
    start_position = list(ctrl.odo_msg.xyz)  # 转换为列表，因为xyz是元组
    
    if observe:
        debug(f"起始位置: {start_position}", "位置")
        info(f"开始移动: {'前进' if forward else '后退'} {abs(distance_to_move):.3f}米", "移动")
        # 在起点放置绿色标记
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True)
    
    # 估算移动时间，但实际上会通过里程计控制
    move_time = abs(distance_to_move) / move_speed
    msg.duration = 0  # wait next cmd
    msg.step_height = [0.06, 0.06]  # 抬腿高度
    msg.life_count += 1
    
    # 发送命令
    ctrl.Send_cmd(msg)
    
    # 使用里程计进行实时监控移动距离
    distance_moved = 0
    start_time = time.time()
    timeout = move_time + 2  # 增加超时时间为预计移动时间加2秒
    
    # 监控移动距离，增加移动距离的系数
    while distance_moved < abs(distance_to_move) * 1.05 and time.time() - start_time < timeout:
        # 计算已移动距离
        current_position = ctrl.odo_msg.xyz
        dx = current_position[0] - start_position[0]
        dy = current_position[1] - start_position[1]
        distance_moved = math.sqrt(dx*dx + dy*dy)
        
        if observe and time.time() % 0.5 < 0.02:  # 每0.5秒左右打印一次
            debug(f"已移动: {distance_moved:.3f}米, 目标: {abs(distance_to_move):.3f}米", "移动")
            
        time.sleep(0.05)  # 小间隔检查位置
    
    # 使用平滑停止
    # ctrl.base_msg.stop_smooth()
    ctrl.base_msg.stop()
    
    if observe:
        success(f"移动完成，平稳停止，通过里程计计算的移动距离: {distance_moved:.3f}米", "完成")
        # 在终点放置红色标记
        end_position = list(ctrl.odo_msg.xyz)
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True)
    
    # 放宽判断标准
    return abs(distance_moved - abs(distance_to_move)) < 0.15  # 如果误差小于15厘米，则认为成功

def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2, 
                              pass_align=False,
                              radius=None,
                              min_radius=0.1,
                              max_radius=5,
                              scan_qrcode=False,
                              qr_check_interval=0.3, 
                              observe=False,
                              bad_big_angle_corret=False,
                              no_end_reset=False,):
    """
    对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
    参数:
        ctrl: Robot_Ctrl对象
        msg: robot_control_cmd_lcmt对象
        target_distance: 横线前目标保持距离，默认0.5米
        angle_deg: 旋转角度，正值为左转，负值为右转
        pass_align: 是否跳过对准步骤，默认为False
        observe: 是否打印调试信息
        scan_qrcode: 是否在旋转过程中扫描QR码，默认为False
        qr_check_interval: QR码检查间隔时间(秒)，默认为0.3秒

        bad_big_angle_corret: 是否对大角度错误进行修正，默认为False # TODO clear
        no_end_reset: 是否在旋转完成后不进行位置重置，默认为False
    返回:
        bool或元组: 如果scan_qrcode为False，返回bool表示是否成功完成操作；
                  如果scan_qrcode为True，返回(bool, str)元组，表示(是否成功完成操作, QR码扫描结果)
    """

    # 返回此任务的中间状态
    res = {}

    # 启动异步QR码扫描（如果需要）
    qr_result = None
    if scan_qrcode:
        # 确保image_processor存在
        if not hasattr(ctrl, 'image_processor') or ctrl.image_processor is None:
            warning("无法启用QR码扫描，image_processor不存在", "警告")
            scan_qrcode = False
        else:
            # 启动异步扫描
            try:
                ctrl.image_processor.start_async_scan(interval=0.2)
                info("已启动异步QR码扫描", "扫描")
            except Exception as e:
                error(f"启动QR码扫描失败: {e}", "失败")
                scan_qrcode = False
    
    # 1. 对准横线
    if not pass_align:
        section("校准到横向线水平", "校准")
        aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
        if not aligned:
            error("无法校准到横向线水平，停止动作", "停止")
            if scan_qrcode:
                ctrl.image_processor.stop_async_scan()
                return False, res
            return False, res
    else:
        info("跳过对准步骤", "信息")

    # 2. 检测横线并计算距离
    r = radius
    if r is None:
        image = ctrl.image_processor.get_current_image()
        edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe)
        if edge_point is None or edge_info is None:
            error("无法检测到横向线，停止动作", "停止")
            if scan_qrcode:
                ctrl.image_processor.stop_async_scan()
                return False, res
            return False, res

        camera_height = 0.355  # 单位: 米
        r = calculate_distance_to_line(edge_info, camera_height, observe=observe)
        if r is None:
            error("无法计算到横向线的距离，停止动作", "停止")
            if scan_qrcode:
                ctrl.image_processor.stop_async_scan()
                return False, res
            return False, res
        
        # 减去目标距离
        r -= target_distance
    res['radius'] = r

    if observe:
        info(f"当前距离: {r:.3f}米", "距离")

    # 3. 计算圆弧运动参数
    # 根据角度大小调整时间补偿系数
    abs_angle_deg = abs(angle_deg)
    if abs_angle_deg <= 70:
        time_compensation = 0.78  # 对70度及以下角度使用更小的补偿系数
    elif abs_angle_deg <= 90:
        time_compensation = 0.85  # 90度左右使用稍大的系数
    else:
        # 对180度旋转，增加补偿系数，使旋转时间更长
        time_compensation = 1.4  # 增加40%的时间
        
    # 调整实际目标角度，针对不同角度应用不同的缩放比例
    actual_angle_deg = angle_deg
    if abs_angle_deg <= 70:
        actual_angle_deg = angle_deg * 0.85  # 70度及以下角度减少到85%
    elif abs_angle_deg <= 90:
        actual_angle_deg = angle_deg * 0.9  # 90度减少到90%
    elif abs_angle_deg >= 170:  # 处理大角度旋转，特别是180度情况
        actual_angle_deg = angle_deg  # 对于180度旋转，使用完整角度，不进行缩放
    
    if observe and actual_angle_deg != angle_deg:
        info(f"应用角度补偿: 目标{angle_deg}度 -> 实际指令{actual_angle_deg:.1f}度", "角度")
 
    angle_rad = math.radians(actual_angle_deg)
    
    # 设定角速度（rad/s），角度的符号决定了转向方向
    base_w = 0.8
    w = math.copysign(base_w, angle_deg)  # 保持角速度与角度同号
    
    # 确保r有一个最小值，避免速度过低
    effective_r = min(max_radius, max(r, min_radius))
    
    # 线速度，必须与角速度和半径的关系一致：v = w * r
    # 这样才能保证走圆弧
    v = abs(w * effective_r)  # 线速度，正负号与角速度方向一致
    
    info(f"开始移动圆弧，半径: {effective_r:.3f}米, 线速度: {v:.3f}m/s, 角速度: {w:.3f}rad/s")
    if observe:
        if effective_r != r:
            warning(f"注意: 实际半径 {r:.3f}米，使用半径 {effective_r:.3f}米 计算速度", "警告")
    
    t = abs(angle_rad / w)  # 运动时间，取绝对值保证为正
    
    # 应用时间补偿系数
    t *= time_compensation
    
    if observe:
        print(f"圆弧半径: {effective_r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s")
        print(f"理论运动时间: {abs(angle_rad / w):.2f}s, 应用补偿系数{time_compensation}后: {t:.2f}s")

    # 4. 发送圆弧运动指令
    msg.mode = 11
    msg.gait_id = 26
    msg.vel_des = [v, 0, w]  # [前进速度, 侧向速度, 角速度]
    msg.duration = 0  # wait next
    msg.step_height = [0.06, 0.06]
    msg.life_count += 1

    ctrl.Send_cmd(msg)

    # 记录起始角度和位置
    start_yaw = ctrl.odo_msg.rpy[2]
    start_position = list(ctrl.odo_msg.xyz)
    
    # DEBUG 在起点放置蓝色标记
    if observe:
        yaw = ctrl.odo_msg.rpy[2]
        ctrl.place_marker(start_position[0], start_position[1], 
                        start_position[2] if len(start_position) > 2 else 0.0, 
                        'blue', observe=True)
        info(f"在起点位置放置蓝色标记: {start_position}", "位置")
    
        # 计算圆心位置 - 相对于机器人前进方向的左侧或右侧
        # 圆心与机器人当前位置的距离就是圆弧半径
        # 角度为正(左转)时，圆心在左侧；角度为负(右转)时，圆心在右侧
        circle_side = 1 if angle_deg > 0 else -1
        center_x = start_position[0] - effective_r * math.sin(yaw) * circle_side
        center_y = start_position[1] + effective_r * math.cos(yaw) * circle_side
        center_z = start_position[2] if len(start_position) > 2 else 0.0
    
        # 在圆心放置绿色标记
        ctrl.place_marker(center_x, center_y, center_z, 'green', observe=True)
        info(f"在旋转圆心放置绿色标记: [{center_x:.3f}, {center_y:.3f}, {center_z:.3f}]", "位置")
    
        # 计算目标点的位置 - 旋转后的位置
        # 使用几何关系：目标点是从起点绕圆心旋转angle_rad弧度后的位置
        target_angle = yaw + angle_rad  # 直接使用angle_rad的正负确定方向
        # 计算从圆心到目标点的向量
        target_x = center_x + effective_r * math.sin(target_angle) * circle_side
        target_y = center_y - effective_r * math.cos(target_angle) * circle_side
        target_z = start_position[2] if len(start_position) > 2 else 0.0
        
        # 在目标点放置红色标记
        ctrl.place_marker(target_x, target_y, target_z, 'red', observe=True)
        info(f"在目标位置放置红色标记: [{target_x:.3f}, {target_y:.3f}, {target_z:.3f}]", "位置")

    # 进入循环，实时判断
    distance_moved = 0
    angle_turned = 0
    start_time = time.time()
    timeout = t + 2  # 给个超时保护
    last_qr_check_time = 0

    # 初始速度
    current_v = v
    current_w = w
    
    # 对小角度提前开始减速
    if abs_angle_deg <= 70:
        slowdown_threshold = 0.65  # 当达到目标角度的65%时开始减速
    elif abs_angle_deg >= 170:  # 针对大角度(如180度)采用更晚的减速时机
        slowdown_threshold = 0.7  # 当达到目标角度的70%时才开始减速
    else:
        slowdown_threshold = 0.75  # 对于大角度，75%时开始减速（原来是80%）
    
    # 根据角度大小调整旋转停止阈值
    if abs_angle_deg >= 170:
        rotation_completion_threshold = 0.85  # 大角度旋转提前停止，避免惯性导致过度旋转
    else:
        rotation_completion_threshold = 0.95  # 默认旋转到95%时停止
    
    # 监控旋转进度并自行控制减速
    # 对于180度旋转使用特殊处理
    if abs_angle_deg >= 170:
        # 简化旋转逻辑，直接持续旋转一段时间
        rotation_time = t  # 使用计算的理论时间
        if observe:
            info(f"大角度旋转({abs_angle_deg}度)，将持续旋转 {rotation_time:.2f} 秒", "旋转")
            
        # 发送固定时间的旋转命令
        start_time = time.time()
        
        # 前段使用全速，但减少前段旋转时间比例，减少过度旋转
        first_segment_time = rotation_time * 0.65  # 原来是0.7，减少到0.65
        elapsed_time = 0
        
        while elapsed_time < first_segment_time and time.time() - start_time < rotation_time + 2:  # 加2秒保护
            elapsed_time = time.time() - start_time
            
            # 计算已旋转角度（仅用于打印）
            if observe and time.time() % 0.5 < 0.02:
                current_yaw = ctrl.odo_msg.rpy[2]
                angle_turned = current_yaw - start_yaw
                # 角度归一化处理
                while angle_turned > math.pi:
                    angle_turned -= 2 * math.pi
                while angle_turned < -math.pi:
                    angle_turned += 2 * math.pi
                debug(f"全速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 已用时间: {elapsed_time:.2f}s/{rotation_time:.2f}s", "旋转")
            
            # 检查QR码扫描结果（如果启用）
            if scan_qrcode:
                current_time = time.time()
                if current_time - last_qr_check_time >= qr_check_interval:
                    qr_data, scan_time = ctrl.image_processor.get_last_qr_result()
                    if qr_data and scan_time > start_time:
                        qr_result = qr_data
                        success(f"在旋转过程中扫描到QR码: {qr_data}", "扫描")
                    last_qr_check_time = current_time
            
            time.sleep(0.05)
        
        # 后段减速，减少减速幅度，保持更高速度
        if observe:
            info(f"进入减速阶段", "旋转")
        
        # 减速到70%，保持前进速度和角速度的比例关系
        reduced_w = w * 0.7  # 原来是0.5
        reduced_v = abs(reduced_w * effective_r)  # 维持圆弧关系
        
        msg.mode = 11
        msg.gait_id = 26
        msg.vel_des = [reduced_v, 0, reduced_w]  # 维持圆弧关系
        msg.duration = 0
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        
        # 继续旋转直到总时间结束，减少总旋转时间
        total_rotation_time = rotation_time * 1.15  # 原来是1.2，减少到1.15
        while time.time() - start_time < total_rotation_time:
            # 计算已旋转角度（仅用于打印）
            if observe and time.time() % 0.5 < 0.02:
                current_yaw = ctrl.odo_msg.rpy[2]
                angle_turned = current_yaw - start_yaw
                # 角度归一化处理
                while angle_turned > math.pi:
                    angle_turned -= 2 * math.pi
                while angle_turned < -math.pi:
                    angle_turned += 2 * math.pi
                debug(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 已用时间: {time.time() - start_time:.2f}s/{total_rotation_time:.2f}s", "旋转")
            
            # 检查QR码扫描结果（如果启用）
            if scan_qrcode:
                current_time = time.time()
                if current_time - last_qr_check_time >= qr_check_interval:
                    qr_data, scan_time = ctrl.image_processor.get_last_qr_result()
                    if qr_data and scan_time > start_time:
                        qr_result = qr_data
                        success(f"在旋转过程中扫描到QR码: {qr_data}", "扫描")
                    last_qr_check_time = current_time
            
            time.sleep(0.05)
        
        # 停止
        ctrl.base_msg.stop()
    else:
        # 非180度的常规旋转逻辑保持不变
        while abs(angle_turned) < abs(angle_rad) * rotation_completion_threshold and time.time() - start_time < timeout:
            # 计算已移动距离
            current_position = ctrl.odo_msg.xyz
            dx = current_position[0] - start_position[0]
            dy = current_position[1] - start_position[1]
            distance_moved = math.sqrt(dx*dx + dy*dy)
            
            # 计算已旋转角度
            current_yaw = ctrl.odo_msg.rpy[2]
            angle_turned = current_yaw - start_yaw
            # 角度归一化处理
            while angle_turned > math.pi:
                angle_turned -= 2 * math.pi
            while angle_turned < -math.pi:
                angle_turned += 2 * math.pi
            
            # 检查QR码扫描结果（如果启用）
            if scan_qrcode:
                current_time = time.time()
                if current_time - last_qr_check_time >= qr_check_interval:
                    qr_data, scan_time = ctrl.image_processor.get_last_qr_result()
                    if qr_data and scan_time > start_time:
                        qr_result = qr_data
                        print(f"🔍 在旋转过程中扫描到QR码: {qr_data}")
                    last_qr_check_time = current_time
            
            # 计算角度完成比例
            completion_ratio = abs(angle_turned) / abs(angle_rad)
            
            # 当完成一定比例时开始减速
            if completion_ratio > slowdown_threshold:
                # 实现多阶段减速，特别针对大角度旋转
                if abs_angle_deg >= 170:
                    if completion_ratio < 0.7:  # 第一阶段：60%-70%
                        speed_factor = 0.65  # 原来是0.7，减少到0.65
                    elif completion_ratio < 0.8:  # 第二阶段：70%-80%
                        speed_factor = 0.35  # 原来是0.4，减少到0.35
                    else:  # 第三阶段：80%以上
                        speed_factor = 0.15  # 原来是0.2，减少到0.15
                else:
                    # 标准减速逻辑保持不变
                    # 剩余比例作为速度系数
                    speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
                    
                    # 根据角度调整最小速度因子
                    min_speed_factor = 0.15  # 默认最小速度因子
                    # 确保速度系数不会太小，但可以更慢一些
                    speed_factor = max(min_speed_factor, speed_factor)
                
                # 应用减速
                current_v = v * speed_factor
                current_w = w * speed_factor
                
                # 更新速度命令
                msg.mode = 11
                msg.gait_id = 26
                msg.vel_des = [current_v, 0, current_w]
                msg.duration = 200  # 短时间更新
                msg.step_height = [0.06, 0.06]
                msg.life_count += 1
                ctrl.Send_cmd(msg)
                
                # 针对180度旋转，在减速命令后稍作等待，以确保减速效果
                if abs_angle_deg >= 170:
                    time.sleep(0.03)  # 短暂延迟，让减速命令更有效
                
                if observe and time.time() % 0.5 < 0.02:  # 每0.5秒左右打印一次
                    debug(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 当前角速度: {current_w:.3f}rad/s", "旋转")
            else:
                if observe and time.time() % 0.5 < 0.02:  # 每0.5秒左右打印一次
                    debug(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度", "旋转")
            
            time.sleep(0.05)

    if observe:
        info(f"旋转过程结束，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]", "停止")
        debug(f'旋转结束角度: {math.degrees(ctrl.odo_msg.rpy[2]):.2f}°', "角度")
    ctrl.base_msg.stop()
    if observe:
        debug(f'停止角度: {math.degrees(ctrl.odo_msg.rpy[2]):.2f}°', "角度")
    
    # 记录停止前的角度
    pre_stop_yaw = ctrl.odo_msg.rpy[2]
    
    # 等待机器人完全停止
    time.sleep(0.5)
    
    # 停下来后的最终角度
    final_yaw = ctrl.odo_msg.rpy[2]
    final_angle_turned = final_yaw - start_yaw
    
    # 记录实际结束位置
    end_position = list(ctrl.odo_msg.xyz)
    
    # 在实际结束位置放置黄色标记，与预计的目标位置进行对比
    if observe and hasattr(ctrl, 'place_marker'):
        ctrl.place_marker(end_position[0], end_position[1], 
                        end_position[2] if len(end_position) > 2 else 0.0, 
                        'yellow', observe=True)
        info(f"在实际结束位置放置黄色标记: {end_position}", "位置")
        
        # 计算实际位置与预期位置的偏差
        position_error = math.sqrt((end_position[0] - target_x)**2 + 
                                   (end_position[1] - target_y)**2)
        info(f"位置误差: {position_error:.3f}米", "距离")
    
    # 角度归一化处理
    while final_angle_turned > math.pi:
        final_angle_turned -= 2 * math.pi
    while final_angle_turned < -math.pi:
        final_angle_turned += 2 * math.pi
    
    final_angle_deg = math.degrees(final_angle_turned)

    # 这里直接使用原始目标角度
    if not bad_big_angle_corret:
        target_angle_deg = angle_deg
    else:
        target_angle_deg = -angle_deg
    
    if observe:
        # 输出停止过程中角度变化
        stop_angle_diff = math.degrees(final_yaw - pre_stop_yaw)
        while stop_angle_diff > 180:
            stop_angle_diff -= 360
        while stop_angle_diff < -180:
            stop_angle_diff += 360
        debug(f"停止过程中旋转了: {stop_angle_diff:.2f}度", "角度")
        info(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度", "角度")
    
    # 检查是否需要微调
    angle_error = abs(final_angle_deg - target_angle_deg)
    
    # 针对不同角度使用不同的微调阈值
    adjustment_threshold = 3.0  # 默认微调阈值
    if abs_angle_deg >= 170:
        adjustment_threshold = 10.0  # 大角度旋转使用更大的微调阈值，因为初步旋转已经确保大致方向正确
        
    if angle_error > adjustment_threshold and not no_end_reset:  # 如果误差超过阈值
        if observe:
            warning(f"角度误差: {angle_error:.2f}度，进行微调", "角度")
        
        # 计算需要微调的角度
        adjust_angle = target_angle_deg - final_angle_deg
        
        # 增加补偿系数，因为实际旋转常会超出理论计算值
        if abs_angle_deg <= 70:
            compensation_factor = 0.55  # 比之前更保守，从0.65降至0.55
        elif abs_angle_deg >= 170:  # 单独处理180度附近的大角度
            # 动态调整补偿因子，更保守处理
            if angle_error > 60:  # 如果误差超过60度
                compensation_factor = 0.4  # 从0.5降至0.4
            elif angle_error > 30:  # 如果误差超过30度
                compensation_factor = 0.35  # 从0.4降至0.35
            else:
                compensation_factor = 0.25  # 从0.3降至0.25，更保守
        else:
            compensation_factor = 0.6  # 从0.7降至0.6
            
        # 对于特别大的误差进行更保守的单次校准
        if abs_angle_deg >= 170 and angle_error > 30:
            # 大误差直接一步校准，使用更保守的补偿系数
            adjusted_compensation = compensation_factor * 0.85  # 在已有补偿基础上再降低15%
            adjust_angle = adjust_angle * adjusted_compensation
            
            if observe:
                info(f"误差较大({angle_error:.2f}度)，使用更积极的补偿系数{adjusted_compensation:.2f}进行一次性校准: {adjust_angle:.2f}度", "角度")
            
            # 使用turn_degree函数进行微调
            turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
            
            # 等待稳定
            time.sleep(1.0)
            
            # 再次检查剩余误差(仅用于显示)
            if observe:
                current_yaw = ctrl.odo_msg.rpy[2]
                current_angle_turned = current_yaw - start_yaw
                # 角度归一化处理
                while current_angle_turned > math.pi:
                    current_angle_turned -= 2 * math.pi
                while current_angle_turned < -math.pi:
                    current_angle_turned += 2 * math.pi
                
                current_angle_deg = math.degrees(current_angle_turned)
                remaining_error = target_angle_deg - current_angle_deg
                info(f"校准后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度", "角度")
        else:
            adjust_angle *= compensation_factor
            
            if observe:
                info(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度", "角度")
            
            # 使用turn_degree函数进行微调
            turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
            
            # 等待稳定(仅用于显示)
            if observe:
                time.sleep(0.8)
                current_yaw = ctrl.odo_msg.rpy[2]
                current_angle_turned = current_yaw - start_yaw
                # 角度归一化处理
                while current_angle_turned > math.pi:
                    current_angle_turned -= 2 * math.pi
                while current_angle_turned < -math.pi:
                    current_angle_turned += 2 * math.pi
                
                current_angle_deg = math.degrees(current_angle_turned)
                remaining_error = target_angle_deg - current_angle_deg
                info(f"校准后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度", "角度")
        
        if observe:
            info(f"微调结果: {'成功' if turn_success else '失败'}", "成功" if turn_success else "失败")
    
    # 移动完成后再检查一次QR码扫描结果
    if scan_qrcode:
        qr_data, scan_time = ctrl.image_processor.get_last_qr_result()
        if qr_data and (qr_result is None or scan_time > last_qr_check_time):
            qr_result = qr_data
            success(f"旋转完成后最终扫描到QR码: {qr_data}", "扫描")
        
        # 停止异步扫描
        ctrl.image_processor.stop_async_scan()
        
        # 将QR码结果添加到res字典中
        res['qr_result'] = qr_result
    
    if observe:
        success("圆弧转弯完成", "完成")

    # 统一返回结果格式
    return True, res

def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None, observe=False):
    """
    控制机器狗在双轨道线中间行走
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    speed: 前进速度(米/秒)，默认为0.5米/秒
    max_time: 最大行走时间(秒)，默认为30秒
    target_distance: 目标移动距离(米)，如果设置，则达到此距离后停止
    observe: 是否输出中间状态信息和可视化结果，默认为False
    
    返回:
    bool: 是否成功完成双轨道跟随
    """
    section("开始双轨道跟随", "轨道跟随")
    
    # 设置移动命令基本参数
    msg.mode = 11  # Locomotion模式
    msg.gait_id = 26  # 自变频步态
    msg.duration = 0  # wait next cmd
    msg.step_height = [0.06, 0.06]  # 抬腿高度
    
    # 记录起始时间
    start_time = time.time()
    
    # 记录起始位置
    start_position = list(ctrl.odo_msg.xyz)
    if observe:
        debug(f"起始位置: {start_position}", "位置")
        if target_distance:
            info(f"目标距离: {target_distance}米", "目标")
        # 在起点放置绿色标记
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True)
    
    # PID控制参数
    kp = 0.005  # 比例系数
    ki = 0.0002  # 积分系数
    kd = 0.001  # 微分系数
    
    # PID控制变量
    previous_error = 0
    integral = 0
    
    # 最大角速度限制
    max_angular_velocity = 0.5  # rad/s
    
    # 记录成功检测到双轨道的次数和总次数，用于计算检测成功率
    detection_success_count = 0
    detection_total_count = 0
    
    # 帧间滤波参数
    filter_size = 5  # 滤波队列大小
    deviation_queue = []  # 偏差值队列
    last_valid_deviation = 0  # 上一次有效的偏差值
    last_valid_center_info = None  # 上一次有效的中心信息
    
    # 开始跟随循环
    distance_moved = 0
    while time.time() - start_time < max_time:
        # 检查距离条件
        if target_distance and distance_moved >= target_distance:
            if observe:
                success(f"已达到目标距离 {target_distance}米，停止移动", "完成")
            break
        
        # 获取当前图像
        image = ctrl.image_processor.get_current_image()
        
        # 检测双轨道线
        detection_total_count += 1
        center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True)
        
        # 使用帧间滤波增强稳定性
        if center_info is not None:
            detection_success_count += 1
            
            # 保存有效的中心信息
            last_valid_center_info = center_info
            
            # 获取当前偏差
            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
            
            if observe and time.time() % 0.5 < 0.02:
                debug(f"原始偏差: {current_deviation}px, 滤波后: {filtered_deviation}px", "滤波")
            
            # 计算PID控制
            error = -filtered_deviation  # 负值表示需要向右转，正值表示需要向左转
            
            # 比例项
            p_control = kp * error
            
            # 积分项 (添加积分限制以防止积分饱和)
            integral += error
            integral = max(-1000, min(1000, integral))  # 限制积分范围
            i_control = ki * integral
            
            # 微分项
            derivative = error - previous_error
            d_control = kd * derivative
            previous_error = error
            
            # 计算总的角速度控制量
            angular_velocity = p_control + i_control + d_control
            
            # 限制角速度范围
            angular_velocity = max(-max_angular_velocity, min(max_angular_velocity, angular_velocity))
            
            # 动态调整前进速度 - 偏差越大，速度越慢
            adaptive_speed = speed
            if abs(filtered_deviation) > 100:  # 如果偏差较大
                # 根据偏差大小动态调整速度，最低降至60%
                deviation_factor = max(0.6, 1.0 - abs(filtered_deviation) / 1000.0)
                adaptive_speed = speed * deviation_factor
                if observe and time.time() % 0.5 < 0.02:
                    debug(f"偏差较大({filtered_deviation:.1f}px)，调整速度至 {adaptive_speed:.2f}m/s", "速度")
            
            if observe and time.time() % 0.5 < 0.02:
                debug(f"偏差: {filtered_deviation:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}, 角速度: {angular_velocity:.4f}", "控制")
            
            # 设置速度命令
            msg.vel_des = [adaptive_speed, 0, angular_velocity]
        else:
            warning("未检测到双轨道线", "警告")
            
            # 如果有之前的有效检测结果，使用上一次的控制值来平滑过渡
            if last_valid_center_info is not None and len(deviation_queue) > 0:
                # 使用上一次的有效偏差，但降低其影响（乘以衰减因子）
                decay_factor = 0.8
                decayed_deviation = last_valid_deviation * decay_factor
                
                # 计算PID控制
                error = -decayed_deviation
                
                # 更新PID控制值但不更新积分项
                p_control = kp * error
                i_control = ki * integral
                d_control = 0  # 不使用微分项以避免抖动
                
                # 计算总的角速度控制量，但降低其幅度
                angular_velocity = (p_control + i_control) * 0.7
                
                # 限制角速度范围
                angular_velocity = max(-max_angular_velocity/2, min(max_angular_velocity/2, angular_velocity))
                
                # 降低速度以增加安全性
                reduced_speed = speed * 0.8
                
                if observe:
                    warning(f"使用上一帧数据，偏差: {decayed_deviation:.1f}px, 角速度: {angular_velocity:.4f}, 速度: {reduced_speed:.2f}m/s", "恢复")
                
                # 设置速度命令
                msg.vel_des = [reduced_speed, 0, angular_velocity]
            else:
                # 如果连续多次无法检测到轨道，降低速度直线行走
                if detection_total_count > 5 and detection_success_count / detection_total_count < 0.3:
                    reduced_speed = speed * 0.6  # 降低至60%的速度
                    msg.vel_des = [reduced_speed, 0, 0]
                    if observe:
                        warning(f"检测成功率低，降低速度至 {reduced_speed:.2f}m/s", "警告")
                else:
                    # 如果刚开始无法检测，使用原速度直线行走
                    msg.vel_des = [speed, 0, 0]
        
        # 发送命令
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        
        # 短暂延时
        time.sleep(0.05)
        
        # 计算已移动距离
        current_position = ctrl.odo_msg.xyz
        dx = current_position[0] - start_position[0]
        dy = current_position[1] - start_position[1]
        distance_moved = math.sqrt(dx*dx + dy*dy)
        
        if observe and time.time() % 0.5 < 0.02:  # 每0.5秒左右打印一次
            info(f"已移动: {distance_moved:.3f}米", "移动")
            if target_distance:
                progress = (distance_moved / target_distance) * 100
                info(f"进度: {progress:.1f}%", "进度")
    
    # 平滑停止
    if observe:
        info("开始平滑停止", "停止")
    
    # 先降低速度再停止，实现平滑停止
    slowdown_steps = 5
    for i in range(slowdown_steps, 0, -1):
        slowdown_factor = i / slowdown_steps
        msg.vel_des = [speed * slowdown_factor, 0, 0]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(0.1)
    
    # 最后完全停止
    ctrl.base_msg.stop()
    
    # 计算最终移动距离
    final_position = ctrl.odo_msg.xyz
    dx = final_position[0] - start_position[0]
    dy = final_position[1] - start_position[1]
    final_distance = math.sqrt(dx*dx + dy*dy)
    
    if observe:
        # 在终点放置红色标记
        end_position = list(final_position)
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True)
        
        success(f"双轨道跟随完成，总移动距离: {final_distance:.3f}米", "完成")
        
        # 显示检测成功率
        if detection_total_count > 0:
            detection_rate = (detection_success_count / detection_total_count) * 100
            info(f"轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计")
    
    # 如果有目标距离，判断是否达到目标
    if target_distance:
        is_success = abs(final_distance - target_distance) < 0.2  # 误差在20cm内算成功
        if observe:
            if is_success:
                success(f"成功到达目标距离，误差: {abs(final_distance - target_distance):.2f}米", "成功")
            else:
                warning(f"未能精确到达目标距离，误差: {abs(final_distance - target_distance):.2f}米", "警告")
        return is_success
    
    return True

def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=30, observe=False):
    """
    控制机器狗向左侧移动并靠近左侧的黄色轨迹线
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    target_distance: 目标与左侧线的像素距离，默认为150像素（保持一定间距）
    speed: 移动速度(米/秒)，默认为0.3米/秒
    max_time: 最大执行时间(秒)，默认为30秒
    observe: 是否输出中间状态信息和可视化结果，默认为False
    
    返回:
    bool: 是否成功达到目标位置
    """
    section("开始左侧轨迹线跟随", "左侧跟踪")
    
    # 设置移动命令基本参数
    msg.mode = 11  # Locomotion模式
    msg.gait_id = 26  # 自变频步态
    msg.duration = 0  # wait next cmd
    msg.step_height = [0.06, 0.06]  # 抬腿高度
    
    # 记录起始时间
    start_time = time.time()
    
    # 记录起始位置
    start_position = list(ctrl.odo_msg.xyz)
    if observe:
        debug(f"起始位置: {start_position}", "位置")
        # 在起点放置绿色标记
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True)
    
    # PID控制参数 - 侧向移动的PID参数
    kp_side = 0.002  # 比例系数
    ki_side = 0.0001  # 积分系数
    kd_side = 0.0005  # 微分系数
    
    # 记录目标到达状态和稳定计数
    target_reached = False
    stable_count = 0
    required_stable_count = 10  # 需要连续稳定的检测次数
    
    # PID控制变量
    previous_error = 0
    integral = 0
    
    # 最大侧向速度限制
    max_side_velocity = 0.3  # m/s
    
    # 检测成功计数器
    detection_success_count = 0
    detection_total_count = 0
    
    # 保存上一次有效的检测结果，用于检测失败时的平滑过渡
    last_valid_track_info = None
    
    # 滤波队列
    filter_size = 5
    distance_queue = []
    
    # 开始跟踪循环
    while time.time() - start_time < max_time:
        # 获取当前图像
        image = ctrl.image_processor.get_current_image()
        
        # 检测左侧轨迹线
        detection_total_count += 1
        track_info, tracking_point = detect_left_side_track(image, observe=observe, delay=500 if observe else 0, save_log=True)
        
        if track_info is not None:
            detection_success_count += 1
            
            # 保存有效的轨迹信息
            last_valid_track_info = track_info
            
            # 获取当前与左侧线的距离
            current_distance = track_info["distance_to_left"]
            
            # 添加到滤波队列
            distance_queue.append(current_distance)
            if len(distance_queue) > filter_size:
                distance_queue.pop(0)
            
            # 计算滤波后的距离值 (去除最大和最小值后的平均)
            if len(distance_queue) >= 3:
                filtered_distances = sorted(distance_queue)[1:-1] if len(distance_queue) > 2 else distance_queue
                filtered_distance = sum(filtered_distances) / len(filtered_distances)
            else:
                filtered_distance = current_distance
            
            if observe and time.time() % 0.5 < 0.02:
                debug(f"原始左侧距离: {current_distance:.1f}px, 滤波后: {filtered_distance:.1f}px, 目标: {target_distance}px", "距离")
            
            # 计算误差 - 正误差表示需要向左移动，负误差表示需要向右移动
            error = target_distance - filtered_distance
            
            # 检查是否达到目标位置
            if abs(error) < 20:  # 误差小于20像素视为达到目标
                stable_count += 1
                if stable_count >= required_stable_count:
                    if not target_reached:
                        target_reached = True
                        if observe:
                            success(f"已达到目标位置，误差: {abs(error):.1f}px", "成功")
            else:
                stable_count = 0
                target_reached = False
            
            # 比例项
            p_control = kp_side * error
            
            # 积分项 (添加积分限制以防止积分饱和)
            integral += error
            integral = max(-500, min(500, integral))  # 限制积分范围
            i_control = ki_side * integral
            
            # 微分项
            derivative = error - previous_error
            d_control = kd_side * derivative
            previous_error = error
            
            # 计算侧向速度
            side_velocity = p_control + i_control + d_control
            
            # 限制侧向速度范围
            side_velocity = max(-max_side_velocity, min(max_side_velocity, side_velocity))
            
            # 根据目标状态调整速度
            forward_speed = speed
            if target_reached:
                # 如果已达到目标，降低侧向速度以减少振荡
                side_velocity *= 0.5
                # 降低前进速度让侧向移动更平稳
                forward_speed *= 0.7
            
            if observe and time.time() % 0.5 < 0.02:
                debug(f"误差: {error:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}", "控制")
                debug(f"控制指令: 前进={forward_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "速度")
            
            # 设置速度命令 - [前进速度, 侧向速度, 角速度]
            # 侧向速度为正表示向左移动，为负表示向右移动
            msg.vel_des = [forward_speed, side_velocity, 0]
            
            # 使用轨迹线斜率进行小角度调整，确保机器人方向与轨迹线平行
            # 斜率大于0表示向右倾斜，需要顺时针旋转(负角速度)
            # 斜率小于0表示向左倾斜，需要逆时针旋转(正角速度)
            if not track_info["is_vertical"]:  # 对于非垂直线，进行角度校正
                slope = track_info["slope"]
                # 计算旋转角度 - 垂直线的ideal_angle应该是0
                angle_correction = -math.atan(1/slope) if abs(slope) > 0.01 else 0
                # 将角度校正应用为小的角速度
                angular_velocity = angle_correction * 0.2  # 角速度控制系数
                # 限制角速度范围
                angular_velocity = max(-0.2, min(0.2, angular_velocity))
                
                if abs(angular_velocity) > 0.02:  # 只在需要明显校正时应用
                    msg.vel_des[2] = angular_velocity
                    if observe and time.time() % 0.5 < 0.02:
                        debug(f"应用角度校正，斜率: {slope:.2f}, 角速度: {angular_velocity:.3f}", "校正")
            
        else:
            warning("未检测到左侧轨迹线", "警告")
            
            # 如果之前有有效检测结果，使用上一次的控制值但降低强度
            if last_valid_track_info is not None and len(distance_queue) > 0:
                # 使用上一次的距离，但降低侧向速度
                reduced_speed = speed * 0.5  # 降低前进速度
                
                # 如果上次距离已经接近目标，维持当前位置
                last_error = target_distance - distance_queue[-1]
                if abs(last_error) < 50:  # 如果接近目标距离
                    side_velocity = 0  # 停止侧向移动
                    if observe:
                        info("接近目标位置，保持当前侧向位置", "保持")
                else:
                    # 向目标方向缓慢移动
                    side_velocity = 0.05 * (1 if last_error > 0 else -1)
                    if observe:
                        info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整")
                
                msg.vel_des = [reduced_speed, side_velocity, 0]
                if observe:
                    warning(f"使用降低强度的控制: 前进={reduced_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "恢复")
            else:
                # 如果从未检测到轨迹线，降低速度直接向前
                reduced_speed = speed * 0.3
                msg.vel_des = [reduced_speed, 0, 0]
                if observe:
                    warning(f"无法检测到左侧轨迹线，降低速度前进: {reduced_speed:.2f}m/s", "警告")
        
        # 发送命令
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        
        # 短暂延时
        time.sleep(0.05)
        
        # 计算已移动距离(仅用于记录)
        current_position = ctrl.odo_msg.xyz
        dx = current_position[0] - start_position[0]
        dy = current_position[1] - start_position[1]
        distance_moved = math.sqrt(dx*dx + dy*dy)
        
        if observe and time.time() % 2.0 < 0.02:  # 每2秒左右打印一次
            info(f"已移动: {distance_moved:.3f}米, 已用时间: {time.time() - start_time:.1f}秒", "移动")
            # 显示检测成功率
            if detection_total_count > 0:
                detection_rate = (detection_success_count / detection_total_count) * 100
                info(f"左侧轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计")
    
    # 平滑停止
    if observe:
        info("开始平滑停止", "停止")
    
    # 先降低速度再停止，实现平滑停止
    slowdown_steps = 5
    for i in range(slowdown_steps, 0, -1):
        slowdown_factor = i / slowdown_steps
        msg.vel_des = [speed * slowdown_factor, 0, 0]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(0.1)
    
    # 最后完全停止
    ctrl.base_msg.stop()
    
    # 计算最终移动距离
    final_position = ctrl.odo_msg.xyz
    dx = final_position[0] - start_position[0]
    dy = final_position[1] - start_position[1]
    final_distance = math.sqrt(dx*dx + dy*dy)
    
    if observe:
        # 在终点放置红色标记
        end_position = list(final_position)
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True)
        
        success(f"左侧轨迹跟随完成，总移动距离: {final_distance:.3f}米", "完成")
        
        # 显示检测成功率
        if detection_total_count > 0:
            detection_rate = (detection_success_count / detection_total_count) * 100
            info(f"左侧轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计")
    
    return target_reached

# 用法示例
if __name__ == "__main__":
    move_to_hori_line(None, None, observe=True)
    # 90度左转
    arc_turn_around_hori_line(None, None, angle_deg=90, observe=True)
    # 180度右转
    arc_turn_around_hori_line(None, None, angle_deg=-90, observe=True)
    # 双轨道跟随
    follow_dual_tracks(None, None, observe=True)
    # 左侧轨迹跟随
    follow_left_side_track(None, None, observe=True)