diff --git a/src/obs.zip b/src/obs.zip
new file mode 100644
index 0000000..c03beee
Binary files /dev/null and b/src/obs.zip differ
diff --git a/src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md b/src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md
new file mode 100644
index 0000000..9e943a3
--- /dev/null
+++ b/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，它更轻量、接口更简单、可控性更强，适合竞赛场景中地图规模较小、障碍物数量有限、需要快速重规划的任务。
diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt
index 443d085..eec7524 100644
--- a/src/planner/CMakeLists.txt
+++ b/src/planner/CMakeLists.txt
@@ -1,20 +1,45 @@
-cmake_minimum_required(VERSION 3.5)
-project(planner)
-
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra)
-endif()
-
+cmake_minimum_required(VERSION 3.5)
+project(planner)
+
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra)
+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,11 +48,16 @@ install(
DESTINATION share/${PROJECT_NAME}
)
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
- set(ament_cmake_copyright_FOUND TRUE)
- set(ament_cmake_cpplint_FOUND TRUE)
- ament_lint_auto_find_test_dependencies()
-endif()
+install(FILES
+ README.md
+ DESTINATION share/${PROJECT_NAME}
+)
-ament_package()
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ set(ament_cmake_copyright_FOUND TRUE)
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/planner/README.md b/src/planner/README.md
new file mode 100644
index 0000000..699747a
--- /dev/null
+++ b/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.
diff --git a/src/planner/config/grid_astar_theta.yaml b/src/planner/config/grid_astar_theta.yaml
new file mode 100644
index 0000000..d8d32cc
--- /dev/null
+++ b/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
diff --git a/src/planner/config/planner.yaml b/src/planner/config/planner.yaml
index 3bd2677..3b2b83b 100644
--- a/src/planner/config/planner.yaml
+++ b/src/planner/config/planner.yaml
@@ -1,399 +1,232 @@
-# ============================================================================
-# planner.yaml - Nav2 minimal global planning stack
-# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
-# 用途: 启动建图、全局规划、全局代价地图和局部代价地图所需的 ROS2 参数。
-# 说明: 本文件只配置参数；实际由 launch 文件把这些参数加载到对应节点。
-# ============================================================================
-
-# ---------------------------------------------------------------------------
-# slam_toolbox - online_async 实时建图
-# 主要作用:
-# 1. 订阅激光 /scan 和里程计 TF。
-# 2. 生成 /map 占用栅格地图。
-# 3. 发布 map -> odom 变换，使 map 坐标系和机器人 odom/base_link 连接起来。
-# ---------------------------------------------------------------------------
-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 占用更高。
- transform_publish_period: 0.02
- # /map 地图更新周期，单位秒。数值越小地图刷新越频繁，但计算/网络开销更高。
- map_update_interval: 1.0
- # 地图分辨率，单位 m/cell。0.05 表示每个栅格 5cm；越小越精细但地图更大。
- 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 内部线程栈大小；地图/图优化规模大时需要足够栈空间。
- 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
- # 精匹配响应最低阈值；低于该值的局部链路匹配会被认为不可靠。
- link_match_minimum_response_fine: 0.1
- # 建立相邻 scan 链路时允许的最大空间距离，超过则不尝试连接。
- link_scan_maximum_distance: 1.5
-
- # Loop closure
- # 是否启用闭环检测。开启后回到旧区域时可修正累计漂移。
- do_loop_closing: false
- # 闭环候选链最小长度；较大可减少误闭环，较小更容易检测短回环。
- 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:
- 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:
- 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:
- 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
+# ============================================================================
+# 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
diff --git a/src/planner/config/planner.yaml.bak- b/src/planner/config/planner.yaml.bak-
deleted file mode 100644
index 31c2ffd..0000000
--- a/src/planner/config/planner.yaml.bak-
+++ /dev/null
@@ -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
diff --git a/src/planner/config/planner.yaml.bak-20260623-160129 b/src/planner/config/planner.yaml.bak-20260623-160129
deleted file mode 100644
index 31c2ffd..0000000
--- a/src/planner/config/planner.yaml.bak-20260623-160129
+++ /dev/null
@@ -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
diff --git a/src/planner/config/pure_pursuit_tracker.yaml b/src/planner/config/pure_pursuit_tracker.yaml
new file mode 100644
index 0000000..c719a56
--- /dev/null
+++ b/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
diff --git a/src/planner/config/topology_graph.yaml b/src/planner/config/topology_graph.yaml
new file mode 100644
index 0000000..c621baa
--- /dev/null
+++ b/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]
diff --git a/src/planner/config/topology_pure_pursuit.yaml b/src/planner/config/topology_pure_pursuit.yaml
new file mode 100644
index 0000000..95f17a5
--- /dev/null
+++ b/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
diff --git a/src/planner/launch/grid_astar_theta.launch.py b/src/planner/launch/grid_astar_theta.launch.py
new file mode 100644
index 0000000..8384c8d
--- /dev/null
+++ b/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,
+ ]
+ )
diff --git a/src/planner/launch/grid_astar_theta_with_tracker.launch.py b/src/planner/launch/grid_astar_theta_with_tracker.launch.py
new file mode 100644
index 0000000..c297ff7
--- /dev/null
+++ b/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,
+ ]
+ )
diff --git a/src/planner/launch/planner.launch.py b/src/planner/launch/planner.launch.py
index aa7669f..bfedd16 100644
--- a/src/planner/launch/planner.launch.py
+++ b/src/planner/launch/planner.launch.py
@@ -1,77 +1,77 @@
-# ============================================================================
-# planner.launch.py
-# Nav2 最小全局规划模块
-# 启动: slam_toolbox + planner_server + lifecycle_manager
-# costmap 由 planner_server 内部实例化，不需要单独启动
-# 不启动: controller / behavior / bt_navigator / velocity_smoother / smoother
-#
-# 参考: gc_nav2_with_slam_online_real.launch.py
-# ============================================================================
-
-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():
- """启动 Nav2 最小规划栈：slam + planner"""
- ld = LaunchDescription()
-
- # === 包路径 ===
- pkg_dir = get_package_share_directory('planner')
-
- # === 参数 ===
- use_sim_time = LaunchConfiguration('use_sim_time', default='false')
- params_file = LaunchConfiguration(
- 'params_file',
- default=os.path.join(pkg_dir, 'config', 'planner.yaml'))
-
- declare_params = DeclareLaunchArgument(
- 'params_file',
- default_value=os.path.join(pkg_dir, 'config', 'planner.yaml'),
- description='Path to the planner parameters file')
- ld.add_action(declare_params)
-
- # =====================================================================
- # 1. slam_toolbox — 异步建图 (online_async)，发布 /map + map→odom
- # =====================================================================
- slam_toolbox_node = Node(
- package='slam_toolbox',
- executable='async_slam_toolbox_node',
- name='slam_toolbox',
- output='screen',
- parameters=[params_file,
- {'use_sim_time': use_sim_time}],
- )
- ld.add_action(slam_toolbox_node)
-
- # =====================================================================
- # 2. planner_server — SmacPlannerHybrid，ComputePathToPose action
- # planner_server 内部实例化 global_costmap（从 planner.yaml 读取参数）
- # =====================================================================
- planner_server_node = Node(
- package='nav2_planner',
- executable='planner_server',
- name='planner_server',
- output='screen',
- parameters=[params_file],
- )
- ld.add_action(planner_server_node)
-
- # =====================================================================
- # 3. lifecycle_manager — 自动激活 planner_server (lifecycle 节点)
- # =====================================================================
- lifecycle_manager_node = Node(
- package='nav2_lifecycle_manager',
- executable='lifecycle_manager',
- name='lifecycle_manager_planner',
- output='screen',
- parameters=[{'autostart': True},
- {'node_names': ['planner_server']}],
- )
- ld.add_action(lifecycle_manager_node)
-
- return ld
+# ============================================================================
+# planner.launch.py
+# Nav2 最小全局规划模块
+# 启动: slam_toolbox + planner_server + lifecycle_manager
+# costmap 由 planner_server 内部实例化，不需要单独启动
+# 不启动: controller / behavior / bt_navigator / velocity_smoother / smoother
+#
+# 参考: gc_nav2_with_slam_online_real.launch.py
+# ============================================================================
+
+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():
+ """启动 Nav2 最小规划栈：slam + planner"""
+ ld = LaunchDescription()
+
+ # === 包路径 ===
+ pkg_dir = get_package_share_directory('planner')
+
+ # === 参数 ===
+ use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+ params_file = LaunchConfiguration(
+ 'params_file',
+ default=os.path.join(pkg_dir, 'config', 'planner.yaml'))
+
+ declare_params = DeclareLaunchArgument(
+ 'params_file',
+ default_value=os.path.join(pkg_dir, 'config', 'planner.yaml'),
+ description='Path to the planner parameters file')
+ ld.add_action(declare_params)
+
+ # =====================================================================
+ # 1. slam_toolbox — 异步建图 (online_async)，发布 /map + map→odom
+ # =====================================================================
+ slam_toolbox_node = Node(
+ package='slam_toolbox',
+ executable='async_slam_toolbox_node',
+ name='slam_toolbox',
+ output='screen',
+ parameters=[params_file,
+ {'use_sim_time': use_sim_time}],
+ )
+ ld.add_action(slam_toolbox_node)
+
+ # =====================================================================
+ # 2. planner_server — SmacPlannerHybrid，ComputePathToPose action
+ # planner_server 内部实例化 global_costmap（从 planner.yaml 读取参数）
+ # =====================================================================
+ planner_server_node = Node(
+ package='nav2_planner',
+ executable='planner_server',
+ name='planner_server',
+ output='screen',
+ parameters=[params_file],
+ )
+ ld.add_action(planner_server_node)
+
+ # =====================================================================
+ # 3. lifecycle_manager — 自动激活 planner_server (lifecycle 节点)
+ # =====================================================================
+ lifecycle_manager_node = Node(
+ package='nav2_lifecycle_manager',
+ executable='lifecycle_manager',
+ name='lifecycle_manager_planner',
+ output='screen',
+ parameters=[{'autostart': True},
+ {'node_names': ['planner_server']}],
+ )
+ ld.add_action(lifecycle_manager_node)
+
+ return ld
diff --git a/src/planner/launch/pure_pursuit_tracker.launch.py b/src/planner/launch/pure_pursuit_tracker.launch.py
new file mode 100644
index 0000000..c1478a1
--- /dev/null
+++ b/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,
+ ]
+ )
diff --git a/src/planner/launch/topology_pure_pursuit.launch.py b/src/planner/launch/topology_pure_pursuit.launch.py
new file mode 100644
index 0000000..5ff0aa0
--- /dev/null
+++ b/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,
+ ]
+ )
diff --git a/src/planner/package.xml b/src/planner/package.xml
index bf4169c..ce7f983 100644
--- a/src/planner/package.xml
+++ b/src/planner/package.xml
@@ -1,26 +1,34 @@
-
-
-
- planner
- 0.0.0
- Minimal Nav2 global planning stack: slam_toolbox + planner_server + costmaps + lifecycle_manager
- sunrise
- TODO: License declaration
-
+
+
+
+ planner
+ 0.0.0
+ Minimal Nav2 global planning stack: slam_toolbox + planner_server + costmaps + lifecycle_manager
+ sunrise
+ TODO: License declaration
+
ament_cmake
rclcpp
+ geometry_msgs
+ nav_msgs
+ std_msgs
slam_toolbox
nav2_planner
nav2_costmap_2d
nav2_lifecycle_manager
nav2_common
nav2_util
+ tf2
+ tf2_geometry_msgs
+ tf2_ros
+ rclpy
+ python3-yaml
ament_lint_auto
- ament_lint_common
-
-
- ament_cmake
-
-
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/src/planner/scripts/grid_astar_theta_node.py b/src/planner/scripts/grid_astar_theta_node.py
new file mode 100644
index 0000000..2cf0fe9
--- /dev/null
+++ b/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()
diff --git a/src/planner/scripts/topology_pure_pursuit_node.py b/src/planner/scripts/topology_pure_pursuit_node.py
new file mode 100644
index 0000000..e2c430b
--- /dev/null
+++ b/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()
diff --git a/src/planner/src/grid_astar_theta_node.cpp b/src/planner/src/grid_astar_theta_node.cpp
new file mode 100644
index 0000000..3b522ab
--- /dev/null
+++ b/src/planner/src/grid_astar_theta_node.cpp
@@ -0,0 +1,11 @@
+#include
+
+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;
+}
diff --git a/src/planner/src/planner_version.cpp b/src/planner/src/planner_version.cpp
index dc219bf..d9f04f4 100644
--- a/src/planner/src/planner_version.cpp
+++ b/src/planner/src/planner_version.cpp
@@ -1,11 +1,11 @@
-#include
-
-int main(int argc, char* argv[])
-{
- 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;
-}
+#include
+
+int main(int argc, char* argv[])
+{
+ 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;
+}