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