havoc420ubuntu
a1e9121761
- Comment out run_go_to_xy_example in main.py to streamline execution flow. - Adjust target coordinates in run_go_to_xy_example for improved navigation accuracy. - Refactor run_task_2 to include calls to go_to_xy and turn_degree for enhanced movement control.
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.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 |