路径规划模块

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

核心功能

路网结构

路网图数据结构：

底层 ROS2 接口

本类型不经 ROS2 通道（纯本地实现：内存数据结构 + 本地 YAML 文件读写—— from_yaml_file / to_yaml_file 对相对路径经 daystar_api.config 解析到路网目录 $DAYSTAR_DATA_ROOT/graph/，默认 /root/data/daystar_api/graph/）。

路点定义：

底层 ROS2 接口

本类型不经 ROS2 通道（纯数据类；position 为空的引用型路点在图加载/规划时由 PathPlanner 从本地点位文件 $DAYSTAR_DATA_ROOT/points/<id>.yaml 读取位姿，仍为本地文件读取）。

边定义：

底层 ROS2 接口

本类型不经 ROS2 通道（纯数据类，仅在内存中表示路点间连接关系）。

路网图构建器：

底层 ROS2 接口

本类型不经 ROS2 通道（纯本地实现：在内存中构建路网； add_waypoint(from_location_file=True) 时从本地点位文件 $DAYSTAR_DATA_ROOT/points/<waypoint_id>.yaml 立即读取位姿）。

路径规划器

底层 ROS2 接口

本类型不经 ROS2 通道（纯本地算法实现：基于 networkx / scipy 及本地 path_planer_api 扩展做图搜索；load_graph 对引用型路点经 config.get_location_file_path 从 $DAYSTAR_DATA_ROOT/points/<name>.yaml 读取坐标，均为本地文件访问）。

Note

邻接矩阵为**纯有向**（双向边在解析期展开为两条有向边），边权优先取路网 yaml 的 weight（如 A* 边的实际路程），缺失时回退欧氏直线距离。 path_plan(keypoints=...) 传入两个以上关键点时使用 PyLKH ATSP 重排求最短总程。

底层 ROS2 接口

本函数不经 ROS2 通道（纯本地实现：按（绝对路径, mtime, size）指纹缓存已加载的 PathPlanner 实例，路网文件重建后自动失效重载）。

使用说明

路网图定义

路网图由路点（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_DIR 解析——纯名称走
# <当前地图>/ 优先、回退一级目录，带前缀（"office/route_a"）精确指定
graph = RoadNetworkGraph.from_yaml_file("warehouse/route_a.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 (可选): 其他元数据。自动路网的 A* 边在此携带 path_points: [[x, y], ...]（绕行拐点序列，方向与边方向一致；反向边存逆序），go_to_location 执行时自动注入为中间位姿

自动路网生成与可视化

无手动路网（graph/<map>/default.yaml）时，go_to_location(auto_graph_planning=True) 按当前地图的避障数据源自动生成 graph/<map>/auto_generated.yaml（带指纹缓存，地图/点位/生成配置变化自动重建）：

避障数据源：ground.pcd 可行域点云优先（有点=可走地面，无 PNG 黑像素歧义），缺失/损坏回退 map.png 占据栅格。
建边三级判据：直线可通 → 管内小幅擦挡可绕（半宽 edge_bypass_halfwidth_m）→ 可行域栅格 A* 可达（仅 pcd 数据源启用；上限 astar_max_expand）。A* 边写实距 weight 并把绕行拐点挂到 metadata.path_points。三级全败的点对保持断连，规划不出路时报错（不静默直走穿墙）。
生成配置：config/graph_planning.yaml（clearance_m / noise_min_area_m2 / edge_bypass_halfwidth_m / pcd_resolution_m / pcd_free_dilation_m / astar_max_expand，env DAYSTAR_GRAPH_* 可逐项覆盖）。

底层 ROS2 接口

本函数不经 ROS2 通道（纯本地实现：读路网 yaml 与地图目录避障数据源，PIL 绘制 png）。

命令行用法（真机调试一条命令出图）:

python -m daystar_api.lowlevel_skills.path_planner.render \
    --graph auto_generated --map-dir /root/data/daystar_api/maps/<图名> \
    [--out /tmp/g.png] [--path A,B,C] [--nav-path latest]

--nav-path latest 叠画最近一次 go_to_location 实际下发的路径（graph/<map>/path/nav_path_latest.yaml，每次导航自动落盘并后台渲染时间戳 png，保留最近 10 张；env DAYSTAR_NAV_PATH_RENDER=0 关闭）。