mi-task/task_3/task_3.py

import time
import sys
import os
import toml
import copy
import math
import lcm
import numpy as np
import cv2
import tempfile

# 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
# 添加当前目录到路径，确保可以找到local文件
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
from base_move.turn_degree import turn_degree, turn_degree_v2
from base_move.go_straight import go_straight, go_lateral
from base_move.go_to_xy import go_to_x_v2
from base_move.center_on_dual_tracks import center_on_dual_tracks
from file_send_lcmt import file_send_lcmt
from utils.yellow_area_analyzer import analyze_yellow_area_ratio

# 创建本模块特定的日志记录器
logger = get_logger("任务3")

observe = True

YELLOW_RATIO_THRESHOLD = 0.15  # TODO 黄色区域比例阈值

def run_task_3(ctrl, msg, time_sleep=5000):
    section('任务3：上下坡', "启动")
    info('开始执行任务3...', "启动")
    turn_degree_v2(ctrl, msg, 90, absolute=True)

    section('任务3-1：up and down', "开始")
    go_straight_with_enhanced_calibration(ctrl, msg, distance = 5, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21])
    # pass_up_down(ctrl, msg)

    turn_degree_v2(ctrl, msg, 90, absolute=True)
    center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)

    section('任务3-2：yellow stop', "开始")
    go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=YELLOW_RATIO_THRESHOLD, speed=0.3)
    
    # 原地站立3秒
    section("原地站立3秒", "站立")
    msg.mode = 12
    msg.gait_id = 0
    msg.duration = 0
    msg.step_height = [0.06, 0.06]
    msg.vel_des = [0, 0, 0]
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    
    info("开始原地站立3秒", "站立")
    time.sleep(time_sleep / 1000)
    info("完成原地站立", "站立")
    

def run_task_3_back(ctrl, msg, time_sleep=5000):
    section('任务3-1：up', "开始")
    section('任务3-2：yellow stop', "开始")
    go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=YELLOW_RATIO_THRESHOLD, speed=0.3)

    # 原地站立3秒
    section("原地站立3秒", "站立")
    msg.mode = 12
    msg.gait_id = 0
    msg.duration = 0
    msg.step_height = [0.06, 0.06]
    msg.vel_des = [0, 0, 0]
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    
    info("开始原地站立3秒", "站立")
    time.sleep(time_sleep / 1000)
    info("完成原地站立", "站立")

    section('任务3-3：up and down', "开始")
    go_straight_with_enhanced_calibration(ctrl, msg, distance = 5, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21])

    # TODO 调优达到一个合适的位置，准备 task 2-5 的返回
    # 或许用到 move-to-line 函数
    

def go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3, max_time=30, observe=True):
    """
    控制机器人直走，直到摄像头检测到黄色区域比例超过指定阈值后开始下降时停止
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    yellow_ratio_threshold: 黄色区域占比阈值(0-1之间的浮点数)，默认为0.15
    speed: 前进速度(米/秒)，默认为0.3米/秒
    max_time: 最大行走时间(秒)，默认为30秒
    observe: 是否输出中间状态信息和可视化结果，默认为True
    
    返回:
    bool: 是否成功检测到黄色区域并停止
    """
    section("开始直行寻找黄色区域", "黄色检测")
    
    # 设置移动命令基本参数
    msg.mode = 11  # Locomotion模式
    msg.gait_id = 26  # 自变频步态
    msg.duration = 0  # wait next cmd
    msg.step_height = [0.06, 0.06]  # 抬腿高度
    msg.vel_des = [speed, 0, 0]  # [前进速度, 侧向速度, 角速度]
    
    # 记录起始时间和位置
    start_time = time.time()
    start_position = list(ctrl.odo_msg.xyz)
    
    if observe:
        info(f"开始寻找黄色区域，阈值: {yellow_ratio_threshold:.2%}", "启动")
        debug(f"起始位置: {start_position}", "位置")
    
    # 检测间隔
    check_interval = 0.3  # 秒
    last_check_time = 0
    
    # 黄色区域监测变量
    yellow_peak_detected = False  # 是否检测到峰值
    yellow_decreasing = False  # 是否开始下降
    max_yellow_ratio = 0.0  # 记录最大黄色区域占比
    yellow_ratio_history = []  # 记录黄色区域占比历史
    history_window_size = 5  # 历史窗口大小，用于平滑处理
    
    try:
        # 开始移动
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        
        # 持续检测黄色区域
        while time.time() - start_time < max_time and not yellow_decreasing:
            current_time = time.time()
            
            # 定期发送移动命令保持移动状态
            if current_time - last_check_time >= check_interval:
                # 获取当前图像并保存到临时文件
                current_image = ctrl.image_processor.get_current_image('ai')  # INFO 默认采用 ai 相机
                
                # 创建临时文件保存图像
                with tempfile.NamedTemporaryFile(suffix='.jpg', delete=False) as temp_file:
                    temp_filename = temp_file.name
                    cv2.imwrite(temp_filename, current_image)
                
                try:
                    # 分析图像中的黄色区域
                    yellow_ratio = analyze_yellow_area_ratio(temp_filename, debug=False, save_result=False)
                    
                    # 添加到历史记录
                    yellow_ratio_history.append(yellow_ratio)
                    if len(yellow_ratio_history) > history_window_size:
                        yellow_ratio_history.pop(0)
                    
                    # 计算平滑后的当前黄色占比（使用最近几次的平均值以减少噪声）
                    current_smooth_ratio = sum(yellow_ratio_history) / len(yellow_ratio_history)
                    
                    # 计算已移动距离（仅用于显示）
                    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:
                        info(f"当前黄色区域占比: {yellow_ratio:.2%}, 平滑值: {current_smooth_ratio:.2%}, 已移动: {distance_moved:.2f}米", "检测")
                    
                    # 检测是否达到阈值（开始监测峰值）
                    if current_smooth_ratio >= yellow_ratio_threshold:
                        # 更新最大值
                        if current_smooth_ratio > max_yellow_ratio:
                            max_yellow_ratio = current_smooth_ratio
                            if not yellow_peak_detected:
                                yellow_peak_detected = True
                                if observe:
                                    info(f"黄色区域占比超过阈值，开始监测峰值", "检测")
                        # 检测是否开始下降
                        elif yellow_peak_detected and current_smooth_ratio < max_yellow_ratio * 0.9:  # 下降到峰值的90%以下认为开始下降
                            yellow_decreasing = True
                            if observe:
                                success(f"检测到黄色区域占比开始下降，峰值: {max_yellow_ratio:.2%}, 当前: {current_smooth_ratio:.2%}", "检测")
                
                finally:
                    # 删除临时文件
                    try:
                        os.unlink(temp_filename)
                    except:
                        pass
                
                # 更新心跳
                msg.life_count += 1
                ctrl.Send_cmd(msg)
                last_check_time = current_time
            
            # 小间隔
            time.sleep(0.05)
        
        # 平滑停止
        if yellow_decreasing:
            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:
            if yellow_decreasing:
                success(f"成功检测到黄色区域峰值并停止，峰值占比: {max_yellow_ratio:.2%}, 总移动距离: {final_distance:.2f}米", "完成")
            else:
                warning(f"未能在限定时间内检测到黄色区域峰值，总移动距离: {final_distance:.2f}米", "超时")
        
        return yellow_decreasing
    
    except KeyboardInterrupt:
        # 处理键盘中断
        ctrl.base_msg.stop()
        if observe:
            warning("操作被用户中断", "中断")
        return False
    except Exception as e:
        # 处理其他异常
        ctrl.base_msg.stop()
        if observe:
            error(f"发生错误: {str(e)}", "错误")
        return False


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]):
    """
    控制机器人在石板路上沿直线行走，使用视觉校准和姿态传感器融合来保持直线
    现在用于 task-3 的上下坡
    
    参数:
    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
        
        # 定期进行视觉检查和修正  # TODO 这个函数如果用于 上下坡的话，感觉或许就不需要了，如果能保证 task 2-5 的效果。
        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)
                    
                    # 如果成功检测到轨道线，使用它进行修正
                    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


# INFO 保持文件相对路径，直接从 task-4中调用
robot_cmd = {
    'mode':0, 'gait_id':0, 'contact':0, 'life_count':0,
    'vel_des':[0.0, 0.0, 0.0],
    'rpy_des':[0.0, 0.0, 0.0],
    'pos_des':[0.0, 0.0, 0.0],
    'acc_des':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    'ctrl_point':[0.0, 0.0, 0.0],
    'foot_pose':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    'step_height':[0.0, 0.0],
    'value':0,  'duration':0
}

def pass_stone(ctrl, msg, distance=5.0, observe=True):
    """
    通过里程计设定distance，执行上坡gait。
    :param ctrl: 控制器对象
    :param msg: 控制消息
    :param distance: 期望移动距离（米）
    :param observe: 是否打印过程信息
    """
    usergait_msg = file_send_lcmt()
    try:
        # 1. 生成gait参数文件
        steps = toml.load("./task_3/Gait_Params_up.toml")
        full_steps = {'step':[robot_cmd]}
        k = 0
        for i in steps['step']:
            cmd = copy.deepcopy(robot_cmd)
            cmd['duration'] = i['duration']
            if i['type'] == 'usergait':                
                cmd['mode'] = 11 # LOCOMOTION
                cmd['gait_id'] = 110 # USERGAIT
                cmd['vel_des'] = i['body_vel_des']
                cmd['rpy_des'] = i['body_pos_des'][0:3]
                cmd['pos_des'] = i['body_pos_des'][3:6]
                cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2]
                cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5]
                cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8]
                cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11]
                cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3
                cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3
                cmd['acc_des'] = i['weight']
                cmd['value'] = i['use_mpc_traj']
                cmd['contact'] = math.floor(i['landing_gain'] * 1e1)
                cmd['ctrl_point'][2] =  i['mu']
            if k == 0:
                full_steps['step'] = [cmd]
            else:
                full_steps['step'].append(cmd)
            k += 1
        with open("./task_3/Gait_Params_up_full.toml", 'w') as f:
            f.write("# Gait Params\n")
            f.writelines(toml.dumps(full_steps))

        # 2. 发送gait定义和参数
        with open("./task_3/Gait_Def_up.toml",'r') as file_obj_gait_def, \
             open("./task_3/Gait_Params_up_full.toml",'r') as file_obj_gait_params:
            usergait_msg.data = file_obj_gait_def.read()
            ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
            time.sleep(0.5)
            usergait_msg.data = file_obj_gait_params.read()
            ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
            time.sleep(0.1)

        # 3. 记录起始位置
        start_xyz = ctrl.odo_msg.xyz[:2]  # 只取x, y
        if observe:
            info(f"pass_up_down: 期望移动距离 {distance:.3f} 米，起始位置: {start_xyz}", "监测")

        # 4. 设置gait执行参数
        msg.mode = 62
        msg.value = 0
        msg.contact = 15
        msg.gait_id = 110
        msg.duration = 1000
        msg.life_count += 1

        # 5. 控制循环，按里程计判断是否到达（去除max-iteration，直接while判断）
        arrived = False
        step = 0
        while True:
            ctrl.Send_cmd(msg)
            cur_xyz = ctrl.odo_msg.xyz[:2]
            dx = cur_xyz[0] - start_xyz[0]
            dy = cur_xyz[1] - start_xyz[1]
            moved = (dx**2 + dy**2) ** 0.5
            if observe and step % 10 == 0:
                info(f"Step:{step} 已移动距离={moved:.3f}m, 目标={distance:.3f}m, 当前xy={cur_xyz}", "监测")
            if moved >= distance:
                arrived = True
                if observe:
                    success(f"pass_up_down: 已到达设定距离 {distance:.3f}m (实际: {moved:.3f}m)", "完成")
                break
            step += 1
            time.sleep(0.1)

        # 6. 完全停止
        if hasattr(ctrl.base_msg, 'stop_smooth'):
            ctrl.base_msg.stop_smooth()
        else:
            ctrl.base_msg.stop()
        ctrl.Send_cmd(msg)
        time.sleep(0.1)

        # 7. 反馈最终位置
        final_xyz = ctrl.odo_msg.xyz[:2]
        dx = final_xyz[0] - start_xyz[0]
        dy = final_xyz[1] - start_xyz[1]
        actual_distance = (dx**2 + dy**2) ** 0.5
        if observe:
            info(f"pass_up_down: 最终位置 {final_xyz}, 实际移动距离 {actual_distance:.3f}m", "监测")
        return arrived

    except KeyboardInterrupt:
        msg.mode = 7 #PureDamper before KeyboardInterrupt:
        msg.gait_id = 0
        msg.duration = 0
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        pass