mi-task/base_move/center_on_dual_tracks.py

import math
import time
import sys
import os

sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
from utils.detect_dual_track_lines import detect_dual_track_lines

def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False,
                         mode=11, gait_id=26, step_height=[0.06, 0.06], 
                         estimated_track_distance=1.0,
                         detect_height=0.4,
                         detection_interval=0.2):
    """
    控制机器狗仅使用Y轴移动调整到双轨道线的中间位置
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    max_time: 最大调整时间(秒)，默认为15秒
    max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中，默认为8像素
    observe: 是否输出中间状态信息和可视化结果，默认为False
    mode: 控制模式，默认为11
    gait_id: 步态ID，默认为26
    step_height: 抬腿高度，默认为[0.06, 0.06]
    estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米
    detect_height: 检测高度，默认为0.4
    detection_interval: 检测间隔时间(秒)，默认为0.2秒
    
    返回:
    bool: 是否成功调整到中心位置
    """
    section("开始双轨道居中", "轨道居中")
    
    # 设置移动命令基本参数
    msg.mode = mode
    msg.gait_id = gait_id
    msg.duration = 0  # wait next cmd
    msg.step_height = step_height
    
    # 记录起始时间
    start_time = time.time()
    last_detection_time = 0  # 记录上次检测时间
    
    # 记录起始位置
    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)
    
    # 根据RGB相机参数，计算像素偏差到实际距离的转换
    # 相机水平视场角FOV = 1.46608弧度（约84度）
    # 图像宽度 = 1920像素
    camera_hfov = 1.46608  # 水平视场角（弧度）
    image_width = 1920     # 图像宽度（像素）
    
    # 计算像素到角度的转换比例
    pixels_per_radian = image_width / camera_hfov
    
    # 统计变量
    detection_success_count = 0
    detection_total_count = 0
    
    # 稳定计数器 - 连续几次在中心位置才认为稳定
    stable_count = 0
    required_stable_count = 2
    
    # 变量记录最后有效的偏差，用于处理检测失败的情况
    last_valid_deviation = 0
    
    # 开始调整循环
    while time.time() - start_time < max_time:
        # 获取当前图像
        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
            
            # 获取当前偏差
            current_deviation = center_info["deviation"]
            last_valid_deviation = current_deviation  # 记录有效偏差
            
            # 转换像素偏差到实际距离偏差
            # 1. 像素偏差转换为弧度
            deviation_rad = current_deviation / pixels_per_radian
            # 2. 根据估计距离计算实际偏差（单位：米）
            actual_deviation = math.tan(deviation_rad) * estimated_track_distance
            
            if observe:
                debug(f"偏差: {current_deviation:.1f}px", "偏差")
                debug(f"实际距离偏差: {actual_deviation:.3f}m (距离估计: {estimated_track_distance:.1f}m)", "实际偏差")
            
            # 判断是否已经居中
            if abs(current_deviation) <= max_deviation:
                stable_count += 1
                if observe:
                    info(f"已接近中心，稳定计数: {stable_count}/{required_stable_count}", "居中")
                    info(f"距离偏差: {actual_deviation:.3f}m", "居中")
                
                # 即使在稳定过程中，也要保持小幅调整以保持居中
                if stable_count < required_stable_count:
                    # 直接根据实际偏差移动
                    msg.vel_des = [0, actual_deviation, 0]
                    ctrl.Send_cmd(msg)
                
                if stable_count >= required_stable_count:
                    # 已经稳定在中心位置
                    if observe:
                        success(f"成功居中，最终偏差: {current_deviation:.1f}px ({actual_deviation:.3f}m)", "完成")
                    break
            else:
                # 不在中心，重置稳定计数
                stable_count = 0
                
                # 直接根据计算出的实际偏差距离进行移动
                # 为了避免移动过猛，可以限制最大移动距离
                max_move_distance = 0.35  # 最大移动距离（米）
                move_distance = actual_deviation
                if abs(move_distance) > max_move_distance:
                    move_distance = max_move_distance * (1 if move_distance > 0 else -1)
                
                if observe:
                    debug(f"横向移动距离: {move_distance:.3f}m", "控制")
                
                # 设置速度命令 - 只使用y轴移动，不前进和转向
                msg.vel_des = [0, move_distance, 0]  # [前进速度, 侧向速度, 角速度]
                
                # 发送命令
                msg.life_count += 1
                ctrl.Send_cmd(msg)
        else:
            warning("未检测到双轨道线", "警告")
            
            # 如果已经有了一些有效的检测，使用最后一次有效的偏差继续移动
            if detection_success_count > 0:
                # 使用最后一次有效偏差值计算移动距离
                deviation_rad = last_valid_deviation / pixels_per_radian
                move_distance = math.tan(deviation_rad) * estimated_track_distance * 0.5  # 缩小系数，避免过度移动
                
                # 限制最大移动距离
                max_move_distance = 0.15
                move_distance = max(-max_move_distance, min(max_move_distance, move_distance))
                
                msg.vel_des = [0, move_distance, 0]
                msg.life_count += 1
                ctrl.Send_cmd(msg)
                
                if observe:
                    warning(f"使用最后偏差值继续移动: {move_distance:.3f}m", "暂停")
            else:
                # 如果一开始就没有检测到，可以尝试小范围搜索
                if detection_total_count < 10:
                    if detection_total_count % 2 == 0:
                        # 向右搜索
                        msg.vel_des = [0, -0.08, 0]
                    else:
                        # 向左搜索
                        msg.vel_des = [0, 0.08, 0]
                    
                    msg.life_count += 1
                    ctrl.Send_cmd(msg)
                    
                    if observe:
                        debug("搜索轨道线...", "搜索")
                else:
                    # 超过一定次数仍未检测到，停止搜索
                    msg.vel_des = [0, 0, 0]
                    msg.life_count += 1
                    ctrl.Send_cmd(msg)
                    
                    if observe:
                        error("无法检测到轨道线，放弃调整", "失败")
                    break
        
        # 短暂延时
        time.sleep(0.05)
    
    # 停止移动
    ctrl.base_msg.stop()
    
    # 计算最终位置与起始位置的变化
    final_position = ctrl.odo_msg.xyz
    dx = final_position[0] - start_position[0]
    dy = final_position[1] - start_position[1]
    
    if observe:
        # 在终点放置红色标记
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(final_position[0], final_position[1], final_position[2] if len(final_position) > 2 else 0.0, 'red', observe=True)
        
        info(f"横向移动距离: {abs(dy):.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})", "统计")
    
    # 判断是否成功
    success = False
    if time.time() - start_time >= max_time:
        if observe:
            warning("超过最大调整时间", "超时")
    else:
        # 如果因为已稳定在中心而退出循环，则认为成功
        if stable_count >= required_stable_count:
            success = True
    
    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], estimated_track_distance=1.0,
                                detection_interval=0.2):
    """
    先居中到双轨道线中间，然后沿轨道线行走指定距离
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    distance: 目标前进距离(米)
    speed: 前进速度(米/秒)，默认为0.5米/秒
    max_centering_time: 最大居中调整时间(秒)，默认为15秒
    observe: 是否输出中间状态信息和可视化结果，默认为False
    mode: 控制模式，默认为11
    gait_id: 步态ID，默认为26
    step_height: 抬腿高度，默认为[0.06, 0.06]
    estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米
    detection_interval: 检测间隔时间(秒)，默认为0.2秒
    
    返回:
    bool: 是否成功完成居中和轨道跟随
    """
    section("开始双轨道居中和跟随", "轨道任务")
    
    # 第一步：居中到轨道中间
    if observe:
        info("步骤1: 调整到轨道中间", "居中")
    
    centering_success = center_on_dual_tracks(
        ctrl, msg, 
        max_time=max_centering_time, 
        observe=observe,
        mode=mode, 
        gait_id=gait_id, 
        step_height=step_height,
        estimated_track_distance=estimated_track_distance,
        detection_interval=detection_interval
    )
    
    if not centering_success:
        if observe:
            error("居中调整失败，无法继续跟随轨道", "失败")
        return False
    
    # 第二步：沿轨道跟随指定距离
    if observe:
        info(f"步骤2: 沿轨道前进 {distance:.2f}米", "跟随")
    
    # 导入轨道跟随函数
    from base_move.follow_dual_tracks import follow_dual_tracks
    
    following_success = follow_dual_tracks(
        ctrl, msg, 
        speed=speed, 
        target_distance=distance, 
        observe=observe,
        mode=mode, 
        gait_id=gait_id, 
        step_height=step_height
    )
    
    if observe:
        if following_success:
            success("成功完成轨道居中和跟随任务", "完成")
        else:
            warning("轨道跟随未完全成功", "警告")
    
    return following_success