mi-task/task_test/go_to_xy_example.py

import time
import sys
import os

# 添加父目录到路径，以便能够导入相关模块
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from base_move.go_to_xy import go_to_xy, go_to_xy_with_correction
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

# 创建本模块特定的日志记录器
logger = get_logger("坐标导航示例")

def run_go_to_xy_example(ctrl, msg):
    """
    坐标导航示例任务：展示如何使用go_to_xy功能实现精确导航
    
    参数:
    ctrl: Robot_Ctrl 对象
    msg: robot_control_cmd_lcmt 对象
    """
    section('坐标导航示例任务', "启动")
    info('开始执行坐标导航示例...', "启动")
    
    
    try:
        # 示例1：简单导航到一个目标点
        section('示例1：简单导航', "开始")
        target_x, target_y = 0.85, 0.2  # 前进1米
        info(f"移动到坐标点: ({target_x}, {target_y})", "目标")
        
        nav_success = go_to_xy(ctrl, msg, target_x, target_y, speed=0.5, observe=True)
        
        if nav_success:
            success("成功到达目标点1", "完成")
        else:
            warning("未能精确到达目标点1", "警告")

        return        
        # 等待2秒
        time.sleep(2)
        
        # 示例2：使用自动修正的方式导航到目标点
        section('示例2：带修正的导航', "开始")
        target_x, target_y = 0.0, 1.0  # 左转并前进1米
        info(f"移动到坐标点: ({target_x}, {target_y})", "目标")
        
        nav_success = go_to_xy_with_correction(
            ctrl, msg, target_x, target_y, 
            speed=0.4, precision=True, max_attempts=2, observe=True
        )
        
        if nav_success:
            success("成功到达目标点2", "完成")
        else:
            warning("未能精确到达目标点2", "警告")
        
        # 等待2秒
        time.sleep(2)
        
        # 示例3：使用坐标导航绕行矩形路径
        section('示例3：矩形路径导航', "开始")
        info("执行矩形路径导航", "路径")
        
        # 定义矩形四个顶点的坐标
        rectangle_points = [
            (0.0, 0.0),   # 起点/终点
            (1.0, 0.0),   # 右侧点
            (1.0, 1.0),   # 右上角点
            (0.0, 1.0),   # 左上角点
        ]
        
        # 依次导航到各个顶点
        for i, (point_x, point_y) in enumerate(rectangle_points):
            section(f'导航到矩形顶点 {i+1}/4', "移动")
            info(f"目标坐标: ({point_x}, {point_y})", "目标")
            
            nav_success = go_to_xy_with_correction(
                ctrl, msg, point_x, point_y, 
                speed=0.5, precision=True, observe=True
            )
            
            if nav_success:
                success(f"成功到达顶点 {i+1}", "完成")
            else:
                warning(f"未能精确到达顶点 {i+1}", "警告")
            
            # 每个点之间短暂停顿
            time.sleep(1)
        
        # 示例完成，回到起点
        section('示例任务完成', "结束")
        info("坐标导航示例任务完成", "完成")
        
        # 复位里程计（可选）
        # ctrl.odo_reset()
        
    except Exception as e:
        error(f"执行过程中发生错误: {e}", "错误")
    finally:
        # 确保机器人停止
        ctrl.base_msg.stop_smooth()

if __name__ == "__main__":
    # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象
    pass