路径规划接口

介绍

路径规划接口：提供基于路网图（Road Network Graph）的路径规划功能。用于在预定义的路网结构上查找两点之间的最优路径，适用于大型场景中的多节点导航规划。

功能	Python
构建路网图	✔
加载/保存路网图	✔
路径规划	✔

路网结构

Python接口

RoadNetworkGraphBuilder

功能说明
路网图构建器，使用链式方法添加路点和边，最终通过 build() 生成 RoadNetworkGraph。

调用示例

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")

主要方法：

方法	说明
add_waypoint(id, x=None, y=None, z=None)	添加路点。id 对应已保存点位名称，或指定坐标创建临时路点。
add_edge(from_wp, to_wp, is_bidirectional=True, weight=None)	添加路点之间的连接边，weight 为可选代价权重。
build()	生成 RoadNetworkGraph 对象。

---

RoadNetworkGraph

功能说明
路网图数据结构，包含路点和边的完整定义。支持从 YAML 文件加载和保存。

调用示例

from daystar_api.lowlevel_skills.path_planner import RoadNetworkGraph

# 从 YAML 文件加载路网图
graph = RoadNetworkGraph.from_yaml_file("warehouse_graph.yaml")

# 保存到文件
graph.to_yaml_file("updated_graph.yaml")

主要方法：

方法	说明
from_yaml_file(path)	从 YAML 文件加载路网图（类方法）。
to_yaml_file(path)	将路网图保存到 YAML 文件。
add_waypoint(id, ...)	添加路点。
add_edge(from_wp, to_wp, ...)	添加边。
get_waypoint(id)	获取指定路点信息。
validate()	验证路网图数据是否合法。

路径规划器

Python接口

PathPlanner

功能说明
基于已加载路网图执行最短路径规划。加载路网图后，调用 path_plan 查找起点到终点之间的最优路径（经过可选中间节点）。

调用示例

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:
    # 规划从 A 到 C 的路径，经过中间节点 B
    success, path, msg = planner.path_plan(
        start_point="A",
        goal_point="C",
        keypoints=["B"]  # 可选中间节点
    )

    if success:
        print(f"规划路径: {' -> '.join(path)}")
        # 按顺序导航各路点
        from daystar_api.lowlevel_skills import navigation_to_location
        for point in path:
            navigation_to_location(point)

主要方法：

方法	返回值	说明
load_graph(graph)	(success: bool, msg: str, waypoints: list)	加载路网图到规划器。
path_plan(start_point, goal_point, keypoints=[])	(success: bool, path: list[str], msg: str)	规划路径，返回经过的路点 ID 列表（含起点和终点）。

---

路网 YAML 格式说明

路网图使用 YAML 格式定义：

# 方式1：引用已保存的点位（推荐）
waypoints:
  - id: A          # 自动从 /root/data/daystar_api/points/A.yaml 加载位置
  - id: B
  - id: C

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.28
      y: 3.52
      z: 0.0