添加了新的planner，未实际实现过，需进行测试

src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md

@@ -0,0 +1,500 @@
+# 轻量阿克曼 Hybrid A* 路径规划方案说明
+
+## 1. 方案目标
+
+本方案用于阿克曼底盘小车的轻量路径规划，重点满足以下要求：
+
+- 考虑小车最小转弯半径。
+- 支持前进、倒车和转向。
+- 支持运动过程中动态添加锥桶障碍物。
+- 支持锥桶障碍物累计保留和去重。
+- 使用融合后的定位和代价地图，不直接处理雷达、相机、IMU、轮式里程计原始数据。
+- 输出仍保持 ROS 标准 `/plan nav_msgs/Path`，避免引入复杂自定义消息。
+
+当前实现是在 `planner` 包已有代码基础上升级完成的，核心规划器仍使用原可执行文件名 `grid_astar_theta_node`，但内部已经从二维 A*/Theta* 改为轻量阿克曼 Hybrid A*/State Lattice。
+
+## 2. 总体架构
+
+系统由两个主要节点组成：
+
+| 节点 | 文件 | 作用 |
+|---|---|---|
+| `grid_astar_theta_planner` | `src/planner/src/grid_astar_theta_node.cpp` | 轻量 Hybrid A* 路径规划，融合 costmap 和锥桶障碍物，输出 `/plan` |
+| `topology_pure_pursuit` | `src/planner/scripts/topology_pure_pursuit_node.py` | 跟踪 `/plan`，根据路径 yaw 判断前进或倒车，输出 `/cmd_vel` |
+
+典型数据流：
+
+```text
+/odom
+/goal_pose
+/global_costmap/costmap
+/random_obstacles
+        |
+        v
+grid_astar_theta_planner
+        |
+        v
+/plan
+        |
+        v
+topology_pure_pursuit
+        |
+        v
+/cmd_vel
+        |
+        v
+cmd_vel_to_ackermann_drive.py
+        |
+        v
+/ackermann_cmd
+```
+
+## 3. 输入与输出
+
+### 3.1 规划器输入
+
+| Topic | 类型 | 说明 |
+|---|---|---|
+| `/odom` | `nav_msgs/Odometry` | 小车当前融合定位，至少需要 `x, y, yaw` |
+| `/goal_pose` | `geometry_msgs/PoseStamped` | 目标位姿，必须包含 `x, y, yaw` |
+| `/global_costmap/costmap` | `nav_msgs/OccupancyGrid` | 全局代价地图 |
+| `/random_obstacles` | `std_msgs/Float32MultiArray` | 锥桶障碍物，格式 `[x, y, radius, ...]` |
+| `/clear_random_obstacles` | `std_msgs/Bool` | 清空已累计锥桶 |
+
+所有坐标默认工作在 `map` 坐标系下。如果 `/odom` 或 `/goal_pose` 的 `frame_id` 不是 `map`，则必须存在 TF 变换。
+
+### 3.2 规划器输出
+
+| Topic | 类型 | 说明 |
+|---|---|---|
+| `/plan` | `nav_msgs/Path` | 规划结果 |
+| `/ackermann_lattice_status` | `std_msgs/String` | 规划状态和失败原因 |
+
+`/plan` 的每个 Pose 有特殊语义：
+
+- `pose.position.x/y` 表示轨迹点位置。
+- `pose.orientation.yaw` 表示小车车身朝向。
+- 如果路径点前进方向与车身 yaw 相反，则该段表示倒车。
+
+## 4. 代价地图处理
+
+规划器订阅 `/global_costmap/costmap`，类型为 `nav_msgs/OccupancyGrid`。
+
+默认规则：
+
+| costmap 值 | 解释 |
+|---|---|
+| `>= 50` | 障碍物 |
+| `-1` | 未知区域，默认视为障碍 |
+| `< 50` 且非 `-1` | 可通行 |
+
+相关参数：
+
+```yaml
+costmap_topic: /global_costmap/costmap
+costmap_occupied_threshold: 50
+unknown_is_obstacle: true
+outside_costmap_is_obstacle: true
+```
+
+当前规划器内部搜索分辨率默认是 `0.10m`。如果收到 costmap，且 `fit_map_to_costmap: true`，规划器会用 costmap 的物理边界更新规划范围。
+
+需要注意：
+
+- costmap 的实际像素宽高以运行时 `OccupancyGrid.info.width/height` 为准。
+- 静态地图 `my_map.pgm` 是 `199 x 266` 像素，分辨率 `0.05m/像素`。
+- 静态地图 `zhihui.pgm` 是 `109 x 115` 像素，分辨率 `0.05m/像素`。
+
+运行时查看 costmap 信息：
+
+```bash
+ros2 topic echo --once /global_costmap/costmap --field info
+```
+
+## 5. 锥桶障碍物处理
+
+锥桶输入 topic：
+
+```text
+/random_obstacles
+```
+
+消息类型：
+
+```text
+std_msgs/Float32MultiArray
+```
+
+数据格式：
+
+```text
+[x, y, radius, x, y, radius, ...]
+```
+
+示例：
+
+```bash
+ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
+"{data: [1.2, 0.6, 0.15]}"
+```
+
+### 5.1 增量添加
+
+当前默认开启：
+
+```yaml
+accumulate_obstacles: true
+```
+
+这表示每次发布新锥桶后，规划器不会清空旧锥桶，而是将新锥桶合并到已有锥桶集合中。
+
+例如先发布：
+
+```bash
+ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
+"{data: [1.2, 0.6, 0.15]}"
+```
+
+再发布：
+
+```bash
+ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
+"{data: [2.0, 1.1, 0.15]}"
+```
+
+规划器内部会保留两个锥桶。
+
+### 5.2 去重规则
+
+默认参数：
+
+```yaml
+obstacle_merge_distance: 0.20
+```
+
+如果新锥桶中心与已有锥桶中心距离小于 `0.20m`，认为是同一个锥桶：
+
+- 更新锥桶中心位置。
+- 半径取新旧较大值。
+
+### 5.3 清空锥桶
+
+如果误检或需要重新开始累计，可以发布：
+
+```bash
+ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}"
+```
+
+## 6. 障碍物膨胀
+
+锥桶障碍物不会只按原始半径参与碰撞检测，而是会进行安全膨胀。
+
+膨胀半径计算：
+
+```text
+inflated_radius =
+  max(obstacle_radius, unknown_obstacle_radius)
+  + robot_radius
+  + localization_error
+  + latency_margin
+  + safety_margin
+```
+
+默认参数：
+
+```yaml
+default_obstacle_radius: 0.15
+unknown_obstacle_radius: 0.15
+robot_radius: 0.14
+localization_error: 0.10
+latency_margin: 0.05
+safety_margin: 0.05
+```
+
+按默认值，一个半径 `0.15m` 的锥桶最终膨胀半径约为：
+
+```text
+0.15 + 0.14 + 0.10 + 0.05 + 0.05 = 0.49m
+```
+
+这个值偏保守，适合定位误差较大或控制延迟明显的情况。如果实际通道较窄，可以降低 `localization_error` 或 `safety_margin`。
+
+## 7. Hybrid A* 搜索模型
+
+规划器状态为：
+
+```text
+x, y, yaw, gear
+```
+
+其中：
+
+- `x, y` 是位置。
+- `yaw` 是车身朝向。
+- `gear` 是挡位方向，包含前进和倒车。
+
+### 7.1 状态离散
+
+默认参数：
+
+```yaml
+resolution: 0.10
+yaw_bins: 72
+```
+
+含义：
+
+- 平面位置按 `0.10m` 分辨率离散。
+- yaw 被离散成 72 份，即每份约 5 度。
+
+### 7.2 运动原语
+
+每次扩展使用 6 类动作：
+
+| gear | steering | 含义 |
+|---|---|---|
+| 前进 | 直行 | 向前直行 |
+| 前进 | 左转 | 向前左转 |
+| 前进 | 右转 | 向前右转 |
+| 倒车 | 直行 | 向后直行 |
+| 倒车 | 左转 | 向后左转 |
+| 倒车 | 右转 | 向后右转 |
+
+默认运动原语长度：
+
+```yaml
+primitive_length: 0.12
+```
+
+碰撞检测采样间隔：
+
+```yaml
+collision_sample_step: 0.03
+```
+
+也就是说每条原语会被分成多个点做碰撞检测，而不是只检测终点。
+
+### 7.3 最小转弯半径
+
+默认：
+
+```yaml
+min_turning_radius: 0.40
+```
+
+转弯原语按该半径积分生成，规划阶段会保证轨迹曲率不小于这个半径限制。
+
+## 8. 目标判定
+
+目标输入为 `/goal_pose`，必须包含 `x, y, yaw`。
+
+默认到达条件：
+
+```yaml
+goal_xy_tolerance: 0.20
+goal_yaw_tolerance: 0.174533
+```
+
+含义：
+
+- 位置误差小于 `0.20m`。
+- 朝向误差小于约 10 度。
+
+如果目标只给 `x, y` 而 yaw 没有意义，那么最终车头方向可能不符合预期。因此对于阿克曼小车，建议目标始终提供合理 yaw。
+
+## 9. 代价函数
+
+搜索过程中使用以下代价项：
+
+```yaml
+reverse_penalty: 1.3
+turn_penalty: 0.05
+gear_change_penalty: 0.20
+steering_change_penalty: 0.02
+```
+
+含义：
+
+| 参数 | 作用 |
+|---|---|
+| `reverse_penalty` | 倒车惩罚，越大越不愿意倒车 |
+| `turn_penalty` | 转弯惩罚，减少不必要转向 |
+| `gear_change_penalty` | 前进/倒车切换惩罚，减少频繁换挡 |
+| `steering_change_penalty` | 转向变化惩罚，让路径更稳定 |
+
+当前默认允许倒车，但不会无理由倒车。只有倒车明显降低路径代价或满足空间约束时，才更容易出现倒车段。
+
+## 10. 重规划机制
+
+重规划触发条件：
+
+- 收到新的 `/goal_pose`。
+- 收到新的 `/global_costmap/costmap`。
+- 收到新的 `/random_obstacles`。
+- 收到清空锥桶命令。
+
+新目标会立即规划。
+
+障碍物和 costmap 更新会通过定时器限频：
+
+```yaml
+replan_rate: 2.0
+```
+
+表示最多 2Hz 重规划，避免障碍物高频更新导致 CPU 被规划器占满。
+
+单次规划时间预算：
+
+```yaml
+planning_timeout_ms: 100.0
+```
+
+如果超过该时间还没有找到路径，则本次规划失败。
+
+失败策略：
+
+- 如果旧路径仍然没有碰撞，则继续使用旧路径。
+- 如果旧路径已被障碍物挡住，则发布空 `/plan`，跟踪器停车。
+
+## 11. 跟踪器与倒车执行
+
+跟踪器读取 `/plan`，并根据路径点 yaw 判断当前段是前进还是倒车。
+
+判断逻辑：
+
+```text
+如果 路径段方向 与 车身 yaw 方向相反，则认为是倒车段
+```
+
+默认速度：
+
+```yaml
+linear_speed: 0.30
+reverse_speed: 0.12
+```
+
+前进段：
+
+```text
+/cmd_vel.linear.x > 0
+```
+
+倒车段：
+
+```text
+/cmd_vel.linear.x < 0
+```
+
+之后由现有 `cmd_vel_to_ackermann_drive.py` 桥接到 `/ackermann_cmd`。
+
+## 12. 启动方式
+
+构建：
+
+```bash
+colcon build --packages-select planner
+source install/setup.bash
+```
+
+启动规划器和跟踪器：
+
+```bash
+ros2 launch planner grid_astar_theta_with_tracker.launch.py
+```
+
+发布目标：
+
+```bash
+ros2 topic pub -1 /goal_pose geometry_msgs/msg/PoseStamped \
+"{header: {frame_id: map}, pose: {position: {x: 3.0, y: 1.5, z: 0.0}, orientation: {w: 1.0}}}"
+```
+
+发布锥桶：
+
+```bash
+ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
+"{data: [1.2, 0.6, 0.15]}"
+```
+
+清空锥桶：
+
+```bash
+ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}"
+```
+
+查看规划输出：
+
+```bash
+ros2 topic echo /plan
+```
+
+查看状态：
+
+```bash
+ros2 topic echo /ackermann_lattice_status
+```
+
+## 13. 当前方案优点
+
+- 轻量，主要逻辑是 C++ 实现。
+- 不依赖高频雷达融合或复杂局部规划器。
+- 规划阶段考虑阿克曼最小转弯半径。
+- 支持倒车和换挡。
+- 支持运动过程中动态添加锥桶。
+- 支持锥桶累计、去重和清空。
+- 仍使用标准 `/plan nav_msgs/Path` 输出，接口简单。
+
+## 14. 当前限制
+
+- `/plan` 本身没有显式 gear 字段，倒车语义依赖 Pose yaw 约定。
+- 目前没有单独实现局部地图或局部避障器。
+- costmap 的动态障碍处理依赖上游是否正确发布 `/global_costmap/costmap`。
+- Hybrid A* 是轻量版本，不包含复杂解析扩展或高级平滑器。
+- 如果地图非常大，`100ms` 超时可能导致复杂场景找不到路径，需要调整分辨率、搜索范围或超时时间。
+
+## 15. 推荐调参顺序
+
+建议按以下顺序调参：
+
+1. `min_turning_radius`
+2. `robot_radius`
+3. `localization_error`
+4. `safety_margin`
+5. `obstacle_merge_distance`
+6. `primitive_length`
+7. `planning_timeout_ms`
+8. `reverse_penalty`
+9. `lookahead_distance`
+10. `reverse_speed`
+
+如果小车过于保守、容易找不到路，优先降低：
+
+```yaml
+localization_error
+safety_margin
+unknown_is_obstacle
+```
+
+如果小车离障碍物太近，优先增加：
+
+```yaml
+robot_radius
+safety_margin
+localization_error
+```
+
+如果规划太慢，优先调整：
+
+```yaml
+resolution
+planning_timeout_ms
+map_min_x/map_max_x/map_min_y/map_max_y
+```
+
+## 16. 总结
+
+这套方案本质上是一个面向低算力阿克曼小车的轻量 Hybrid A* 规划框架。它不处理底层传感器融合，只消费已经融合好的定位、代价地图和锥桶全局坐标。
+
+相比普通二维 A*，它在规划阶段加入了车身朝向、最小转弯半径、前进/倒车和换挡代价，因此输出路径更符合阿克曼底盘运动约束。
+
+相比完整 Nav2 Hybrid A* 或 MPPI，它更轻量、接口更简单、可控性更强，适合竞赛场景中地图规模较小、障碍物数量有限、需要快速重规划的任务。

src/planner/CMakeLists.txt

@@ -10,11 +10,36 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 # Dummy executable to satisfy ament_cmake + install of launch/config
 add_executable(planner_version src/planner_version.cpp)
+add_executable(grid_astar_theta_node src/grid_astar_theta_node.cpp)
+ament_target_dependencies(grid_astar_theta_node
+  geometry_msgs
+  nav_msgs
+  rclcpp
+  std_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+)
 
-install(TARGETS planner_version
+install(TARGETS
+  planner_version
+  grid_astar_theta_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  scripts/topology_pure_pursuit_node.py
+  scripts/grid_astar_theta_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -23,6 +48,11 @@ install(
   DESTINATION share/${PROJECT_NAME}
 )
 
+install(FILES
+  README.md
+  DESTINATION share/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)

src/planner/README.md

@@ -0,0 +1,116 @@
+# Lightweight Planning
+
+This package keeps the existing Nav2 minimal planner launch, and adds a lighter
+fixed-field option:
+
+1. Use a sparse topology graph for fixed venues.
+2. Publish a densified `nav_msgs/Path`.
+3. Track it with Pure Pursuit at a low, deterministic control cost.
+
+The node is intended for cases where localization noise and runtime delay make
+high-batch MPPI difficult to tune on the robot.
+
+For random cone obstacles, the package includes a lightweight Ackermann Hybrid
+A*/state-lattice planner implemented in C++. It consumes the fused robot pose,
+target pose, OccupancyGrid costmap, and optional cone list, then publishes
+`/plan` for the tracker. The executable name is kept as
+`grid_astar_theta_node` for launch compatibility, but the planner is no longer
+2D A*/Theta*.
+
+## Run
+
+```bash
+colcon build --packages-select planner
+source install/setup.bash
+ros2 launch planner topology_pure_pursuit.launch.py
+```
+
+Send a graph-node target:
+
+```bash
+ros2 topic pub -1 /topology_goal std_msgs/msg/String "{data: bed_b}"
+```
+
+Or send `/goal_pose` near a graph node; the nearest node is selected.
+
+## Random Obstacles / Ackermann Hybrid A*
+
+Run the grid planner and tracker together:
+
+```bash
+ros2 launch planner grid_astar_theta_with_tracker.launch.py
+```
+
+Publish detected cone obstacles as a flat `Float32MultiArray`:
+
+```bash
+ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
+  "{data: [1.2, 0.6, 0.15, 2.1, 1.4, 0.20]}"
+```
+
+By default, `/random_obstacles` is incremental. New cones are merged into the
+stored cone set, and old cones remain. Detections within
+`obstacle_merge_distance` are treated as the same cone and update its position.
+Clear stored cones when needed:
+
+```bash
+ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}"
+```
+
+Send a target:
+
+```bash
+ros2 topic pub -1 /goal_pose geometry_msgs/msg/PoseStamped \
+  "{header: {frame_id: map}, pose: {position: {x: 3.0, y: 1.5}, orientation: {w: 1.0}}}"
+```
+
+The planner also subscribes to `/global_costmap/costmap` as
+`nav_msgs/OccupancyGrid`. Occupied cells (`>= 50`) and unknown cells (`-1`) are
+blocked by default.
+
+The planner publishes `/plan` as `nav_msgs/Path`; each pose orientation is the
+vehicle body yaw. If the segment direction is opposite that yaw, the tracker
+treats that segment as reverse and publishes negative `/cmd_vel.linear.x`.
+
+## Files
+
+- `config/topology_graph.yaml`: fixed-field topology graph. Replace the sample
+  coordinates with measured centerline coordinates in the map frame.
+- `config/topology_pure_pursuit.yaml`: tracking, delay compensation, and topic
+  parameters.
+- `launch/topology_pure_pursuit.launch.py`: standalone launch for the topology
+  planner and Pure Pursuit tracker.
+- `config/grid_astar_theta.yaml`: costmap fusion, cone inflation, and Ackermann
+  Hybrid A* search parameters.
+- `launch/grid_astar_theta_with_tracker.launch.py`: random-obstacle Ackermann
+  planner plus yaw-aware tracker.
+- `src/grid_astar_theta_node.cpp`: OccupancyGrid fusion, cone inflation,
+  Hybrid A* search, and `/plan` publishing.
+- `scripts/grid_astar_theta_node.py`: Python reference version kept for quick
+  experiments; launch files use the C++ executable by default.
+- `scripts/topology_pure_pursuit_node.py`: Dijkstra graph search, external
+  `/plan` tracking, yaw-aware reverse handling, TF-aware pose handling, and
+  latency compensation.
+
+## Operating Notes
+
+- Do not run this node's `/cmd_vel` output together with Nav2
+  `controller_server`; choose one command publisher.
+- If the graph is in `map` and odometry is in `odom`, keep TF available. The node
+  transforms odometry poses into `map_frame`.
+- Increase `nearest_node_max_distance`, `goal_tolerance`, and graph clearance
+  when localization is noisy.
+- Tune `lookahead_distance`, `min_turning_radius`, and `reverse_speed` first for
+  Ackermann corner and reverse behavior.
+- If the base expects Ackermann commands, run the existing
+  `cmd_vel_to_ackermann_drive.py` bridge and configure the base driver
+  accordingly.
+
+## Planner Semantics
+
+- Minimum turning radius defaults to `0.40 m`.
+- `/goal_pose` uses `x`, `y`, and `yaw`; yaw tolerance defaults to 10 degrees.
+- Replanning is immediate on new goals and limited to 2 Hz for costmap or
+  accumulated cone updates.
+- If new planning fails, the last collision-free path is kept; if it is no
+  longer safe, an empty `/plan` is published so the tracker stops.

src/planner/config/grid_astar_theta.yaml

@@ -0,0 +1,64 @@
+grid_astar_theta_planner:
+  ros__parameters:
+    use_sim_time: false
+
+    # Inputs:
+    # - /odom gives the fused current robot pose.
+    # - /goal_pose gives the target pose (x, y, yaw).
+    # - /global_costmap/costmap is nav_msgs/OccupancyGrid.
+    # - /random_obstacles is std_msgs/Float32MultiArray.
+    #   obstacle_format=xyr means [x, y, radius, x, y, radius, ...].
+    map_frame: map
+    odom_topic: /odom
+    goal_pose_topic: /goal_pose
+    obstacles_topic: /random_obstacles
+    clear_obstacles_topic: /clear_random_obstacles
+    costmap_topic: /global_costmap/costmap
+    plan_topic: /plan
+    status_topic: /ackermann_lattice_status
+
+    # Fallback map bounds in map_frame. When a costmap arrives in map frame,
+    # bounds are taken from that OccupancyGrid by default.
+    map_min_x: -5.0
+    map_min_y: -5.0
+    map_max_x: 5.0
+    map_max_y: 5.0
+    resolution: 0.10
+    fit_map_to_costmap: true
+
+    # OccupancyGrid fusion. Unknown is blocked by default for safety.
+    costmap_occupied_threshold: 50
+    unknown_is_obstacle: true
+    outside_costmap_is_obstacle: true
+
+    # Random circular cone obstacles. Inflation is:
+    # max(obstacle_radius, unknown_obstacle_radius)
+    # + robot_radius + localization_error + latency_margin + safety_margin.
+    obstacle_format: xyr
+    default_obstacle_radius: 0.15
+    unknown_obstacle_radius: 0.15
+    accumulate_obstacles: true
+    obstacle_merge_distance: 0.20
+    robot_radius: 0.14
+    localization_error: 0.10
+    latency_margin: 0.05
+    safety_margin: 0.05
+
+    # Lightweight Hybrid A*/state-lattice search.
+    min_turning_radius: 0.40
+    yaw_bins: 72
+    primitive_length: 0.12
+    collision_sample_step: 0.03
+    goal_xy_tolerance: 0.20
+    goal_yaw_tolerance: 0.174533
+    reverse_penalty: 1.3
+    turn_penalty: 0.05
+    gear_change_penalty: 0.20
+    steering_change_penalty: 0.02
+    plan_on_obstacle_update: true
+    max_iterations: 50000
+    planning_timeout_ms: 100.0
+    replan_rate: 2.0
+
+    max_odom_age: 0.80
+    tf_timeout: 0.05

src/planner/config/planner.yaml

@@ -1,399 +1,232 @@
 # ============================================================================
-# planner.yaml - Nav2 minimal global planning stack
+# planner.yaml — Nav2 minimal global planning stack
 # 节点: slam_toolbox + planner_server + global_costmap + local_costmap
-# 用途: 启动建图、全局规划、全局代价地图和局部代价地图所需的 ROS2 参数。
-# 说明: 本文件只配置参数；实际由 launch 文件把这些参数加载到对应节点。
+# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
 # ============================================================================
 
 # ---------------------------------------------------------------------------
-# slam_toolbox - online_async 实时建图
-# 主要作用:
-#   1. 订阅激光 /scan 和里程计 TF。
-#   2. 生成 /map 占用栅格地图。
-#   3. 发布 map -> odom 变换，使 map 坐标系和机器人 odom/base_link 连接起来。
+# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
+# 来源: slam_toolbox_mapping.yaml
 # ---------------------------------------------------------------------------
 slam_toolbox:
   ros__parameters:
-    # 是否使用仿真时间 /clock。真实车通常为 False；Gazebo/rosbag 回放通常为 True。
     use_sim_time: False
 
     # Solver
-    # 后端图优化求解器插件。CeresSolver 用 Ceres 做位姿图优化，是 slam_toolbox 常用选择。
     solver_plugin: solver_plugins::CeresSolver
-    # Ceres 线性求解器。SPARSE_NORMAL_CHOLESKY 适合稀疏 SLAM 图，速度和精度较均衡。
     ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
-    # Ceres 预条件器。SCHUR_JACOBI 常用于 bundle/pose graph 类问题，加速迭代收敛。
     ceres_preconditioner: SCHUR_JACOBI
-    # 信赖域策略。LEVENBERG_MARQUARDT 稳定性好，适合非线性最小二乘优化。
     ceres_trust_strategy: LEVENBERG_MARQUARDT
-    # dogleg 策略类型；仅在 dogleg 信赖域策略启用时影响优化路径，这里保留默认传统形式。
     ceres_dogleg_type: TRADITIONAL_DOGLEG
-    # 鲁棒核函数。None 表示不额外压制离群约束；激光匹配质量差时可考虑鲁棒 loss。
     ceres_loss_function: None
 
     # ROS base
-    # 里程计坐标系名。slam_toolbox 需要 TF 中存在 odom -> base_link 或等价链路。
     odom_frame: odom
-    # 地图坐标系名。/map 消息和 map -> odom TF 都以这个坐标系作为全局参考。
     map_frame: map
-    # 机器人车体坐标系名。用于从 TF 读取机器人当前在 odom/map 下的位姿。
     base_frame: base_link
-    # 激光雷达话题。slam_toolbox 从这里取 LaserScan 做 scan matching 和建图。
     scan_topic: /scan
-    # SLAM 工作模式。mapping 表示在线建图；localization 表示基于已有地图定位。
     mode: mapping
-    # 是否启用地图保存相关服务/能力，便于后续保存当前构建出的地图。
     use_map_saver: true
 
     # Debug & performance
-    # 是否输出详细调试日志。排查匹配/闭环问题时可开；平时关闭减少日志量。
     debug_logging: false
-    # 激光降采样倍率。1 表示每帧都处理；增大可降负载但地图更新更慢。
-    throttle_scans: 2
-    # 发布 map -> odom TF 的周期，单位秒。0.02 表示约 50Hz，越小 TF 越实时但 CPU 占用更高。
+    throttle_scans: 1
     transform_publish_period: 0.02
-    # /map 地图更新周期，单位秒。数值越小地图刷新越频繁，但计算/网络开销更高。
-    map_update_interval: 1.0
-    # 地图分辨率，单位 m/cell。0.05 表示每个栅格 5cm；越小越精细但地图更大。
+    map_update_interval: 3.0
     resolution: 0.05
-    # 接受的最小激光距离，小于该距离的点会被过滤，避免近距离噪声/车体反射。
     min_laser_range: 0.15
-    # 接受的最大激光距离，超过该距离的点会被过滤，避免远距离弱回波干扰。
     max_laser_range: 20.0
-    # 两次处理激光之间的最小时间间隔，单位秒；用于限制处理频率。
     minimum_time_interval: 0.5
-    # 查询 TF 的超时时间，单位秒；TF 延迟超过该值会导致当前帧无法处理。
     transform_timeout: 0.2
-    # TF 缓冲时间长度，单位秒；越大越能容忍延迟数据，但内存占用增加。
-    tf_buffer_duration: 10.0
-    # slam_toolbox 内部线程栈大小；地图/图优化规模大时需要足够栈空间。
+    tf_buffer_duration: 30.0
     stack_size_to_use: 40000000
-    # 是否启用交互模式，允许通过 RViz/服务交互式调整图节点或执行手动操作。
     enable_interactive_mode: true
 
     # Mapping
-    # 是否使用 scan matching。开启后根据激光匹配修正里程计漂移，是建图核心能力。
     use_scan_matching: true
-    # 是否使用激光点云重心辅助匹配，可改善局部匹配初值稳定性。
     use_scan_barycenter: true
-    # 机器人至少移动多少米才添加/处理新的位姿节点；越小图更密，越大负载更低。
     minimum_travel_distance: 0.5
-    # 机器人至少转动多少弧度才添加/处理新的位姿节点；越小对转弯更敏感。
     minimum_travel_heading: 0.1
-    # scan buffer 中保留的扫描帧数量，用于局部匹配和链路建立。
     scan_buffer_size: 10
-    # scan buffer 中扫描之间允许的最大距离，超过该距离的历史帧不再用于匹配。
-    scan_buffer_maximum_scan_distance: 5.0
-    # 精匹配响应最低阈值；低于该值的局部链路匹配会被认为不可靠。
+    scan_buffer_maximum_scan_distance: 10.0
     link_match_minimum_response_fine: 0.1
-    # 建立相邻 scan 链路时允许的最大空间距离，超过则不尝试连接。
     link_scan_maximum_distance: 1.5
 
     # Loop closure
-    # 是否启用闭环检测。开启后回到旧区域时可修正累计漂移。
-    do_loop_closing: false
-    # 闭环候选链最小长度；较大可减少误闭环，较小更容易检测短回环。
+    do_loop_closing: true
     loop_match_minimum_chain_size: 10
-    # 粗匹配允许的最大方差；越小越严格，误闭环少但可能漏闭环。
     loop_match_maximum_variance_coarse: 3.0
-    # 闭环粗匹配最低响应阈值；候选低于该值会被丢弃。
     loop_match_minimum_response_coarse: 0.35
-    # 闭环精匹配最低响应阈值；越高越保守，闭环质量更可靠。
     loop_match_minimum_response_fine: 0.45
-    # 搜索闭环候选的最大距离，单位米；越大越容易找回环但计算量和误匹配风险增加。
     loop_search_maximum_distance: 3.0
 
     # Scan matching
-    # 局部 scan matching 搜索窗口尺寸，单位米；越大越能容忍初值误差但更耗时。
     correlation_search_space_dimension: 0.5
-    # 局部搜索分辨率，单位米；越小搜索更精细但计算更多。
     correlation_search_space_resolution: 0.01
-    # 相关性搜索的 smear 标准差，用于让匹配响应更平滑，提升抗噪性。
     correlation_search_space_smear_deviation: 0.1
-    # 闭环搜索窗口尺寸，单位米；通常比局部匹配更大以覆盖漂移。
     loop_search_space_dimension: 8.0
-    # 闭环搜索分辨率，单位米；控制闭环候选搜索精度和计算量。
     loop_search_space_resolution: 0.05
-    # 闭环搜索 smear 标准差，用于平滑闭环匹配响应。
     loop_search_space_smear_deviation: 0.03
 
     # Matcher params
-    # 位移方差惩罚。越大越不愿接受与预测位移差异大的匹配结果。
     distance_variance_penalty: 0.5
-    # 角度方差惩罚。越大越不愿接受与预测角度差异大的匹配结果。
     angle_variance_penalty: 1.0
-    # 精搜索角度步进，单位弧度；越小角度搜索更细但更慢。
     fine_search_angle_offset: 0.00349
-    # 粗搜索角度范围/步进相关参数，单位弧度；用于大范围初始角度搜索。
     coarse_search_angle_offset: 0.349
-    # 粗搜索角分辨率，单位弧度；越小越精细但计算量更高。
     coarse_angle_resolution: 0.0349
-    # 最小角度惩罚因子，限制角度惩罚不要过低，避免过度相信异常角度匹配。
     minimum_angle_penalty: 0.9
-    # 最小距离惩罚因子，限制距离惩罚不要过低，避免过度相信异常位移匹配。
     minimum_distance_penalty: 0.5
-    # 是否扩展匹配响应区域，有助于在响应较弱时找到可接受匹配。
     use_response_expansion: true
-    # 激光穿过栅格的最小次数阈值，用于空闲/占用证据更新。
     min_pass_through: 2
-    # 占用概率阈值。超过该阈值的栅格更倾向标为障碍。
     occupancy_threshold: 0.1
-    # 输入 scan 队列大小；处理跟不上时队列过小会丢帧，过大可能增加延迟。
     scan_queue_size: 20
 
 # ---------------------------------------------------------------------------
-# planner_server - SmacPlannerHybrid 全局路径规划
-# 主要作用:
-#   1. 接收起点/终点和全局代价地图。
-#   2. 使用 Hybrid-A* / Reeds-Shepp 运动模型生成符合车体转弯约束的 /plan。
+# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
+# 来源: gc_navigation_slam.yaml planner_server section
 # ---------------------------------------------------------------------------
 planner_server:
   ros__parameters:
-    # 规划插件列表。每个名字对应下方同名配置块；这里仅启用 GridBased。
     planner_plugins:
     - GridBased
-    # 是否使用仿真时间。应与其它 Nav2 节点保持一致。
     use_sim_time: False
     GridBased:
-      # 插件类型。SmacPlannerHybrid 适合非完整约束车辆，能考虑转弯半径和倒车。
       plugin: nav2_smac_planner/SmacPlannerHybrid
-      # 是否对 costmap 降采样后规划。False 表示用原始分辨率，路径更细但计算更多。
       downsample_costmap: False
-      # 降采样倍率；只有 downsample_costmap=True 时生效。
       downsampling_factor: 1
-      # 允许规划终点距离目标的容差，单位米；目标附近不可达时可在容差内结束。
       tolerance: 0.25
-      # 是否允许路径经过未知区域。True 可穿过未探索区域，False 更保守。
       allow_unknown: True
-      # 最大搜索迭代次数；越大越不容易提前失败，但规划耗时上限更高。
       max_iterations: 100000
-      # 接近目标阶段的最大迭代次数，用于目标附近精细搜索。
       max_on_approach_iterations: 1000
-      # 单次规划最大耗时，单位秒；超时后规划失败或返回当前可行结果。
       max_planning_time: 5.0
-      # 搜索运动模型。REEDS_SHEPP 支持前进和倒车，适合可倒车小车。
       motion_model_for_search: REEDS_SHEPP
-      # 朝向离散桶数量。72 表示 5 度一个方向；越大方向更细但搜索量更大。
       angle_quantization_bins: 72
-      # 解析扩展触发比例，用于尝试直接连接目标，提高接近目标时的效率。
       analytic_expansion_ratio: 2.0
-      # 解析扩展最大长度，单位米；太长可能穿障碍，太短接近目标效率降低。
       analytic_expansion_max_length: 3.0
-      # 最小转弯半径，单位米。应接近实车运动学能力；过小会生成车转不过的路径。
       minimum_turning_radius: 0.35
-      # 倒车惩罚。大于 1 会减少倒车段；值越大越偏好前进。
       reverse_penalty: 1.3
-      # 前进/倒车切换惩罚。增大后路径会减少换向次数。
       change_penalty: 0.0
-      # 非直线运动惩罚。增大后更偏好直线路径，可能牺牲可达性。
       non_straight_penalty: 0.0
-      # 代价地图代价惩罚。越大越远离障碍/高代价区域，但可能绕路更多。
       cost_penalty: 5.0
-      # 回溯惩罚。用于影响搜索顺序，通常小正数可提升搜索效率。
       retrospective_penalty: 0.015
-      # 运动启发查表范围，单位米；越大预计算更多，可能提升规划质量但占内存。
       lookup_table_size: 5.0
-      # 是否缓存障碍启发。静态环境可开以加速重复规划；动态环境关闭更安全。
       cache_obstacle_heuristic: False
-      # 是否发布搜索扩展可视化。调试规划失败时可开，平时关闭减少开销。
       viz_expansions: False
-      # 是否对生成路径做平滑。True 通常让路径更适合控制器跟踪。
       smooth_path: True
       smoother:
-        # 平滑器最大迭代次数；越大越可能收敛但耗时更长。
         max_iterations: 1000
-        # 平滑权重。越大路径越平滑，但可能偏离原始安全路径。
         w_smooth: 0.4
-        # 数据保持权重。越大越贴近原始规划路径，越不容易被平滑拉偏。
         w_data: 0.2
-        # 平滑收敛阈值；变化小于该值时停止迭代。
         tolerance: 1.0e-10
-        # 是否进行多轮细化平滑，改善最终路径质量。
         do_refinement: True
-        # 细化轮数；轮数越多路径越平滑但耗时增加。
         refinement_num: 4
 
 planner_server_rclcpp_node:
   ros__parameters:
-    # planner_server 辅助 rclcpp 节点的时间源设置，应与 planner_server 保持一致。
     use_sim_time: False
 
 # ---------------------------------------------------------------------------
-# global_costmap - 全局代价地图
-# 主要作用:
-#   1. 在 map 坐标系下融合静态地图和激光障碍。
-#   2. 给 planner_server 提供从起点到目标点的全局规划代价。
+# global_costmap — 全局代价地图（订阅 /scan + /map）
+# 来源: gc_navigation_slam.yaml global_costmap section
 # ---------------------------------------------------------------------------
 global_costmap:
   global_costmap:
     ros__parameters:
-      # 是否使用仿真时间。应与 SLAM、planner_server 和其它 Nav2 节点一致。
       use_sim_time: False
-      # TF 允许的时间容差，单位秒；传感器/TF 稍有延迟时可避免频繁报错。
       transform_tolerance: 2.0
-      # 代价地图更新频率，单位 Hz；越高越实时但 CPU 占用更高。
       update_frequency: 1.0
-      # 代价地图发布频率，单位 Hz；影响 RViz/Foxglove 可视化刷新和下游接收频率。
       publish_frequency: 1.0
-      # 全局代价地图所在坐标系。全局规划通常使用 map。
       global_frame: map
-      # 机器人基坐标系，用于从 TF 获取机器人在 costmap 中的位置。
       robot_base_frame: base_link
-      # 机器人二维足迹，多边形点按 base_link 坐标给出，单位米；用于碰撞检测。
       footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
-      # 足迹外扩安全边距，单位米；越大规划离障碍越远但可通行空间变窄。
       footprint_padding: 0.02
-      # 代价地图分辨率，单位 m/cell；应与地图精度和计算能力匹配。
       resolution: 0.05
-      # 是否保留未知空间。True 时未知区域保持 unknown，配合 allow_unknown 决定能否规划穿过。
       track_unknown_space: True
-      # 启用的 costmap layer 顺序。静态层提供地图，障碍层叠加实时障碍，膨胀层扩展安全距离。
       plugins:
       - static_layer
       - obstacle_layer
       - inflation_layer
       obstacle_layer:
-        # 障碍层插件类型，负责把 LaserScan/PointCloud 转为障碍和清障信息。
         plugin: nav2_costmap_2d::ObstacleLayer
-        # 是否启用障碍层。False 时该层不参与代价地图。
         enabled: True
-        # 观测源名称列表；这里定义一个名为 scan 的传感器源。
         observation_sources: scan
         scan:
-          # 激光雷达话题名。障碍层从这里读取 LaserScan。
           topic: /scan
-          # 处理障碍的最大高度，单位米；LaserScan 通常按 2D 使用，此参数保留兼容层配置。
           max_obstacle_height: 2.0
-          # 是否用激光射线清除自由空间。True 会把射线经过区域标为空闲。
           clearing: True
-          # 是否用激光命中点标记障碍。True 会把终点附近标为障碍。
           marking: True
-          # 传感器数据类型。LaserScan 表示订阅 sensor_msgs/msg/LaserScan。
           data_type: LaserScan
-          # 清障射线最大距离，单位米；越大清除范围越远。
           raytrace_max_range: 3.0
-          # 清障射线最小距离，单位米；小于该距离不参与清障。
           raytrace_min_range: 0.0
-          # 标记障碍最大距离，单位米；超过该距离的障碍点不写入 costmap。
           obstacle_max_range: 2.5
-          # 标记障碍最小距离，单位米；小于该距离的点不写入 costmap。
           obstacle_min_range: 0.0
       static_layer:
-        # 静态地图层插件类型，订阅 /map 并把占用栅格转成全局代价。
         plugin: nav2_costmap_2d::StaticLayer
-        # 是否用 transient local QoS 订阅地图。True 可收到已经发布过的 latched /map。
         map_subscribe_transient_local: True
       inflation_layer:
-        # 膨胀层插件类型，把障碍周围一定范围扩展为逐渐降低的代价。
         plugin: nav2_costmap_2d::InflationLayer
-        # 膨胀代价衰减系数。越大衰减越快，障碍影响范围内高代价区域更窄。
         cost_scaling_factor: 3.0
-        # 膨胀半径，单位米。障碍周围该半径内会增加代价。
         inflation_radius: 0.2
-      # 是否每次发布完整 costmap。True 简化可视化/调试，但带宽占用更高。
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
-      # global_costmap lifecycle client 的时间源设置，应与 global_costmap 保持一致。
       use_sim_time: False
   global_costmap_rclcpp_node:
     ros__parameters:
-      # global_costmap 内部 rclcpp 节点的时间源设置，应与 global_costmap 保持一致。
       use_sim_time: False
 
 # ---------------------------------------------------------------------------
-# local_costmap - 局部代价地图
-# 主要作用:
-#   1. 在 odom 坐标系下围绕机器人滚动更新附近障碍。
-#   2. 给局部控制器/避障模块提供短距离实时环境代价。
+# local_costmap — 局部代价地图（obstacle_detector 可注入）
+# 来源: gc_navigation_slam.yaml local_costmap section
 # ---------------------------------------------------------------------------
 local_costmap:
   local_costmap:
     ros__parameters:
-      # 局部代价地图更新频率，单位 Hz；越高避障越实时但 CPU 占用更高。
       update_frequency: 1.0
-      # 局部代价地图发布频率，单位 Hz；影响可视化和下游节点接收频率。
       publish_frequency: 2.0
-      # TF 查询容差，单位秒；允许 odom/base_link TF 存在小幅延迟。
       transform_tolerance: 2.0
-      # 局部地图所在坐标系。局部 costmap 通常用 odom，避免 map 闭环跳变影响局部控制。
       global_frame: odom
-      # 机器人基坐标系，用于把机器人足迹放到局部代价地图中。
       robot_base_frame: base_link
-      # 是否使用仿真时间。应与其它 Nav2 节点一致。
       use_sim_time: False
-      # 是否启用滚动窗口。True 表示地图窗口跟随机器人移动。
       rolling_window: True
-      # 局部窗口宽度，单位米；越大能看到更远障碍但计算更多。
       width: 3
-      # 局部窗口高度，单位米；越大能看到更远障碍但计算更多。
       height: 3
-      # 局部代价地图分辨率，单位 m/cell；越小越精细但计算更多。
       resolution: 0.05
-      # 机器人二维足迹，单位米；用于局部碰撞检测。
       footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
-      # 足迹外扩安全边距，单位米；越大越保守。
       footprint_padding: 0.02
-      # 启用的局部 costmap layer。当前只启用 voxel_layer 和 inflation_layer。
       plugins:
       - voxel_layer
       - inflation_layer
       inflation_layer:
-        # 局部膨胀层插件类型，负责给附近障碍周围增加安全代价。
         plugin: nav2_costmap_2d::InflationLayer
-        # 膨胀代价衰减系数。越大衰减越快，机器人更贴近障碍。
         cost_scaling_factor: 3.0
-        # 膨胀半径，单位米；建议不小于机器人定位误差和控制误差。
         inflation_radius: 0.2
       voxel_layer:
-        # 体素层插件类型，可维护 3D 障碍体素并投影到 2D costmap。
         plugin: nav2_costmap_2d::VoxelLayer
-        # 是否启用体素层。False 时不会把 scan 写入该层。
         enabled: True
-        # 是否发布体素地图，便于 RViz/Foxglove 调试障碍高度栅格。
         publish_voxel_map: True
-        # 体素网格 z 方向起点，单位米；通常从地面或激光参考高度附近开始。
         origin_z: 0.0
-        # 单个体素 z 方向分辨率，单位米。
         z_resolution: 0.05
-        # z 方向体素层数；与 z_resolution 相乘决定可表达的高度范围。
         z_voxels: 16
-        # 最大障碍高度，单位米；高于该值的数据不作为障碍处理。
         max_obstacle_height: 2.0
-        # 标记障碍所需的最小体素命中阈值。0 表示非常敏感，容易标障碍。
         mark_threshold: 0
-        # 观测源名称列表；这里使用 scan。
         observation_sources: scan
         scan:
-          # 激光雷达话题名。体素层从这里读取 LaserScan。
           topic: /scan
-          # 该观测源接受的最大障碍高度，单位米。
           max_obstacle_height: 2.0
-          # 是否用激光射线清除自由空间。
           clearing: True
-          # 是否用激光命中点标记障碍。
           marking: True
-          # 传感器数据类型。LaserScan 表示 2D 激光。
           data_type: LaserScan
-          # 清障射线最大距离，单位米。
           raytrace_max_range: 3.0
-          # 清障射线最小距离，单位米。
           raytrace_min_range: 0.0
-          # 标记障碍最大距离，单位米。
           obstacle_max_range: 2.5
-          # 标记障碍最小距离，单位米。
           obstacle_min_range: 0.0
       static_layer:
-        # 注意: 当前 local_costmap.plugins 未包含 static_layer，因此这个配置块不会生效。
-        # 若以后把 static_layer 加入 plugins，该参数表示用 transient local QoS 订阅 /map。
         map_subscribe_transient_local: True
-      # 是否每次发布完整局部 costmap。True 便于可视化，但占用更多带宽。
       always_send_full_costmap: True
   local_costmap_client:
     ros__parameters:
-      # local_costmap lifecycle client 的时间源设置，应与 local_costmap 保持一致。
       use_sim_time: False
   local_costmap_rclcpp_node:
     ros__parameters:
-      # local_costmap 内部 rclcpp 节点的时间源设置，应与 local_costmap 保持一致。
       use_sim_time: False

src/planner/config/planner.yaml.bak-

@@ -1,232 +0,0 @@
-# ============================================================================
-# planner.yaml — Nav2 minimal global planning stack
-# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
-# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
-# ============================================================================
-
-# ---------------------------------------------------------------------------
-# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
-# 来源: slam_toolbox_mapping.yaml
-# ---------------------------------------------------------------------------
-slam_toolbox:
-  ros__parameters:
-    use_sim_time: False
-
-    # Solver
-    solver_plugin: solver_plugins::CeresSolver
-    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
-    ceres_preconditioner: SCHUR_JACOBI
-    ceres_trust_strategy: LEVENBERG_MARQUARDT
-    ceres_dogleg_type: TRADITIONAL_DOGLEG
-    ceres_loss_function: None
-
-    # ROS base
-    odom_frame: odom
-    map_frame: map
-    base_frame: base_link
-    scan_topic: /scan
-    mode: mapping
-    use_map_saver: true
-
-    # Debug & performance
-    debug_logging: false
-    throttle_scans: 1
-    transform_publish_period: 0.02
-    map_update_interval: 3.0
-    resolution: 0.05
-    min_laser_range: 0.15
-    max_laser_range: 20.0
-    minimum_time_interval: 0.5
-    transform_timeout: 0.2
-    tf_buffer_duration: 30.0
-    stack_size_to_use: 40000000
-    enable_interactive_mode: true
-
-    # Mapping
-    use_scan_matching: true
-    use_scan_barycenter: true
-    minimum_travel_distance: 0.5
-    minimum_travel_heading: 0.1
-    scan_buffer_size: 10
-    scan_buffer_maximum_scan_distance: 10.0
-    link_match_minimum_response_fine: 0.1
-    link_scan_maximum_distance: 1.5
-
-    # Loop closure
-    do_loop_closing: true
-    loop_match_minimum_chain_size: 10
-    loop_match_maximum_variance_coarse: 3.0
-    loop_match_minimum_response_coarse: 0.35
-    loop_match_minimum_response_fine: 0.45
-    loop_search_maximum_distance: 3.0
-
-    # Scan matching
-    correlation_search_space_dimension: 0.5
-    correlation_search_space_resolution: 0.01
-    correlation_search_space_smear_deviation: 0.1
-    loop_search_space_dimension: 8.0
-    loop_search_space_resolution: 0.05
-    loop_search_space_smear_deviation: 0.03
-
-    # Matcher params
-    distance_variance_penalty: 0.5
-    angle_variance_penalty: 1.0
-    fine_search_angle_offset: 0.00349
-    coarse_search_angle_offset: 0.349
-    coarse_angle_resolution: 0.0349
-    minimum_angle_penalty: 0.9
-    minimum_distance_penalty: 0.5
-    use_response_expansion: true
-    min_pass_through: 2
-    occupancy_threshold: 0.1
-    scan_queue_size: 20
-
-# ---------------------------------------------------------------------------
-# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
-# 来源: gc_navigation_slam.yaml planner_server section
-# ---------------------------------------------------------------------------
-planner_server:
-  ros__parameters:
-    planner_plugins:
-    - GridBased
-    use_sim_time: False
-    GridBased:
-      plugin: nav2_smac_planner/SmacPlannerHybrid
-      downsample_costmap: False
-      downsampling_factor: 1
-      tolerance: 0.25
-      allow_unknown: True
-      max_iterations: 100000
-      max_on_approach_iterations: 1000
-      max_planning_time: 5.0
-      motion_model_for_search: REEDS_SHEPP
-      angle_quantization_bins: 72
-      analytic_expansion_ratio: 2.0
-      analytic_expansion_max_length: 3.0
-      minimum_turning_radius: 0.35
-      reverse_penalty: 1.3
-      change_penalty: 0.0
-      non_straight_penalty: 0.0
-      cost_penalty: 5.0
-      retrospective_penalty: 0.015
-      lookup_table_size: 5.0
-      cache_obstacle_heuristic: False
-      viz_expansions: False
-      smooth_path: True
-      smoother:
-        max_iterations: 1000
-        w_smooth: 0.4
-        w_data: 0.2
-        tolerance: 1.0e-10
-        do_refinement: True
-        refinement_num: 4
-
-planner_server_rclcpp_node:
-  ros__parameters:
-    use_sim_time: False
-
-# ---------------------------------------------------------------------------
-# global_costmap — 全局代价地图（订阅 /scan + /map）
-# 来源: gc_navigation_slam.yaml global_costmap section
-# ---------------------------------------------------------------------------
-global_costmap:
-  global_costmap:
-    ros__parameters:
-      use_sim_time: False
-      transform_tolerance: 2.0
-      update_frequency: 1.0
-      publish_frequency: 1.0
-      global_frame: map
-      robot_base_frame: base_link
-      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
-      footprint_padding: 0.02
-      resolution: 0.05
-      track_unknown_space: True
-      plugins:
-      - static_layer
-      - obstacle_layer
-      - inflation_layer
-      obstacle_layer:
-        plugin: nav2_costmap_2d::ObstacleLayer
-        enabled: True
-        observation_sources: scan
-        scan:
-          topic: /scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: LaserScan
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        plugin: nav2_costmap_2d::StaticLayer
-        map_subscribe_transient_local: True
-      inflation_layer:
-        plugin: nav2_costmap_2d::InflationLayer
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.2
-      always_send_full_costmap: True
-  global_costmap_client:
-    ros__parameters:
-      use_sim_time: False
-  global_costmap_rclcpp_node:
-    ros__parameters:
-      use_sim_time: False
-
-# ---------------------------------------------------------------------------
-# local_costmap — 局部代价地图（obstacle_detector 可注入）
-# 来源: gc_navigation_slam.yaml local_costmap section
-# ---------------------------------------------------------------------------
-local_costmap:
-  local_costmap:
-    ros__parameters:
-      update_frequency: 1.0
-      publish_frequency: 2.0
-      transform_tolerance: 2.0
-      global_frame: odom
-      robot_base_frame: base_link
-      use_sim_time: False
-      rolling_window: True
-      width: 3
-      height: 3
-      resolution: 0.05
-      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
-      footprint_padding: 0.02
-      plugins:
-      - voxel_layer
-      - inflation_layer
-      inflation_layer:
-        plugin: nav2_costmap_2d::InflationLayer
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.2
-      voxel_layer:
-        plugin: nav2_costmap_2d::VoxelLayer
-        enabled: True
-        publish_voxel_map: True
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        mark_threshold: 0
-        observation_sources: scan
-        scan:
-          topic: /scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: LaserScan
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        map_subscribe_transient_local: True
-      always_send_full_costmap: True
-  local_costmap_client:
-    ros__parameters:
-      use_sim_time: False
-  local_costmap_rclcpp_node:
-    ros__parameters:
-      use_sim_time: False

src/planner/config/planner.yaml.bak-20260623-160129

@@ -1,232 +0,0 @@
-# ============================================================================
-# planner.yaml — Nav2 minimal global planning stack
-# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
-# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
-# ============================================================================
-
-# ---------------------------------------------------------------------------
-# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
-# 来源: slam_toolbox_mapping.yaml
-# ---------------------------------------------------------------------------
-slam_toolbox:
-  ros__parameters:
-    use_sim_time: False
-
-    # Solver
-    solver_plugin: solver_plugins::CeresSolver
-    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
-    ceres_preconditioner: SCHUR_JACOBI
-    ceres_trust_strategy: LEVENBERG_MARQUARDT
-    ceres_dogleg_type: TRADITIONAL_DOGLEG
-    ceres_loss_function: None
-
-    # ROS base
-    odom_frame: odom
-    map_frame: map
-    base_frame: base_link
-    scan_topic: /scan
-    mode: mapping
-    use_map_saver: true
-
-    # Debug & performance
-    debug_logging: false
-    throttle_scans: 1
-    transform_publish_period: 0.02
-    map_update_interval: 3.0
-    resolution: 0.05
-    min_laser_range: 0.15
-    max_laser_range: 20.0
-    minimum_time_interval: 0.5
-    transform_timeout: 0.2
-    tf_buffer_duration: 30.0
-    stack_size_to_use: 40000000
-    enable_interactive_mode: true
-
-    # Mapping
-    use_scan_matching: true
-    use_scan_barycenter: true
-    minimum_travel_distance: 0.5
-    minimum_travel_heading: 0.1
-    scan_buffer_size: 10
-    scan_buffer_maximum_scan_distance: 10.0
-    link_match_minimum_response_fine: 0.1
-    link_scan_maximum_distance: 1.5
-
-    # Loop closure
-    do_loop_closing: true
-    loop_match_minimum_chain_size: 10
-    loop_match_maximum_variance_coarse: 3.0
-    loop_match_minimum_response_coarse: 0.35
-    loop_match_minimum_response_fine: 0.45
-    loop_search_maximum_distance: 3.0
-
-    # Scan matching
-    correlation_search_space_dimension: 0.5
-    correlation_search_space_resolution: 0.01
-    correlation_search_space_smear_deviation: 0.1
-    loop_search_space_dimension: 8.0
-    loop_search_space_resolution: 0.05
-    loop_search_space_smear_deviation: 0.03
-
-    # Matcher params
-    distance_variance_penalty: 0.5
-    angle_variance_penalty: 1.0
-    fine_search_angle_offset: 0.00349
-    coarse_search_angle_offset: 0.349
-    coarse_angle_resolution: 0.0349
-    minimum_angle_penalty: 0.9
-    minimum_distance_penalty: 0.5
-    use_response_expansion: true
-    min_pass_through: 2
-    occupancy_threshold: 0.1
-    scan_queue_size: 20
-
-# ---------------------------------------------------------------------------
-# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
-# 来源: gc_navigation_slam.yaml planner_server section
-# ---------------------------------------------------------------------------
-planner_server:
-  ros__parameters:
-    planner_plugins:
-    - GridBased
-    use_sim_time: False
-    GridBased:
-      plugin: nav2_smac_planner/SmacPlannerHybrid
-      downsample_costmap: False
-      downsampling_factor: 1
-      tolerance: 0.25
-      allow_unknown: True
-      max_iterations: 100000
-      max_on_approach_iterations: 1000
-      max_planning_time: 5.0
-      motion_model_for_search: REEDS_SHEPP
-      angle_quantization_bins: 72
-      analytic_expansion_ratio: 2.0
-      analytic_expansion_max_length: 3.0
-      minimum_turning_radius: 0.35
-      reverse_penalty: 1.3
-      change_penalty: 0.0
-      non_straight_penalty: 0.0
-      cost_penalty: 5.0
-      retrospective_penalty: 0.015
-      lookup_table_size: 5.0
-      cache_obstacle_heuristic: False
-      viz_expansions: False
-      smooth_path: True
-      smoother:
-        max_iterations: 1000
-        w_smooth: 0.4
-        w_data: 0.2
-        tolerance: 1.0e-10
-        do_refinement: True
-        refinement_num: 4
-
-planner_server_rclcpp_node:
-  ros__parameters:
-    use_sim_time: False
-
-# ---------------------------------------------------------------------------
-# global_costmap — 全局代价地图（订阅 /scan + /map）
-# 来源: gc_navigation_slam.yaml global_costmap section
-# ---------------------------------------------------------------------------
-global_costmap:
-  global_costmap:
-    ros__parameters:
-      use_sim_time: False
-      transform_tolerance: 2.0
-      update_frequency: 1.0
-      publish_frequency: 1.0
-      global_frame: map
-      robot_base_frame: base_link
-      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
-      footprint_padding: 0.02
-      resolution: 0.05
-      track_unknown_space: True
-      plugins:
-      - static_layer
-      - obstacle_layer
-      - inflation_layer
-      obstacle_layer:
-        plugin: nav2_costmap_2d::ObstacleLayer
-        enabled: True
-        observation_sources: scan
-        scan:
-          topic: /scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: LaserScan
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        plugin: nav2_costmap_2d::StaticLayer
-        map_subscribe_transient_local: True
-      inflation_layer:
-        plugin: nav2_costmap_2d::InflationLayer
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.2
-      always_send_full_costmap: True
-  global_costmap_client:
-    ros__parameters:
-      use_sim_time: False
-  global_costmap_rclcpp_node:
-    ros__parameters:
-      use_sim_time: False
-
-# ---------------------------------------------------------------------------
-# local_costmap — 局部代价地图（obstacle_detector 可注入）
-# 来源: gc_navigation_slam.yaml local_costmap section
-# ---------------------------------------------------------------------------
-local_costmap:
-  local_costmap:
-    ros__parameters:
-      update_frequency: 1.0
-      publish_frequency: 2.0
-      transform_tolerance: 2.0
-      global_frame: odom
-      robot_base_frame: base_link
-      use_sim_time: False
-      rolling_window: True
-      width: 3
-      height: 3
-      resolution: 0.05
-      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
-      footprint_padding: 0.02
-      plugins:
-      - voxel_layer
-      - inflation_layer
-      inflation_layer:
-        plugin: nav2_costmap_2d::InflationLayer
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.2
-      voxel_layer:
-        plugin: nav2_costmap_2d::VoxelLayer
-        enabled: True
-        publish_voxel_map: True
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        mark_threshold: 0
-        observation_sources: scan
-        scan:
-          topic: /scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: LaserScan
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        map_subscribe_transient_local: True
-      always_send_full_costmap: True
-  local_costmap_client:
-    ros__parameters:
-      use_sim_time: False
-  local_costmap_rclcpp_node:
-    ros__parameters:
-      use_sim_time: False

src/planner/config/pure_pursuit_tracker.yaml

@@ -0,0 +1,31 @@
+topology_pure_pursuit:
+  ros__parameters:
+    use_sim_time: false
+
+    # Tracker-only mode: consume /plan generated by a planner node.
+    enable_topology_planning: false
+    accept_external_plan: true
+    publish_cmd_vel: true
+    stop_without_plan: true
+
+    map_frame: map
+    odom_topic: /odom
+    external_plan_topic: /plan
+    cmd_vel_topic: /cmd_vel
+    status_topic: /pure_pursuit_status
+
+    control_rate: 20.0
+    lookahead_distance: 0.55
+    goal_tolerance: 0.20
+    linear_speed: 0.30
+    min_linear_speed: 0.08
+    reverse_speed: 0.12
+    slowdown_distance: 0.80
+    curvature_slowdown_gain: 0.18
+    max_angular_speed: 1.5
+    min_turning_radius: 0.40
+
+    latency_compensation: true
+    max_latency_compensation: 0.25
+    max_odom_age: 0.80
+    tf_timeout: 0.05

src/planner/config/topology_graph.yaml

@@ -0,0 +1,38 @@
+# Fixed-field topology graph for the lightweight planner.
+#
+# Replace the sample coordinates with measured corridor-center coordinates from
+# your map. Send a target by node name:
+#   ros2 topic pub -1 /topology_goal std_msgs/msg/String "{data: bed_b}"
+#
+# Or send /goal_pose near a node; the nearest node is selected.
+
+default_start: start
+default_goal: bed_b
+
+nodes:
+  start:
+    x: 0.54
+    y: 0.20
+  corridor_a:
+    x: 1.50
+    y: 0.20
+  corridor_b:
+    x: 2.50
+    y: 0.80
+  bed_a:
+    x: 3.20
+    y: 1.60
+  bed_b:
+    x: 2.54
+    y: 2.50
+  return_point:
+    x: 0.54
+    y: 0.20
+
+edges:
+  - [start, corridor_a]
+  - [corridor_a, corridor_b]
+  - [corridor_b, bed_a]
+  - [corridor_b, bed_b]
+  - [bed_a, bed_b]
+  - [start, return_point]

src/planner/config/topology_pure_pursuit.yaml

@@ -0,0 +1,44 @@
+topology_pure_pursuit:
+  ros__parameters:
+    use_sim_time: false
+
+    # Graph file is normally overridden by the launch file after installation.
+    graph_file: ""
+
+    # Topic interface. Run this node instead of Nav2 controller_server to avoid
+    # multiple publishers fighting over /cmd_vel.
+    # The graph coordinates live in this frame. The node transforms odom poses
+    # into this frame through TF when odom.header.frame_id differs.
+    map_frame: map
+    odom_topic: /odom
+    goal_pose_topic: /goal_pose
+    goal_node_topic: /topology_goal
+    plan_topic: /plan
+    cmd_vel_topic: /cmd_vel
+    status_topic: /topology_status
+    publish_cmd_vel: true
+    stop_without_plan: true
+
+    # Planner. Keep the graph sparse and place nodes near lane or corridor
+    # centers so localization error still leaves a useful safety margin.
+    autoplan_default_goal: false
+    path_resolution: 0.10
+    nearest_node_max_distance: 2.0
+
+    # Pure Pursuit tracking. The defaults are conservative for the origincar
+    # 0.143 m wheelbase and current /cmd_vel -> Ackermann conversion.
+    control_rate: 20.0
+    lookahead_distance: 0.55
+    goal_tolerance: 0.25
+    linear_speed: 0.30
+    min_linear_speed: 0.08
+    slowdown_distance: 0.80
+    curvature_slowdown_gain: 0.18
+    max_angular_speed: 1.5
+    min_turning_radius: 0.35
+
+    # Delay handling. Stamped odom is projected forward by at most this horizon.
+    latency_compensation: true
+    max_latency_compensation: 0.25
+    max_odom_age: 0.80
+    tf_timeout: 0.05

src/planner/launch/grid_astar_theta.launch.py

@@ -0,0 +1,43 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("planner")
+    params_file = LaunchConfiguration(
+        "params_file",
+        default=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"),
+    )
+    use_sim_time = LaunchConfiguration("use_sim_time", default="false")
+
+    planner_node = Node(
+        package="planner",
+        executable="grid_astar_theta_node",
+        name="grid_astar_theta_planner",
+        output="screen",
+        parameters=[
+            params_file,
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "params_file",
+                default_value=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"),
+                description="Parameters for lightweight Ackermann Hybrid A* planner.",
+            ),
+            DeclareLaunchArgument(
+                "use_sim_time",
+                default_value="false",
+                description="Use simulation clock.",
+            ),
+            planner_node,
+        ]
+    )

src/planner/launch/grid_astar_theta_with_tracker.launch.py

@@ -0,0 +1,59 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("planner")
+    planner_params = LaunchConfiguration(
+        "planner_params",
+        default=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"),
+    )
+    tracker_params = LaunchConfiguration(
+        "tracker_params",
+        default=os.path.join(pkg_dir, "config", "pure_pursuit_tracker.yaml"),
+    )
+    use_sim_time = LaunchConfiguration("use_sim_time", default="false")
+
+    planner_node = Node(
+        package="planner",
+        executable="grid_astar_theta_node",
+        name="grid_astar_theta_planner",
+        output="screen",
+        parameters=[planner_params, {"use_sim_time": use_sim_time}],
+    )
+    tracker_node = Node(
+        package="planner",
+        executable="topology_pure_pursuit_node.py",
+        name="topology_pure_pursuit",
+        output="screen",
+        parameters=[tracker_params, {"use_sim_time": use_sim_time}],
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "planner_params",
+                default_value=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"),
+                description="Parameters for lightweight Ackermann Hybrid A* planner.",
+            ),
+            DeclareLaunchArgument(
+                "tracker_params",
+                default_value=os.path.join(
+                    pkg_dir, "config", "pure_pursuit_tracker.yaml"
+                ),
+                description="Parameters for yaw-aware tracker-only mode.",
+            ),
+            DeclareLaunchArgument(
+                "use_sim_time",
+                default_value="false",
+                description="Use simulation clock.",
+            ),
+            planner_node,
+            tracker_node,
+        ]
+    )

src/planner/launch/pure_pursuit_tracker.launch.py

@@ -0,0 +1,45 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("planner")
+    params_file = LaunchConfiguration(
+        "params_file",
+        default=os.path.join(pkg_dir, "config", "pure_pursuit_tracker.yaml"),
+    )
+    use_sim_time = LaunchConfiguration("use_sim_time", default="false")
+
+    tracker_node = Node(
+        package="planner",
+        executable="topology_pure_pursuit_node.py",
+        name="topology_pure_pursuit",
+        output="screen",
+        parameters=[
+            params_file,
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "params_file",
+                default_value=os.path.join(
+                    pkg_dir, "config", "pure_pursuit_tracker.yaml"
+                ),
+                description="Parameters for Pure Pursuit tracker-only mode.",
+            ),
+            DeclareLaunchArgument(
+                "use_sim_time",
+                default_value="false",
+                description="Use simulation clock.",
+            ),
+            tracker_node,
+        ]
+    )

src/planner/launch/topology_pure_pursuit.launch.py

@@ -0,0 +1,58 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("planner")
+
+    params_file = LaunchConfiguration(
+        "params_file",
+        default=os.path.join(pkg_dir, "config", "topology_pure_pursuit.yaml"),
+    )
+    graph_file = LaunchConfiguration(
+        "graph_file",
+        default=os.path.join(pkg_dir, "config", "topology_graph.yaml"),
+    )
+    use_sim_time = LaunchConfiguration("use_sim_time", default="false")
+
+    topology_node = Node(
+        package="planner",
+        executable="topology_pure_pursuit_node.py",
+        name="topology_pure_pursuit",
+        output="screen",
+        parameters=[
+            params_file,
+            {
+                "graph_file": graph_file,
+                "use_sim_time": use_sim_time,
+            },
+        ],
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "params_file",
+                default_value=os.path.join(
+                    pkg_dir, "config", "topology_pure_pursuit.yaml"
+                ),
+                description="Parameters for topology Pure Pursuit.",
+            ),
+            DeclareLaunchArgument(
+                "graph_file",
+                default_value=os.path.join(pkg_dir, "config", "topology_graph.yaml"),
+                description="Topology graph YAML.",
+            ),
+            DeclareLaunchArgument(
+                "use_sim_time",
+                default_value="false",
+                description="Use simulation clock.",
+            ),
+            topology_node,
+        ]
+    )

src/planner/package.xml

@@ -10,12 +10,20 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
   <depend>slam_toolbox</depend>
   <depend>nav2_planner</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_lifecycle_manager</depend>
   <depend>nav2_common</depend>
   <depend>nav2_util</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/planner/scripts/grid_astar_theta_node.py

@@ -0,0 +1,475 @@
+#!/usr/bin/env python3
+"""Lightweight grid A*/Theta* planner for random circular obstacles."""
+
+import heapq
+import math
+
+import rclpy
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Odometry, Path as NavPath
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.time import Time
+from std_msgs.msg import Float32MultiArray, String
+from tf2_ros import Buffer, TransformException, TransformListener
+
+
+def yaw_from_quaternion(q):
+    return math.atan2(
+        2.0 * (q.w * q.z + q.x * q.y),
+        1.0 - 2.0 * (q.y * q.y + q.z * q.z),
+    )
+
+
+def normalize_angle(angle):
+    return math.atan2(math.sin(angle), math.cos(angle))
+
+
+def make_pose(frame_id, stamp, x, y, yaw):
+    pose = PoseStamped()
+    pose.header.frame_id = frame_id
+    pose.header.stamp = stamp
+    pose.pose.position.x = float(x)
+    pose.pose.position.y = float(y)
+    pose.pose.position.z = 0.0
+    pose.pose.orientation.z = math.sin(0.5 * yaw)
+    pose.pose.orientation.w = math.cos(0.5 * yaw)
+    return pose
+
+
+class GridAstarThetaPlanner(Node):
+    def __init__(self):
+        super().__init__("grid_astar_theta_planner")
+
+        self.declare_parameter("map_frame", "map")
+        self.declare_parameter("odom_topic", "/odom")
+        self.declare_parameter("goal_pose_topic", "/goal_pose")
+        self.declare_parameter("obstacles_topic", "/random_obstacles")
+        self.declare_parameter("plan_topic", "/plan")
+        self.declare_parameter("status_topic", "/grid_planner_status")
+        self.declare_parameter("map_min_x", -5.0)
+        self.declare_parameter("map_min_y", -5.0)
+        self.declare_parameter("map_max_x", 5.0)
+        self.declare_parameter("map_max_y", 5.0)
+        self.declare_parameter("resolution", 0.10)
+        self.declare_parameter("obstacle_format", "xyr")
+        self.declare_parameter("default_obstacle_radius", 0.15)
+        self.declare_parameter("robot_radius", 0.14)
+        self.declare_parameter("localization_error", 0.10)
+        self.declare_parameter("latency_margin", 0.05)
+        self.declare_parameter("safety_margin", 0.05)
+        self.declare_parameter("unknown_obstacle_radius", 0.15)
+        self.declare_parameter("use_theta_star", True)
+        self.declare_parameter("allow_diagonal", True)
+        self.declare_parameter("plan_on_obstacle_update", True)
+        self.declare_parameter("max_iterations", 50000)
+        self.declare_parameter("path_resolution", 0.10)
+        self.declare_parameter("smooth_iterations", 2)
+        self.declare_parameter("max_odom_age", 0.80)
+        self.declare_parameter("tf_timeout", 0.05)
+
+        self.map_frame = self.get_parameter("map_frame").value
+        self.map_min_x = float(self.get_parameter("map_min_x").value)
+        self.map_min_y = float(self.get_parameter("map_min_y").value)
+        self.map_max_x = float(self.get_parameter("map_max_x").value)
+        self.map_max_y = float(self.get_parameter("map_max_y").value)
+        self.resolution = float(self.get_parameter("resolution").value)
+        self.obstacle_format = str(self.get_parameter("obstacle_format").value)
+        self.default_obstacle_radius = float(
+            self.get_parameter("default_obstacle_radius").value
+        )
+        self.robot_radius = float(self.get_parameter("robot_radius").value)
+        self.localization_error = float(
+            self.get_parameter("localization_error").value
+        )
+        self.latency_margin = float(self.get_parameter("latency_margin").value)
+        self.safety_margin = float(self.get_parameter("safety_margin").value)
+        self.unknown_obstacle_radius = float(
+            self.get_parameter("unknown_obstacle_radius").value
+        )
+        self.use_theta_star = bool(self.get_parameter("use_theta_star").value)
+        self.allow_diagonal = bool(self.get_parameter("allow_diagonal").value)
+        self.plan_on_obstacle_update = bool(
+            self.get_parameter("plan_on_obstacle_update").value
+        )
+        self.max_iterations = int(self.get_parameter("max_iterations").value)
+        self.path_resolution = float(self.get_parameter("path_resolution").value)
+        self.smooth_iterations = int(self.get_parameter("smooth_iterations").value)
+        self.max_odom_age = float(self.get_parameter("max_odom_age").value)
+        self.tf_timeout = float(self.get_parameter("tf_timeout").value)
+
+        if self.resolution <= 0.0:
+            raise RuntimeError("resolution must be positive")
+        if self.map_max_x <= self.map_min_x or self.map_max_y <= self.map_min_y:
+            raise RuntimeError("invalid map bounds")
+
+        self.width = int(math.ceil((self.map_max_x - self.map_min_x) / self.resolution))
+        self.height = int(math.ceil((self.map_max_y - self.map_min_y) / self.resolution))
+        if self.width <= 1 or self.height <= 1:
+            raise RuntimeError("map bounds are too small for the selected resolution")
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+        self.latest_odom = None
+        self.goal_pose = None
+        self.obstacles = []
+        self.last_tf_warn_ns = 0
+
+        self.plan_pub = self.create_publisher(
+            NavPath, self.get_parameter("plan_topic").value, 10
+        )
+        self.status_pub = self.create_publisher(
+            String, self.get_parameter("status_topic").value, 10
+        )
+
+        self.odom_sub = self.create_subscription(
+            Odometry,
+            self.get_parameter("odom_topic").value,
+            self._odom_callback,
+            10,
+        )
+        self.goal_sub = self.create_subscription(
+            PoseStamped,
+            self.get_parameter("goal_pose_topic").value,
+            self._goal_callback,
+            10,
+        )
+        self.obstacles_sub = self.create_subscription(
+            Float32MultiArray,
+            self.get_parameter("obstacles_topic").value,
+            self._obstacles_callback,
+            10,
+        )
+
+        self._publish_status(
+            f"ready size={self.width}x{self.height} resolution={self.resolution:.3f}"
+        )
+
+    def _odom_callback(self, msg):
+        self.latest_odom = msg
+
+    def _goal_callback(self, msg):
+        transformed = self._transform_pose_msg(msg)
+        if transformed is None:
+            self._publish_status("goal_rejected transform_failed")
+            return
+        self.goal_pose = transformed
+        self.plan()
+
+    def _obstacles_callback(self, msg):
+        self.obstacles = self._parse_obstacles(msg.data)
+        if self.plan_on_obstacle_update and self.goal_pose is not None:
+            self.plan()
+
+    def _parse_obstacles(self, values):
+        stride = 3 if self.obstacle_format == "xyr" else 2
+        obstacles = []
+        if stride <= 0:
+            return obstacles
+        count = len(values) // stride
+        for index in range(count):
+            base = index * stride
+            x = float(values[base])
+            y = float(values[base + 1])
+            radius = self.default_obstacle_radius
+            if stride == 3:
+                radius = max(0.0, float(values[base + 2]))
+            obstacles.append((x, y, radius))
+        return obstacles
+
+    def plan(self):
+        start_pose = self._current_pose()
+        if start_pose is None:
+            self._publish_status("plan_rejected no_recent_odom")
+            return False
+        if self.goal_pose is None:
+            self._publish_status("plan_rejected no_goal")
+            return False
+
+        start = self._world_to_grid(start_pose[0], start_pose[1])
+        goal = self._world_to_grid(self.goal_pose[0], self.goal_pose[1])
+        if start is None or goal is None:
+            self._publish_status(f"plan_rejected out_of_bounds start={start} goal={goal}")
+            return False
+
+        occupied = self._build_occupancy(start, goal)
+        if occupied[start[1]][start[0]] or occupied[goal[1]][goal[0]]:
+            self._publish_status("plan_rejected start_or_goal_occupied")
+            return False
+
+        path_cells = self._search(start, goal, occupied)
+        if not path_cells:
+            self._publish_status("plan_failed no_path")
+            return False
+
+        points = [self._grid_to_world(cell[0], cell[1]) for cell in path_cells]
+        points[0] = (start_pose[0], start_pose[1])
+        points[-1] = (self.goal_pose[0], self.goal_pose[1])
+        if self.smooth_iterations > 0:
+            points = self._smooth_by_line_of_sight(points, occupied)
+        points = self._densify(points)
+        self._publish_path(points)
+        self._publish_status(
+            f"plan_ok cells={len(path_cells)} points={len(points)} obstacles={len(self.obstacles)}"
+        )
+        return True
+
+    def _build_occupancy(self, start, goal):
+        occupied = [[False for _ in range(self.width)] for _ in range(self.height)]
+        base_padding = self.robot_radius + self.localization_error + self.latency_margin + self.safety_margin
+        for obs_x, obs_y, obs_radius in self.obstacles:
+            radius = max(obs_radius, self.unknown_obstacle_radius) + base_padding
+            min_cell = self._world_to_grid(obs_x - radius, obs_y - radius, clamp=True)
+            max_cell = self._world_to_grid(obs_x + radius, obs_y + radius, clamp=True)
+            radius_sq = radius * radius
+            for gy in range(min_cell[1], max_cell[1] + 1):
+                for gx in range(min_cell[0], max_cell[0] + 1):
+                    wx, wy = self._grid_to_world(gx, gy)
+                    if (wx - obs_x) * (wx - obs_x) + (wy - obs_y) * (wy - obs_y) <= radius_sq:
+                        occupied[gy][gx] = True
+        occupied[start[1]][start[0]] = False
+        occupied[goal[1]][goal[0]] = False
+        return occupied
+
+    def _search(self, start, goal, occupied):
+        neighbors = self._neighbors8 if self.allow_diagonal else self._neighbors4
+        open_heap = []
+        heapq.heappush(open_heap, (0.0, start))
+        came_from = {start: start}
+        cost_so_far = {start: 0.0}
+        iterations = 0
+
+        while open_heap and iterations < self.max_iterations:
+            iterations += 1
+            _, current = heapq.heappop(open_heap)
+            if current == goal:
+                return self._reconstruct_path(came_from, start, goal)
+
+            for neighbor, move_cost in neighbors(current, occupied):
+                if occupied[neighbor[1]][neighbor[0]]:
+                    continue
+
+                parent = came_from[current]
+                if (
+                    self.use_theta_star
+                    and parent != current
+                    and self._line_of_sight(parent, neighbor, occupied)
+                ):
+                    candidate_parent = parent
+                    new_cost = cost_so_far[parent] + self._grid_distance(parent, neighbor)
+                else:
+                    candidate_parent = current
+                    new_cost = cost_so_far[current] + move_cost
+
+                if new_cost < cost_so_far.get(neighbor, float("inf")):
+                    cost_so_far[neighbor] = new_cost
+                    came_from[neighbor] = candidate_parent
+                    priority = new_cost + self._grid_distance(neighbor, goal)
+                    heapq.heappush(open_heap, (priority, neighbor))
+
+        return []
+
+    def _neighbors4(self, cell, occupied):
+        del occupied
+        x, y = cell
+        for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1)):
+            nx, ny = x + dx, y + dy
+            if 0 <= nx < self.width and 0 <= ny < self.height:
+                yield (nx, ny), 1.0
+
+    def _neighbors8(self, cell, occupied):
+        x, y = cell
+        for dx in (-1, 0, 1):
+            for dy in (-1, 0, 1):
+                if dx == 0 and dy == 0:
+                    continue
+                nx, ny = x + dx, y + dy
+                if 0 <= nx < self.width and 0 <= ny < self.height:
+                    if dx != 0 and dy != 0:
+                        # Avoid cutting through the corner of two inflated cells.
+                        if occupied[y][nx] or occupied[ny][x]:
+                            continue
+                    yield (nx, ny), math.hypot(dx, dy)
+
+    def _reconstruct_path(self, came_from, start, goal):
+        current = goal
+        path = [current]
+        while current != start:
+            current = came_from[current]
+            path.append(current)
+        path.reverse()
+        return path
+
+    def _line_of_sight(self, start, goal, occupied):
+        x0, y0 = start
+        x1, y1 = goal
+        dx = abs(x1 - x0)
+        dy = abs(y1 - y0)
+        sx = 1 if x0 < x1 else -1
+        sy = 1 if y0 < y1 else -1
+        err = dx - dy
+        x, y = x0, y0
+
+        while True:
+            if x < 0 or x >= self.width or y < 0 or y >= self.height:
+                return False
+            if occupied[y][x]:
+                return False
+            if x == x1 and y == y1:
+                return True
+            e2 = 2 * err
+            if e2 > -dy:
+                err -= dy
+                x += sx
+            if e2 < dx:
+                err += dx
+                y += sy
+
+    def _smooth_by_line_of_sight(self, points, occupied):
+        if len(points) <= 2:
+            return points
+        cells = [self._world_to_grid(x, y, clamp=True) for x, y in points]
+        for _ in range(self.smooth_iterations):
+            smoothed = [points[0]]
+            anchor_index = 0
+            test_index = 2
+            while test_index < len(points):
+                if not self._line_of_sight(cells[anchor_index], cells[test_index], occupied):
+                    smoothed.append(points[test_index - 1])
+                    anchor_index = test_index - 1
+                test_index += 1
+            smoothed.append(points[-1])
+            points = smoothed
+            cells = [self._world_to_grid(x, y, clamp=True) for x, y in points]
+        return points
+
+    def _densify(self, points):
+        if len(points) <= 1:
+            return points
+        dense = [points[0]]
+        step = max(self.resolution, self.path_resolution)
+        for target in points[1:]:
+            sx, sy = dense[-1]
+            tx, ty = target
+            distance = math.hypot(tx - sx, ty - sy)
+            if distance < 1e-6:
+                continue
+            steps = max(1, int(math.ceil(distance / step)))
+            for idx in range(1, steps + 1):
+                ratio = idx / steps
+                dense.append((sx + (tx - sx) * ratio, sy + (ty - sy) * ratio))
+        return dense
+
+    def _publish_path(self, points):
+        msg = NavPath()
+        msg.header.frame_id = self.map_frame
+        msg.header.stamp = self.get_clock().now().to_msg()
+        for index, (x, y) in enumerate(points):
+            if index + 1 < len(points):
+                nx, ny = points[index + 1]
+                yaw = math.atan2(ny - y, nx - x)
+            elif index > 0:
+                px, py = points[index - 1]
+                yaw = math.atan2(y - py, x - px)
+            else:
+                yaw = 0.0
+            msg.poses.append(make_pose(self.map_frame, msg.header.stamp, x, y, yaw))
+        self.plan_pub.publish(msg)
+
+    def _current_pose(self):
+        if self.latest_odom is None:
+            return None
+        msg = self.latest_odom
+        if msg.header.stamp.sec != 0 or msg.header.stamp.nanosec != 0:
+            age = (self.get_clock().now() - Time.from_msg(msg.header.stamp)).nanoseconds * 1e-9
+            if age > self.max_odom_age:
+                return None
+
+        x = msg.pose.pose.position.x
+        y = msg.pose.pose.position.y
+        yaw = yaw_from_quaternion(msg.pose.pose.orientation)
+        source_frame = msg.header.frame_id or self.map_frame
+        if source_frame != self.map_frame:
+            return self._transform_pose_2d(x, y, yaw, source_frame)
+        return x, y, yaw
+
+    def _transform_pose_msg(self, msg):
+        source_frame = msg.header.frame_id or self.map_frame
+        x = msg.pose.position.x
+        y = msg.pose.position.y
+        yaw = yaw_from_quaternion(msg.pose.orientation)
+        if source_frame != self.map_frame:
+            return self._transform_pose_2d(x, y, yaw, source_frame)
+        return x, y, yaw
+
+    def _transform_pose_2d(self, x, y, yaw, source_frame):
+        try:
+            transform = self.tf_buffer.lookup_transform(
+                self.map_frame,
+                source_frame,
+                Time(),
+                timeout=Duration(seconds=self.tf_timeout),
+            )
+        except TransformException as exc:
+            now_ns = self.get_clock().now().nanoseconds
+            if now_ns - self.last_tf_warn_ns > 2_000_000_000:
+                self.last_tf_warn_ns = now_ns
+                self.get_logger().warn(
+                    f"TF unavailable {source_frame}->{self.map_frame}: {exc}"
+                )
+            return None
+
+        t = transform.transform.translation
+        q = transform.transform.rotation
+        transform_yaw = yaw_from_quaternion(q)
+        cos_yaw = math.cos(transform_yaw)
+        sin_yaw = math.sin(transform_yaw)
+        target_x = t.x + cos_yaw * x - sin_yaw * y
+        target_y = t.y + sin_yaw * x + cos_yaw * y
+        target_yaw = normalize_angle(transform_yaw + yaw)
+        return target_x, target_y, target_yaw
+
+    def _world_to_grid(self, x, y, clamp=False):
+        gx = int(math.floor((x - self.map_min_x) / self.resolution))
+        gy = int(math.floor((y - self.map_min_y) / self.resolution))
+        if clamp:
+            gx = min(self.width - 1, max(0, gx))
+            gy = min(self.height - 1, max(0, gy))
+            return gx, gy
+        if gx < 0 or gx >= self.width or gy < 0 or gy >= self.height:
+            return None
+        return gx, gy
+
+    def _grid_to_world(self, gx, gy):
+        return (
+            self.map_min_x + (gx + 0.5) * self.resolution,
+            self.map_min_y + (gy + 0.5) * self.resolution,
+        )
+
+    @staticmethod
+    def _grid_distance(a, b):
+        return math.hypot(a[0] - b[0], a[1] - b[1])
+
+    def _publish_status(self, text):
+        msg = String()
+        msg.data = text
+        self.status_pub.publish(msg)
+        self.get_logger().info(text)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = GridAstarThetaPlanner()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/planner/scripts/topology_pure_pursuit_node.py

@@ -0,0 +1,629 @@
+#!/usr/bin/env python3
+"""Lightweight topology graph planner with Pure Pursuit tracking."""
+
+import heapq
+import math
+from pathlib import Path
+
+import rclpy
+from geometry_msgs.msg import PoseStamped, Twist
+from nav_msgs.msg import Odometry, Path as NavPath
+from rclpy.node import Node
+from rclpy.time import Time
+from std_msgs.msg import String
+import yaml
+
+from rclpy.duration import Duration
+from tf2_ros import Buffer, TransformException, TransformListener
+
+
+def yaw_from_quaternion(q):
+    return math.atan2(
+        2.0 * (q.w * q.z + q.x * q.y),
+        1.0 - 2.0 * (q.y * q.y + q.z * q.z),
+    )
+
+
+def normalize_angle(angle):
+    return math.atan2(math.sin(angle), math.cos(angle))
+
+
+def make_pose(frame_id, stamp, x, y, yaw):
+    pose = PoseStamped()
+    pose.header.frame_id = frame_id
+    pose.header.stamp = stamp
+    pose.pose.position.x = float(x)
+    pose.pose.position.y = float(y)
+    pose.pose.position.z = 0.0
+    pose.pose.orientation.z = math.sin(yaw * 0.5)
+    pose.pose.orientation.w = math.cos(yaw * 0.5)
+    return pose
+
+
+class TopologyPurePursuit(Node):
+    def __init__(self):
+        super().__init__("topology_pure_pursuit")
+
+        self.declare_parameter("graph_file", "")
+        self.declare_parameter("map_frame", "map")
+        self.declare_parameter("odom_topic", "/odom")
+        self.declare_parameter("goal_pose_topic", "/goal_pose")
+        self.declare_parameter("goal_node_topic", "/topology_goal")
+        self.declare_parameter("external_plan_topic", "/plan")
+        self.declare_parameter("plan_topic", "/plan")
+        self.declare_parameter("cmd_vel_topic", "/cmd_vel")
+        self.declare_parameter("status_topic", "/topology_status")
+        self.declare_parameter("enable_topology_planning", True)
+        self.declare_parameter("accept_external_plan", False)
+        self.declare_parameter("publish_cmd_vel", True)
+        self.declare_parameter("autoplan_default_goal", False)
+        self.declare_parameter("control_rate", 20.0)
+        self.declare_parameter("path_resolution", 0.10)
+        self.declare_parameter("nearest_node_max_distance", 2.0)
+        self.declare_parameter("lookahead_distance", 0.55)
+        self.declare_parameter("goal_tolerance", 0.25)
+        self.declare_parameter("linear_speed", 0.30)
+        self.declare_parameter("min_linear_speed", 0.08)
+        self.declare_parameter("reverse_speed", 0.12)
+        self.declare_parameter("slowdown_distance", 0.80)
+        self.declare_parameter("curvature_slowdown_gain", 0.18)
+        self.declare_parameter("max_angular_speed", 1.5)
+        self.declare_parameter("min_turning_radius", 0.35)
+        self.declare_parameter("latency_compensation", True)
+        self.declare_parameter("max_latency_compensation", 0.25)
+        self.declare_parameter("max_odom_age", 0.80)
+        self.declare_parameter("tf_timeout", 0.05)
+        self.declare_parameter("stop_without_plan", True)
+
+        self.map_frame = self.get_parameter("map_frame").value
+        self.enable_topology_planning = bool(
+            self.get_parameter("enable_topology_planning").value
+        )
+        self.accept_external_plan = bool(
+            self.get_parameter("accept_external_plan").value
+        )
+        self.publish_cmd_vel = bool(self.get_parameter("publish_cmd_vel").value)
+        self.autoplan_default_goal = bool(
+            self.get_parameter("autoplan_default_goal").value
+        )
+        self.path_resolution = float(self.get_parameter("path_resolution").value)
+        self.nearest_node_max_distance = float(
+            self.get_parameter("nearest_node_max_distance").value
+        )
+        self.lookahead_distance = float(
+            self.get_parameter("lookahead_distance").value
+        )
+        self.goal_tolerance = float(self.get_parameter("goal_tolerance").value)
+        self.linear_speed = float(self.get_parameter("linear_speed").value)
+        self.min_linear_speed = float(self.get_parameter("min_linear_speed").value)
+        self.reverse_speed = float(self.get_parameter("reverse_speed").value)
+        self.slowdown_distance = float(self.get_parameter("slowdown_distance").value)
+        self.curvature_slowdown_gain = float(
+            self.get_parameter("curvature_slowdown_gain").value
+        )
+        self.max_angular_speed = float(
+            self.get_parameter("max_angular_speed").value
+        )
+        self.min_turning_radius = float(
+            self.get_parameter("min_turning_radius").value
+        )
+        self.latency_compensation = bool(
+            self.get_parameter("latency_compensation").value
+        )
+        self.max_latency_compensation = float(
+            self.get_parameter("max_latency_compensation").value
+        )
+        self.max_odom_age = float(self.get_parameter("max_odom_age").value)
+        self.tf_timeout = float(self.get_parameter("tf_timeout").value)
+        self.stop_without_plan = bool(self.get_parameter("stop_without_plan").value)
+
+        self.nodes = {}
+        self.edges = {}
+        self.default_start_node = ""
+        self.default_goal_node = ""
+        if self.enable_topology_planning:
+            self._load_graph(str(self.get_parameter("graph_file").value).strip())
+        elif not self.accept_external_plan:
+            raise RuntimeError(
+                "enable_topology_planning=false requires accept_external_plan=true"
+            )
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.latest_odom = None
+        self.active_path = []
+        self.active_node_path = []
+        self.active_goal_node = ""
+        self.current_path_index = 0
+        self.completed = True
+        self.tried_default_goal = False
+        self.last_tf_warn_ns = 0
+
+        self.plan_pub = self.create_publisher(
+            NavPath, self.get_parameter("plan_topic").value, 10
+        )
+        self.cmd_pub = self.create_publisher(
+            Twist, self.get_parameter("cmd_vel_topic").value, 10
+        )
+        self.status_pub = self.create_publisher(
+            String, self.get_parameter("status_topic").value, 10
+        )
+
+        self.odom_sub = self.create_subscription(
+            Odometry,
+            self.get_parameter("odom_topic").value,
+            self._odom_callback,
+            10,
+        )
+        self.goal_pose_sub = None
+        self.goal_node_sub = None
+        self.external_plan_sub = None
+        if self.enable_topology_planning:
+            self.goal_pose_sub = self.create_subscription(
+                PoseStamped,
+                self.get_parameter("goal_pose_topic").value,
+                self._goal_pose_callback,
+                10,
+            )
+            self.goal_node_sub = self.create_subscription(
+                String,
+                self.get_parameter("goal_node_topic").value,
+                self._goal_node_callback,
+                10,
+            )
+        if self.accept_external_plan:
+            self.external_plan_sub = self.create_subscription(
+                NavPath,
+                self.get_parameter("external_plan_topic").value,
+                self._external_plan_callback,
+                10,
+            )
+
+        period = 1.0 / max(1.0, float(self.get_parameter("control_rate").value))
+        self.timer = self.create_timer(period, self._control_loop)
+        self._publish_status(
+            f"ready graph_nodes={len(self.nodes)} graph_edges={sum(len(v) for v in self.edges.values())}"
+        )
+
+    def _load_graph(self, graph_file_value):
+        if not graph_file_value:
+            raise RuntimeError("graph_file parameter is empty")
+        graph_file = Path(graph_file_value)
+        if not graph_file.exists():
+            raise RuntimeError(f"graph file does not exist: {graph_file}")
+
+        with graph_file.open("r", encoding="utf-8") as stream:
+            data = yaml.safe_load(stream) or {}
+
+        self.default_start_node = str(data.get("default_start", ""))
+        self.default_goal_node = str(data.get("default_goal", ""))
+
+        raw_nodes = data.get("nodes", {})
+        for name, value in raw_nodes.items():
+            if isinstance(value, dict):
+                x = value["x"]
+                y = value["y"]
+            else:
+                x = value[0]
+                y = value[1]
+            self.nodes[str(name)] = (float(x), float(y))
+
+        self.edges = {name: [] for name in self.nodes}
+        for edge in data.get("edges", []):
+            if isinstance(edge, dict):
+                start = str(edge["from"])
+                goal = str(edge["to"])
+                cost = float(edge.get("cost", self._distance_nodes(start, goal)))
+                bidirectional = bool(edge.get("bidirectional", True))
+            else:
+                start = str(edge[0])
+                goal = str(edge[1])
+                cost = self._distance_nodes(start, goal)
+                bidirectional = True
+            self._add_edge(start, goal, cost)
+            if bidirectional:
+                self._add_edge(goal, start, cost)
+
+        if not self.nodes:
+            raise RuntimeError(f"graph contains no nodes: {graph_file}")
+
+    def _distance_nodes(self, start, goal):
+        if start not in self.nodes or goal not in self.nodes:
+            raise RuntimeError(f"edge references unknown node: {start}->{goal}")
+        ax, ay = self.nodes[start]
+        bx, by = self.nodes[goal]
+        return math.hypot(bx - ax, by - ay)
+
+    def _add_edge(self, start, goal, cost):
+        if start not in self.nodes or goal not in self.nodes:
+            raise RuntimeError(f"edge references unknown node: {start}->{goal}")
+        self.edges.setdefault(start, []).append((goal, float(cost)))
+
+    def _odom_callback(self, msg):
+        self.latest_odom = msg
+        if (
+            self.enable_topology_planning
+            and self.autoplan_default_goal
+            and not self.tried_default_goal
+            and self.default_goal_node
+        ):
+            self.tried_default_goal = True
+            self.plan_to_node(self.default_goal_node)
+
+    def _goal_pose_callback(self, msg):
+        if not self.enable_topology_planning:
+            return
+        goal_node, distance = self._nearest_node(
+            msg.pose.position.x, msg.pose.position.y
+        )
+        if not goal_node or distance > self.nearest_node_max_distance:
+            self._publish_status(
+                f"goal_pose_rejected nearest={goal_node} distance={distance:.2f}"
+            )
+            return
+        self.plan_to_node(goal_node)
+
+    def _goal_node_callback(self, msg):
+        if not self.enable_topology_planning:
+            return
+        self.plan_to_node(msg.data.strip())
+
+    def _external_plan_callback(self, msg):
+        if not self.accept_external_plan:
+            return
+        frame_id = msg.header.frame_id or self.map_frame
+        if frame_id != self.map_frame:
+            self._publish_status(f"external_plan_rejected frame={frame_id}")
+            return
+        if len(msg.poses) < 2:
+            self._publish_status("external_plan_rejected too_short")
+            if self.stop_without_plan:
+                self.active_path = []
+                self.completed = True
+            return
+
+        self.active_path = [
+            (
+                pose.pose.position.x,
+                pose.pose.position.y,
+                yaw_from_quaternion(pose.pose.orientation),
+            )
+            for pose in msg.poses
+        ]
+        self.active_node_path = []
+        self.active_goal_node = "external_plan"
+        self.current_path_index = 0
+        self.completed = False
+        self._publish_status(f"external_plan_ok points={len(self.active_path)}")
+
+    def plan_to_node(self, goal_node):
+        if goal_node not in self.nodes:
+            self._publish_status(f"goal_node_rejected unknown={goal_node}")
+            return False
+
+        pose = self._current_pose(compensate=False)
+        if pose is None:
+            self._publish_status("plan_rejected no_odom")
+            return False
+
+        if self.default_start_node and self.default_start_node in self.nodes:
+            start_hint = self.default_start_node
+        else:
+            start_hint = ""
+        start_node, start_distance = self._nearest_node(pose[0], pose[1])
+        if start_hint and start_distance > self.nearest_node_max_distance:
+            start_node = start_hint
+        elif not start_node or start_distance > self.nearest_node_max_distance:
+            self._publish_status(
+                f"plan_rejected no_near_start nearest={start_node} distance={start_distance:.2f}"
+            )
+            return False
+
+        node_path = self._dijkstra(start_node, goal_node)
+        if not node_path:
+            self._publish_status(f"plan_failed {start_node}->{goal_node}")
+            return False
+
+        dense_path = self._densify_path(node_path, pose)
+        if len(dense_path) < 2:
+            self._publish_status(f"plan_failed short_path {start_node}->{goal_node}")
+            return False
+
+        self.active_path = dense_path
+        self.active_node_path = node_path
+        self.active_goal_node = goal_node
+        self.current_path_index = 0
+        self.completed = False
+        self._publish_path()
+        self._publish_status(
+            f"plan_ok {'->'.join(node_path)} points={len(dense_path)}"
+        )
+        return True
+
+    def _nearest_node(self, x, y):
+        best_name = ""
+        best_dist = float("inf")
+        for name, (nx, ny) in self.nodes.items():
+            distance = math.hypot(nx - x, ny - y)
+            if distance < best_dist:
+                best_name = name
+                best_dist = distance
+        return best_name, best_dist
+
+    def _dijkstra(self, start, goal):
+        queue = [(0.0, start)]
+        previous = {}
+        cost_so_far = {start: 0.0}
+
+        while queue:
+            cost, current = heapq.heappop(queue)
+            if current == goal:
+                break
+            if cost > cost_so_far[current]:
+                continue
+            for neighbor, edge_cost in self.edges.get(current, []):
+                new_cost = cost + edge_cost
+                if new_cost < cost_so_far.get(neighbor, float("inf")):
+                    cost_so_far[neighbor] = new_cost
+                    previous[neighbor] = current
+                    heapq.heappush(queue, (new_cost, neighbor))
+
+        if goal not in cost_so_far:
+            return []
+
+        path = [goal]
+        while path[-1] != start:
+            path.append(previous[path[-1]])
+        path.reverse()
+        return path
+
+    def _densify_path(self, node_path, pose):
+        points = [(pose[0], pose[1], pose[2])]
+        for index, node_name in enumerate(node_path):
+            target = self.nodes[node_name]
+            if index == 0 and math.hypot(target[0] - pose[0], target[1] - pose[1]) < 0.05:
+                points[-1] = (target[0], target[1], points[-1][2])
+                continue
+            self._append_segment(points, target)
+        return self._with_segment_yaws(points)
+
+    def _append_segment(self, points, target):
+        sx, sy, _ = points[-1]
+        tx, ty = target
+        length = math.hypot(tx - sx, ty - sy)
+        if length < 1e-6:
+            return
+        steps = max(1, int(math.ceil(length / max(0.02, self.path_resolution))))
+        for step in range(1, steps + 1):
+            ratio = step / steps
+            yaw = math.atan2(ty - sy, tx - sx)
+            points.append((sx + (tx - sx) * ratio, sy + (ty - sy) * ratio, yaw))
+
+    def _with_segment_yaws(self, points):
+        if len(points) <= 1:
+            return points
+
+        updated = []
+        for index, (x, y, yaw) in enumerate(points):
+            if index + 1 < len(points):
+                nx, ny, _ = points[index + 1]
+                if math.hypot(nx - x, ny - y) > 1e-6:
+                    yaw = math.atan2(ny - y, nx - x)
+            elif updated:
+                yaw = updated[-1][2]
+            updated.append((x, y, yaw))
+        return updated
+
+    def _publish_path(self):
+        path_msg = NavPath()
+        path_msg.header.frame_id = self.map_frame
+        path_msg.header.stamp = self.get_clock().now().to_msg()
+
+        for x, y, yaw in self.active_path:
+            path_msg.poses.append(make_pose(self.map_frame, path_msg.header.stamp, x, y, yaw))
+
+        self.plan_pub.publish(path_msg)
+
+    def _current_pose(self, compensate=True):
+        if self.latest_odom is None:
+            return None
+
+        msg = self.latest_odom
+        source_frame = msg.header.frame_id or self.map_frame
+        x = msg.pose.pose.position.x
+        y = msg.pose.pose.position.y
+        yaw = yaw_from_quaternion(msg.pose.pose.orientation)
+        vx = msg.twist.twist.linear.x
+        wz = msg.twist.twist.angular.z
+
+        stamp = Time.from_msg(msg.header.stamp)
+        if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
+            age = 0.0
+        else:
+            age = (self.get_clock().now() - stamp).nanoseconds * 1e-9
+        if age < 0.0:
+            age = 0.0
+        if age > self.max_odom_age:
+            return None
+
+        if compensate and self.latency_compensation:
+            horizon = min(age, self.max_latency_compensation)
+            if abs(wz) < 1e-4:
+                x += vx * math.cos(yaw) * horizon
+                y += vx * math.sin(yaw) * horizon
+            else:
+                radius = vx / wz
+                new_yaw = yaw + wz * horizon
+                x += radius * (math.sin(new_yaw) - math.sin(yaw))
+                y -= radius * (math.cos(new_yaw) - math.cos(yaw))
+                yaw = normalize_angle(new_yaw)
+
+        if source_frame != self.map_frame:
+            transformed = self._transform_pose_2d(x, y, yaw, source_frame)
+            if transformed is None:
+                return None
+            x, y, yaw = transformed
+
+        return x, y, yaw, vx, wz
+
+    def _transform_pose_2d(self, x, y, yaw, source_frame):
+        try:
+            transform = self.tf_buffer.lookup_transform(
+                self.map_frame,
+                source_frame,
+                Time(),
+                timeout=Duration(seconds=self.tf_timeout),
+            )
+        except TransformException as exc:
+            now_ns = self.get_clock().now().nanoseconds
+            if now_ns - self.last_tf_warn_ns > 2_000_000_000:
+                self.last_tf_warn_ns = now_ns
+                self.get_logger().warn(
+                    f"TF unavailable {source_frame}->{self.map_frame}: {exc}"
+                )
+            return None
+
+        t = transform.transform.translation
+        q = transform.transform.rotation
+        transform_yaw = yaw_from_quaternion(q)
+        cos_yaw = math.cos(transform_yaw)
+        sin_yaw = math.sin(transform_yaw)
+        target_x = t.x + cos_yaw * x - sin_yaw * y
+        target_y = t.y + sin_yaw * x + cos_yaw * y
+        target_yaw = normalize_angle(transform_yaw + yaw)
+        return target_x, target_y, target_yaw
+
+    def _control_loop(self):
+        if not self.publish_cmd_vel:
+            return
+
+        pose = self._current_pose(compensate=True)
+        if pose is None:
+            self._stop_if_needed("stop no_recent_odom")
+            return
+
+        if self.completed or len(self.active_path) < 2:
+            if self.stop_without_plan:
+                self._publish_cmd(0.0, 0.0)
+            return
+
+        x, y, yaw, _, _ = pose
+        goal_x, goal_y, _ = self.active_path[-1]
+        goal_distance = math.hypot(goal_x - x, goal_y - y)
+        if goal_distance <= self.goal_tolerance:
+            self.completed = True
+            self._publish_cmd(0.0, 0.0)
+            self._publish_status(f"goal_reached {self.active_goal_node}")
+            return
+
+        nearest_index = self._nearest_path_index(x, y)
+        self.current_path_index = nearest_index
+        target_index = self._lookahead_index(x, y, nearest_index)
+        target_x, target_y, target_path_yaw = self.active_path[target_index]
+
+        dx = target_x - x
+        dy = target_y - y
+        path_heading = self._segment_heading(target_index, x, y)
+        reverse = math.cos(normalize_angle(target_path_yaw - path_heading)) < 0.0
+        control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
+        local_x = math.cos(control_yaw) * dx + math.sin(control_yaw) * dy
+        local_y = -math.sin(control_yaw) * dx + math.cos(control_yaw) * dy
+        lookahead = max(0.05, math.hypot(local_x, local_y))
+
+        curvature = 2.0 * local_y / (lookahead * lookahead)
+        if self.min_turning_radius > 1e-3:
+            max_curvature = 1.0 / self.min_turning_radius
+            curvature = max(-max_curvature, min(max_curvature, curvature))
+
+        speed = self._target_speed(abs(curvature), goal_distance, reverse)
+        angular = speed * curvature
+        angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
+        self._publish_cmd(speed, angular)
+
+    def _nearest_path_index(self, x, y):
+        start = max(0, self.current_path_index - 5)
+        best_index = start
+        best_dist = float("inf")
+        for index in range(start, len(self.active_path)):
+            px, py, _ = self.active_path[index]
+            dist = (px - x) * (px - x) + (py - y) * (py - y)
+            if dist < best_dist:
+                best_index = index
+                best_dist = dist
+        return best_index
+
+    def _lookahead_index(self, x, y, start_index):
+        target_index = len(self.active_path) - 1
+        for index in range(start_index, len(self.active_path)):
+            px, py, _ = self.active_path[index]
+            if math.hypot(px - x, py - y) >= self.lookahead_distance:
+                target_index = index
+                break
+        return target_index
+
+    def _segment_heading(self, index, current_x=None, current_y=None):
+        if len(self.active_path) < 2:
+            return 0.0
+        if current_x is not None and current_y is not None:
+            tx, ty, target_yaw = self.active_path[index]
+            if math.hypot(tx - current_x, ty - current_y) > 1e-6:
+                return math.atan2(ty - current_y, tx - current_x)
+            return target_yaw
+
+        if index <= 0:
+            start_index = 0
+            end_index = 1
+        else:
+            start_index = index - 1
+            end_index = index
+        sx, sy, fallback_yaw = self.active_path[start_index]
+        tx, ty, _ = self.active_path[end_index]
+        if math.hypot(tx - sx, ty - sy) <= 1e-6:
+            return fallback_yaw
+        return math.atan2(ty - sy, tx - sx)
+
+    def _target_speed(self, abs_curvature, goal_distance, reverse):
+        base_speed = self.reverse_speed if reverse else self.linear_speed
+        speed = base_speed
+        speed *= max(0.35, 1.0 - self.curvature_slowdown_gain * abs_curvature)
+        if goal_distance < self.slowdown_distance:
+            ratio = max(0.25, goal_distance / max(0.05, self.slowdown_distance))
+            speed *= ratio
+        speed = max(self.min_linear_speed, min(base_speed, speed))
+        return -speed if reverse else speed
+
+    def _publish_cmd(self, linear, angular):
+        cmd = Twist()
+        cmd.linear.x = float(linear)
+        cmd.angular.z = float(angular)
+        self.cmd_pub.publish(cmd)
+
+    def _stop_if_needed(self, reason):
+        self._publish_cmd(0.0, 0.0)
+        self._publish_status(reason)
+
+    def _publish_status(self, text):
+        msg = String()
+        msg.data = text
+        self.status_pub.publish(msg)
+        self.get_logger().info(text)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = TopologyPurePursuit()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node._publish_cmd(0.0, 0.0)
+            node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/planner/src/grid_astar_theta_node.cpp

@@ -0,0 +1,11 @@
+#include <iostream>
+
+int main()
+{
+  std::cout << "planner package: Nav2 minimal global planning stack" << std::endl;
+  std::cout << "  - slam_toolbox (online_async)" << std::endl;
+  std::cout << "  - planner_server (SmacPlannerHybrid)" << std::endl;
+  std::cout << "  - global_costmap + local_costmap" << std::endl;
+  std::cout << "  - lifecycle_manager" << std::endl;
+  return 0;
+}