mi-task/base_move/move_base_hori_line.py

import math
import time
import cv2

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
from base_move.turn_degree import turn_degree

def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
    """
    控制机器人旋转到横向线水平的位置
    
    参数:
    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()
    
    while attempts < max_attempts and not aligned:
        print(f"尝试次数: {attempts+1}/{max_attempts}")
        # 检测横向线边缘
        edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0)
        
        if edge_point is None or edge_info is None:
            print("未检测到横向线，无法进行校准")
            return False
        
        # 获取检测到的斜率和其他信息
        slope = edge_info["slope"]
        is_horizontal = edge_info["is_horizontal"]
        
        if observe:
            print(f"检测到横向线，斜率: {slope:.6f}")
            print(f"是否足够水平: {is_horizontal}")
        
        # 如果已经水平，则无需旋转
        if is_horizontal:
            print("横向线已经水平，无需校准")
            return True
        
        # 计算需要旋转的角度
        # 斜率 = tan(θ)，因此 θ = arctan(斜率)
        angle_rad = math.atan(slope)
        angle_deg = math.degrees(angle_rad)
        
        # 调整角度方向
        # 正的斜率意味着线条从左到右上升，需要逆时针旋转校正
        # 负的斜率意味着线条从左到右下降，需要顺时针旋转校正
        # 注意旋转方向: 顺时针为负角度，逆时针为正角度
        angle_to_rotate = -angle_deg  # 取负值使旋转方向正确
        
        if observe:
            print(f"需要旋转的角度: {angle_to_rotate:.2f}度")
        
        # 执行旋转
        # 如果角度很小，增加一个小的偏移以确保旋转足够
        if abs(angle_to_rotate) < 3.0:
            angle_to_rotate *= 1.5  # 对小角度进行放大以确保效果
        
        # 限制旋转角度，避免过度旋转
        angle_to_rotate = max(-30, min(30, angle_to_rotate))  
        
        # 使用turn_degree函数执行旋转
        turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False)
        
        if observe:
            print(f"旋转结果: {'成功' if turn_success else '失败'}")
        
        # 增加尝试次数
        attempts += 1
        
        # 在旋转后重新获取图像，这里需要调用获取图像的函数
        # 代码中没有提供获取实时图像的方法，假设每次外部会更新image参数
        
        # 检查是否已经对齐
        # 对于实际应用，应该在旋转后重新捕获图像并检测横向线
        # 这里简单地根据旋转是否成功和旋转角度是否足够小来判断
        if turn_success and abs(angle_to_rotate) < 5.0:
            aligned = 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:
        print(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:
        print(f"相机参数: 水平FOV={math.degrees(horizontal_fov_rad):.1f}°, 垂直FOV={vertical_fov_deg:.1f}°")
        print(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:
        print(f"视场角度关系:")
        print(f"  - 图像底部到横线角度: {math.degrees(bottom_to_line_angle_rad):.2f}°")
        print(f"  - 视场中心到横线角度: {math.degrees(center_to_line_angle_rad):.2f}°")
        print(f"  - 相机倾斜角度: {camera_tilt_angle_deg}°")
        print(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:
            print(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:
            print(f"计算公式: 距离 = 相机高度({camera_height}米) / tan(|视线角度|({abs(math.degrees(view_angle_rad)):.2f}°))")
            print(f"计算结果: 距离 = {ground_distance:.3f}米")
    else:  # 视线平行或向上，无法确定地面交点
        if observe:
            print(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:
        print(f"应用范围限制: 原始距离 {distance:.3f}米 -> 最终距离 {final_distance:.3f}米")
    elif observe:
        print(f"最终距离: {final_distance:.3f}米")
    
    return final_distance

def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
    """
    控制机器人校准并移动到横向线前的指定距离
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    target_distance: 目标位置与横向线的距离(米)，默认为0.5米
    observe: 是否输出中间状态信息和可视化结果，默认为False
    
    返回:
    bool: 是否成功到达目标位置
    """
    # 首先校准到水平
    print("校准到横向线水平")
    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
    
    if not aligned:
        print("无法校准到横向线水平，停止移动")
        return False
    
    # 检测横向线
    # image = cv2.imread("current_image.jpg")  # TEST
    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:
        print("无法检测到横向线，停止移动")
        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:
        print("无法计算到横向线的距离，停止移动")
        return False
    
    if observe:
        print(f"当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米")
    
    # 计算需要移动的距离
    distance_to_move = current_distance - target_distance
    
    if abs(distance_to_move) < 0.05:  # 如果已经很接近目标距离
        print("已经达到目标距离，无需移动")
        return True
    
    # 设置移动命令
    msg.mode = 11  # Locomotion模式
    msg.gait_id = 26  # 自变频步态
    
    # 移动方向设置
    forward = distance_to_move > 0  # 判断是前进还是后退
    
    # 设置移动速度
    move_speed = 1  # 米/秒
    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:
        print(f"起始位置: {start_position}")
        print(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 = int((move_time + 2) * 1000)  # 加一点余量，确保有足够时间移动
    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 + 1  # 超时时间设置为预计移动时间加1秒
    
    # 监控移动距离，但不执行减速（改用stop_smooth）
    while distance_moved < abs(distance_to_move) * 0.95 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秒左右打印一次
            print(f"已移动: {distance_moved:.3f}米, 目标: {abs(distance_to_move):.3f}米")
            
        time.sleep(0.05)  # 小间隔检查位置
    
    # 使用平滑停止方法
    if forward:
        current_vel = [move_speed, 0, 0]
    else:
        current_vel = [-move_speed, 0, 0]
    
    # 使用平滑停止
    ctrl.base_msg.stop_smooth(current_vel=current_vel)
    
    if observe:
        print(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.1  # 如果误差小于10厘米，则认为成功

def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False):
    """
    对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
    参数:
        ctrl: Robot_Ctrl对象
        msg: robot_control_cmd_lcmt对象
        target_distance: 横线前目标保持距离，默认0.5米
        angle_deg: 旋转角度，支持90或180度
        left: True为左转，False为右转
        observe: 是否打印调试信息
    返回:
        bool: 是否成功完成动作
    """
    # 1. 对准横线
    print("校准到横向线水平")
    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
    if not aligned:
        print("无法校准到横向线水平，停止动作")
        return False

    # 2. 检测横线并计算距离
    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:
        print("无法检测到横向线，停止动作")
        return False

    camera_height = 0.355  # 单位: 米
    r = calculate_distance_to_line(edge_info, camera_height, observe=observe)

    # 减去目标距离
    r -= target_distance

    if r is None:
        print("无法计算到横向线的距离，停止动作")
        return False

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

    # 3. 计算圆弧运动参数
    angle_rad = math.radians(angle_deg)
    # 设定角速度（rad/s），可根据实际调整

    # 减小基础角速度，增加精度
    base_w = 0.8  # 原来是0.6

    w = base_w if left else -base_w  # 左转为正，右转为负
    v = abs(w * r)  # 线速度，正负号与角速度方向一致
    t = abs(angle_rad / w)  # 运动时间，取绝对值保证为正
    
    # 应用时间补偿系数，因为实际旋转常会比理论计算多
    time_compensation = 0.92  # 时间减少到92%
    t *= time_compensation
    
    if observe:
        print(f"圆弧半径: {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 = int((t + 2) * 1000)  # 加2秒余量
    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)

    # 计算圆弧长度
    arc_length = abs(angle_rad * r)

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

    # 初始速度
    current_v = v
    current_w = w
    
    # 旋转精度监控
    target_angle = angle_rad if left else -angle_rad  # 目标角度（弧度）
    slowdown_threshold = 0.8  # 当达到目标角度的80%时开始减速（原来是85%）
    
    # 监控旋转进度并自行控制减速
    while abs(angle_turned) < abs(angle_rad) * 0.95 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
            
        # 计算角度完成比例
        completion_ratio = abs(angle_turned) / abs(angle_rad)
        
        # 当完成一定比例时开始减速
        if completion_ratio > slowdown_threshold:
            # 剩余比例作为速度系数
            speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
            # 确保速度系数不会太小，但可以更慢一些
            speed_factor = max(0.2, speed_factor)  # 原来是0.3
            
            # 应用减速
            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)
            
            if observe and time.time() % 0.5 < 0.02:  # 每0.5秒左右打印一次
                print(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秒左右打印一次
                print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度")
        
        time.sleep(0.05)

    # 使用改进的平滑停止方法替代强制停止
    if observe:
        print(f"旋转过程结束，使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
    
    # 记录停止前的角度
    pre_stop_yaw = ctrl.odo_msg.rpy[2]
    
    # 使用专门的旋转停止函数替代原来的强制停止
    ctrl.base_msg.stop_turn_smooth(current_vel=[current_v, 0, current_w], steps=5, delay=0.1, final_steps=2)
    
    # 停下来后的最终角度
    final_yaw = ctrl.odo_msg.rpy[2]
    final_angle_turned = final_yaw - start_yaw
    
    # 角度归一化处理
    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)
    target_angle_deg = angle_deg if left else -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
        print(f"停止过程中旋转了: {stop_angle_diff:.2f}度")
        print(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度")
    
    # 检查是否需要微调
    angle_error = abs(final_angle_deg - target_angle_deg)
    if angle_error > 3.0:  # 如果误差超过3度
        if observe:
            print(f"角度误差: {angle_error:.2f}度，进行微调")
        
        # 计算需要微调的角度
        adjust_angle = target_angle_deg - final_angle_deg
        
        # 增加补偿系数，因为实际旋转常会超出理论计算值
        compensation_factor = 0.85  # 只旋转计算值的85%
        adjust_angle *= compensation_factor
        
        if observe:
            print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度")
        
        # 使用turn_degree函数进行微调
        turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
        
        if observe:
            print(f"微调结果: {'成功' if turn_success else '失败'}")
    
    if observe:
        print("圆弧转弯完成")

    return True

def arc_turn_precise(ctrl, msg, radius, angle_deg, left=True, observe=False):
    """
    实现更精确的圆弧转弯控制，特别改进了停止阶段的处理
    
    参数:
        ctrl: Robot_Ctrl对象
        msg: robot_control_cmd_lcmt对象
        radius: 圆弧半径(米)
        angle_deg: 旋转角度(度)
        left: True为左转，False为右转
        observe: 是否打印调试信息
    返回:
        bool: 是否成功完成动作
    """
    # 1. 参数准备
    angle_rad = math.radians(angle_deg)
    # 设定基础角速度，为更好的控制降低了角速度
    base_w = 0.6  # 降低基础角速度提高精度
    
    # 设定方向
    w = base_w if left else -base_w  # 左转为正，右转为负
    v = abs(w * radius)  # 线速度与角速度和半径成正比
    
    # 计算理论时间
    t = abs(angle_rad / w)
    
    # 应用时间补偿系数
    time_compensation = 0.92
    t *= time_compensation
    
    if observe:
        print(f"圆弧参数: 半径={radius:.3f}米, 角度={angle_deg}度, 方向={'左' if left else '右'}")
        print(f"速度参数: 角速度={w:.3f}rad/s, 线速度={v:.3f}m/s")
        print(f"时间参数: 理论时间={abs(angle_rad / w):.2f}s, 实际时间={t:.2f}s")
    
    # 2. 启动转向
    msg.mode = 11
    msg.gait_id = 26
    msg.vel_des = [v, 0, w]
    msg.duration = int((t + 2) * 1000)  # 余量2秒
    msg.step_height = [0.06, 0.06]
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    
    # 3. 记录初始状态
    start_yaw = ctrl.odo_msg.rpy[2]
    start_position = list(ctrl.odo_msg.xyz)
    start_time = time.time()
    
    # 4. 设置控制参数
    target_angle = angle_rad if left else -angle_rad
    distance_moved = 0
    angle_turned = 0
    timeout = t + 2  # 超时保护
    current_v = v
    current_w = w
    
    # 分阶段控制参数
    # 提前减速区间设置更大，从70%开始减速
    slowdown_threshold = 0.7  
    # 慢速区间设置更大，从90%开始进入超慢速
    precision_threshold = 0.9
    
    # 5. 实时监控和控制
    while abs(angle_turned) < abs(angle_rad) * 0.98 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
            
        # 计算完成比例
        completion_ratio = abs(angle_turned) / abs(angle_rad)
        
        # 分阶段减速控制
        if completion_ratio > precision_threshold:
            # 超低速阶段（精确控制）
            speed_factor = 0.1  # 使用固定的极低速度
            
            # 应用超低速
            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 = 150  # 更短的更新周期
            msg.step_height = [0.06, 0.06]
            msg.life_count += 1
            ctrl.Send_cmd(msg)
            
            if observe and time.time() % 0.5 < 0.02:
                print(f"精确阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度: {speed_factor:.2f}, 角速度: {current_w:.3f}rad/s")
        
        elif completion_ratio > slowdown_threshold:
            # 一般减速阶段
            speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
            speed_factor = max(0.2, speed_factor * 0.8)  # 整体降低一点速度
            
            # 应用减速
            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)
            
            if observe and time.time() % 0.5 < 0.02:
                print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 角速度: {current_w:.3f}rad/s")
        
        else:
            if observe and time.time() % 0.5 < 0.02:
                print(f"匀速阶段 - 已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度")
        
        # 更短的循环检查间隔
        time.sleep(0.03)
    
    # 6. 使用改进的平滑停止方法
    if observe:
        print(f"旋转过程结束，使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
    
    # 记录停止前的角度
    pre_stop_yaw = ctrl.odo_msg.rpy[2]
    
    # 使用专门的旋转停止函数
    # 增加步数和降低延迟以获得更平滑的停止
    ctrl.base_msg.stop_turn_smooth(current_vel=[current_v, 0, current_w], steps=7, delay=0.08, final_steps=3)
    
    # 7. 停止后的角度分析
    final_yaw = ctrl.odo_msg.rpy[2]
    final_angle_turned = final_yaw - start_yaw
    
    # 角度归一化处理
    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)
    target_angle_deg = angle_deg if left else -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
        print(f"停止过程中旋转了: {stop_angle_diff:.2f}度")
        print(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度")
    
    # 8. 微调处理
    angle_error = abs(final_angle_deg - target_angle_deg)
    
    # 如果误差超过阈值，进行微调
    if angle_error > 2.0:  # 降低误差阈值到2度
        if observe:
            print(f"角度误差: {angle_error:.2f}度，进行微调")
        
        # 计算微调角度
        adjust_angle = target_angle_deg - final_angle_deg
        
        # 应用补偿系数
        compensation_factor = 0.85
        adjust_angle *= compensation_factor
        
        if observe:
            print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度")
        
        # 使用turn_degree进行精确微调
        turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
        
        if observe:
            print(f"微调结果: {'成功' if turn_success else '失败'}")
    else:
        if observe:
            print(f"角度误差在允许范围内: {angle_error:.2f}度，无需微调")
    
    # 9. 最终位置和角度
    final_position = ctrl.odo_msg.xyz
    final_yaw = ctrl.odo_msg.rpy[2]
    
    if observe:
        dx = final_position[0] - start_position[0]
        dy = final_position[1] - start_position[1]
        total_distance = math.sqrt(dx*dx + dy*dy)
        
        # 计算理论圆弧长度
        arc_length = abs(radius * angle_rad)
        print(f"位移统计: 直线距离={total_distance:.3f}米, 圆弧长度={arc_length:.3f}米")
        print(f"精确圆弧转弯完成")
    
    return True

def arc_turn_around_hori_line_precise(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False):
    """
    使用精确圆弧转弯方法，对准前方横线，然后以计算出的距离为半径，做一个向左或向右的圆弧旋转。
    
    参数:
        ctrl: Robot_Ctrl对象
        msg: robot_control_cmd_lcmt对象
        target_distance: 横线前目标保持距离，默认0.2米
        angle_deg: 旋转角度，支持任意角度
        left: True为左转，False为右转
        observe: 是否打印调试信息
    返回:
        bool: 是否成功完成动作
    """
    # 1. 对准横线
    print("校准到横向线水平")
    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
    if not aligned:
        print("无法校准到横向线水平，停止动作")
        return False

    # 2. 检测横线并计算距离
    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:
        print("无法检测到横向线，停止动作")
        return False

    camera_height = 0.355  # 单位: 米
    r = calculate_distance_to_line(edge_info, camera_height, observe=observe)

    # 减去目标距离得到圆弧半径
    r -= target_distance

    if r is None or r < 0.1:  # 半径太小无法执行圆弧
        print(f"计算的半径太小: {r if r is not None else 'None'}，无法执行圆弧转弯")
        return False
        
    if observe:
        print(f"当前距离: {r+target_distance:.3f}米，目标距离: {target_distance:.3f}米")
        print(f"将使用半径: {r:.3f}米 执行圆弧转弯")

    # 3. 使用精确圆弧转弯函数执行
    return arc_turn_precise(ctrl, msg, r, angle_deg, left, observe)

# 用法示例
if __name__ == "__main__":
    move_to_hori_line(None, None, observe=True)
    # 90度左转
    arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True)
    # 180度右转
    arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True)
    # 使用精确版本做90度左转
    arc_turn_around_hori_line_precise(None, None, angle_deg=90, left=True, observe=True)

"""
代码改进说明：

1. 添加了专用的平滑旋转停止函数 stop_turn_smooth (在 utils/base_msg.py 中):
   - 分两个阶段减速：先减小角速度，再减小线速度
   - 使用平方递减使角速度更快地降低
   - 发送多次零速度命令确保完全停止

2. 改进了原始的 arc_turn_around_hori_line 函数:
   - 使用平滑停止替代强制停止
   - 添加停止过程中角度变化的监控
   - 改进了角度误差计算和微调逻辑

3. 新增了 arc_turn_precise 函数，实现更精确的转弯控制:
   - 速度更低，控制更精细
   - 分三个阶段控制：匀速阶段、减速阶段、精确阶段
   - 更频繁的控制更新和监控
   - 更完善的停止策略
   - 更严格的角度误差要求

4. 新增 arc_turn_around_hori_line_precise 结合横线检测和精确转弯功能:
   - 保持原有的横线检测和校准功能
   - 使用改进的精确转弯控制

这些改进有效解决了转弯后快速切换到零速度导致的角度偏移问题，提高了转弯的精度和稳定性。
"""