105 lines
3.6 KiB
Python
105 lines
3.6 KiB
Python
|
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.8, 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
|