mi-task/base_move/go_to_xy.py

import math
import time
import sys
import os

sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from utils.localization_lcmt import localization_lcmt
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_twice
from base_move.go_straight import go_straight

# 创建本模块特定的日志记录器
logger = get_logger("坐标移动")

def go_to_xy(ctrl, msg, target_x, target_y, speed=0.5, precision=True, observe=False):
    """
    控制机器人移动到指定的(x,y)坐标位置，使用直接xy速度控制
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    target_x: 目标X坐标(米)
    target_y: 目标Y坐标(米)
    speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5
    precision: 是否使用高精度模式（更慢速度），默认为True
    observe: 是否输出中间状态信息，默认为False
    
    返回:
    bool: 是否成功完成移动到目标位置
    """
    # 获取当前位置
    current_position = ctrl.odo_msg.xyz
    current_x, current_y = current_position[0], current_position[1]
    
    if observe:
        section('目标点导航', "开始")
        info(f"当前位置: ({current_x:.3f}, {current_y:.3f})", "位置")
        info(f"目标位置: ({target_x:.3f}, {target_y:.3f})", "目标")
        # 在起点放置标记
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(current_x, current_y, 
                            current_position[2] if len(current_position) > 2 else 0.0, 
                            'blue', observe=True)
        # 在目标点放置标记
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(target_x, target_y, 0.0, 'green', observe=True)
    
    # 计算与目标点的距离和角度
    dx = target_x - current_x
    dy = target_y - current_y
    distance = math.sqrt(dx*dx + dy*dy)
    
    # 如果已经非常接近目标，无需移动
    if distance < 0.05:  # 5厘米以内视为已达到
        if observe:
            success("已处于目标位置附近，无需移动", "完成")
        return True
    
    if observe:
        info(f"到目标点的距离: {distance:.3f}米", "路径")
    
    # 限制速度范围
    max_speed = min(max(abs(speed), 0.1), 1.0)
    if precision:
        max_speed = min(max_speed, 0.5)  # 高精度模式下限制最大速度
    
    # 根据距离动态调整速度
    if distance > 1.0:
        actual_speed = max_speed
    elif distance > 0.5:
        actual_speed = max_speed * 0.8
    elif distance > 0.2:
        actual_speed = max_speed * 0.6
    else:
        actual_speed = max_speed * 0.4
    
    # 设置移动命令基本参数
    msg.mode = 11  # Locomotion模式
    msg.gait_id = 26  # 自变频步态
    msg.step_height = [0.06, 0.06]  # 抬腿高度
    
    # 估算移动时间，但实际上会通过里程计控制
    estimated_time = distance / actual_speed
    timeout = estimated_time + 5  # 增加超时时间为预计移动时间加5秒
    
    # 监控移动过程
    start_time = time.time()
    position_check_interval = 0.05  # 位置检查间隔(秒)
    last_check_time = start_time
    last_distance = distance
    stall_count = 0
    
    if observe:
        info(f"开始移动，预计用时 {estimated_time:.2f}秒", "移动")
    
    # 监控移动到达目标点
    while 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_x, current_y = current_position[0], current_position[1]
            
            # 计算新的位移向量和距离
            dx = target_x - current_x
            dy = target_y - current_y
            current_distance = math.sqrt(dx*dx + dy*dy)
            
            # 判断是否已经足够接近目标
            if current_distance < 0.05:  # 5厘米以内视为已达到
                break
            
            # 计算移动进度
            progress = 1.0 - (current_distance / distance)
            
            # 检测是否卡住（距离变化很小）
            distance_change = last_distance - current_distance
            if abs(distance_change) < 0.005:  # 变化小于5毫米
                stall_count += 1
                if stall_count > 20:  # 连续多次检测到几乎没有移动
                    if observe:
                        warning("检测到机器人可能卡住，调整策略", "移动")
                    # 可能是卡住了，稍微增加速度
                    actual_speed = min(actual_speed * 1.2, max_speed)
                    stall_count = 0
            else:
                stall_count = 0
            
            # 动态调整速度，接近目标时减速
            if current_distance < 0.2:  # 接近目标时减速
                target_speed = max_speed * 0.3
            elif current_distance < 0.5:
                target_speed = max_speed * 0.5
            else:
                target_speed = actual_speed
            
            # 计算速度分量，归一化后乘以目标速度
            speed_factor = target_speed / current_distance if current_distance > 0 else 0
            vel_x = dx * speed_factor
            vel_y = dy * speed_factor
            
            # 更新速度命令
            msg.vel_des = [vel_x, vel_y, 0]  # [前进速度, 侧向速度, 角速度]
            msg.duration = 0  # 持续执行直到下一个命令
            msg.life_count = (msg.life_count + 1) % 127  # 防止溢出，确保life_count在int8_t范围内
            ctrl.Send_cmd(msg)
            
            if observe and (current_time - start_time) % 1.0 < position_check_interval:
                debug(f"当前位置: ({current_x:.3f}, {current_y:.3f}), 剩余距离: {current_distance:.3f}米, 完成: {progress*100:.1f}%", "移动")
                debug(f"速度: x={vel_x:.2f}, y={vel_y:.2f}", "速度")
            
            last_check_time = current_time
            last_distance = current_distance
        
        time.sleep(0.01)  # 小间隔检查位置
    
    # 平滑停止
    if hasattr(ctrl.base_msg, 'stop_smooth'):
        ctrl.base_msg.stop_smooth()
    else:
        ctrl.base_msg.stop()
    
    # 获取最终位置并计算与目标的误差
    final_position = ctrl.odo_msg.xyz
    final_x, final_y = final_position[0], final_position[1]
    final_dx = target_x - final_x
    final_dy = target_y - final_y
    final_distance = math.sqrt(final_dx*final_dx + final_dy*final_dy)
    
    if observe:
        info(f"最终位置: ({final_x:.3f}, {final_y:.3f})", "位置")
        info(f"与目标的距离误差: {final_distance:.3f}米", "误差")
        # 在终点放置标记
        if hasattr(ctrl, 'place_marker'):
            ctrl.place_marker(final_x, final_y, 
                           final_position[2] if len(final_position) > 2 else 0.0, 
                           'red', observe=True)
    
    # 判断是否成功到达目标点（误差在10厘米以内）
    nav_success = final_distance < 0.1
    
    if observe:
        if nav_success:
            success(f"成功到达目标点，误差: {final_distance:.3f}米", "成功")
        else:
            warning(f"未能精确到达目标点，误差: {final_distance:.3f}米", "警告")
    
    return nav_success

def go_to_xy_with_correction(ctrl, msg, target_x, target_y, speed=0.5, precision=True, 
                            max_attempts=2, observe=False):
    """
    控制机器人移动到指定的(x,y)坐标位置，并在必要时进行路径修正
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    target_x: 目标X坐标(米)
    target_y: 目标Y坐标(米)
    speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5
    precision: 是否使用高精度模式，默认为True
    max_attempts: 最大尝试次数，默认为2
    observe: 是否输出中间状态信息，默认为False
    
    返回:
    bool: 是否成功完成移动到目标位置
    """
    attempt = 1
    
    while attempt <= max_attempts:
        if observe:
            section(f'导航尝试 {attempt}/{max_attempts}', "开始")
        
        # 执行基本导航
        success = go_to_xy(ctrl, msg, target_x, target_y, speed, precision, observe)
        
        if success:
            return True
        
        # 如果导航失败且还有尝试次数，进行修正
        if attempt < max_attempts:
            if observe:
                warning(f"第{attempt}次导航未达到预期精度，尝试修正", "修正")
            
            # 获取当前位置
            current_position = ctrl.odo_msg.xyz
            current_x, current_y = current_position[0], current_position[1]
            
            # 计算与目标点的距离和角度
            dx = target_x - current_x
            dy = target_y - current_y
            distance = math.sqrt(dx*dx + dy*dy)
            
            # 如果距离很近但未达到成功标准，可能是卡住了
            if distance < 0.2:  # 20厘米以内
                if observe:
                    info(f"距离目标很近 ({distance:.3f}米)，使用更高精度方式移动", "策略")
                # 使用更慢的速度和更高的精度
                speed = speed * 0.6
                precision = True
        
        attempt += 1
    
    # 获取最终位置并计算与目标的误差
    final_position = ctrl.odo_msg.xyz
    final_x, final_y = final_position[0], final_position[1]
    final_dx = target_x - final_x
    final_dy = target_y - final_y
    final_distance = math.sqrt(final_dx*final_dx + final_dy*final_dy)
    
    if observe:
        if final_distance < 0.2:
            warning(f"虽未达到精确目标，但已在目标附近 ({final_distance:.3f}米)", "部分成功")
        else:
            error(f"多次尝试后仍未能达到目标点，最终误差: {final_distance:.3f}米", "失败")
    
    return False

# 用法示例
if __name__ == "__main__":
    # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象
    # 移动到坐标(1.0, 2.0)
    # go_to_xy(ctrl, msg, 1.0, 2.0, speed=0.5, observe=True)
    # 移动到坐标(0.0, 0.0)并自动修正
    # go_to_xy_with_correction(ctrl, msg, 0.0, 0.0, speed=0.4, observe=True)
    pass