路径规划模块

本模块提供基于路网图的路径规划功能，包括路网结构定义和路径查找算法。

核心功能

路网结构

路网图数据结构：

路点定义：

边定义：

路网图构建器：

路径规划器

使用说明

路网图定义

路网图由路点（Waypoint）和边（Edge）组成：

路点（Waypoint）: 表示路网中的节点，可以是位置点或引用已保存的位置
边（Edge）: 表示路点之间的连接关系，支持单向和双向

基本示例

创建路网图：

from daystar_api.lowlevel_skills.path_planner import RoadNetworkGraphBuilder

# 使用构建器创建路网图
builder = RoadNetworkGraphBuilder()

# 添加路点（引用已保存的位置）
builder.add_waypoint("A")
builder.add_waypoint("B")
builder.add_waypoint("C")

# 添加边（双向）
builder.add_edge("A", "B", is_bidirectional=True)
builder.add_edge("B", "C", is_bidirectional=True)

# 构建并保存
graph = builder.build()
graph.to_yaml_file("warehouse_graph.yaml")

加载路网图并规划路径：

from daystar_api.lowlevel_skills.path_planner import PathPlanner, RoadNetworkGraph

# 创建规划器
planner = PathPlanner()

# 加载路网图
graph = RoadNetworkGraph.from_yaml_file("warehouse_graph.yaml")
success, msg, waypoints = planner.load_graph(graph)

if success:
    # 规划路径
    success, path, msg = planner.path_plan(
        start_point="A",
        goal_point="C",
        keypoints=["B"]  # 可选的中间点
    )

    if success:
        print(f"路径: {' -> '.join(path)}")

路点引用方式

路点支持两种定义方式：

引用位置文件：路点 ID 对应已保存的位置文件

# Waypoint(id="A") 将从 /root/data/daystar_api/points/A.yaml 加载位置
builder.add_waypoint("A")

直接指定坐标：用于临时路点

builder.add_waypoint("temp_001", x=1.0, y=2.0, z=0.0)

路网文件格式

YAML 格式

路网图可以用 YAML 格式定义，包含 waypoints 和 edges 两个主要部分。

方式1：引用位置文件（推荐）

waypoints:
  - id: A          # 自动从 /root/data/daystar_api/points/A.yaml 加载
  - id: B
  - id: C
    annotations:
      name: "充电站"

edges:
  - id:
      from_waypoint: A
      to_waypoint: B
    is_bidirectional: true

  - id:
      from_waypoint: B
      to_waypoint: C
    is_bidirectional: true

方式2：直接指定坐标

waypoints:
  - id: wp_001
    position:
      x: -1.28704667
      y: 3.521595
      z: 0.617
    annotations:
      name: "路点1"

  - id: wp_002
    position:
      x: -2.72104716
      y: 2.841408
      z: 0.617
    annotations:
      name: "路点2"

edges:
  - id:
      from_waypoint: wp_001
      to_waypoint: wp_002
    is_bidirectional: true
    weight: 2.5    # 可选：指定边的权重

字段说明

waypoints 字段：

id (必填): 路点唯一标识符
position (可选): 位置坐标，包含 x, y, z 字段
orientation (可选): 姿态四元数，包含 x, y, z, w 字段
annotations (可选): 注释信息，可包含 name, description 等

edges 字段：

id.from_waypoint (必填): 起始路点 ID
id.to_waypoint (必填): 目标路点 ID
is_bidirectional (可选): 是否为双向边，默认 true
weight (可选): 边的权重，用于路径规划代价计算
metadata (可选): 其他元数据