目前实现较快速从原点到达某个点，但是然后回到原点时有较大问题

src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md

@@ -43,7 +43,7 @@ topology_pure_pursuit
 /cmd_vel
         |
         v
-cmd_vel_to_ackermann_drive.py
+ackermann_cmd_bridge.py
         |
         v
 /ackermann_cmd
@@ -385,7 +385,7 @@ reverse_speed: 0.12
 /cmd_vel.linear.x < 0
 ```
 
-之后由现有 `cmd_vel_to_ackermann_drive.py` 桥接到 `/ackermann_cmd`。
+实车启动时由 `planner/scripts/ackermann_cmd_bridge.py` 桥接到 `/ackermann_cmd`。
 
 ## 12. 启动方式
 

src/planner/CMakeLists.txt

@@ -18,7 +18,6 @@ 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
@@ -40,16 +39,30 @@ install(TARGETS
 install(PROGRAMS
   scripts/topology_pure_pursuit_node.py
   scripts/grid_astar_theta_node.py
+  scripts/send_goal.py
+  scripts/send_start_goal.py
+  scripts/odom_map_tf.py
+  scripts/preflight_check.py
+  scripts/map_info.py
+  scripts/navigation_test_recorder.py
+  scripts/goal_reach_debug_collector.py
+  scripts/ackermann_cmd_bridge.py
+  scripts/lidar_health_check.py
+  scripts/evaluate_navigation_test.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
 install(
-  DIRECTORY launch config
+  DIRECTORY launch config maps
   DESTINATION share/${PROJECT_NAME}
 )
 
 install(FILES
   README.md
+  ACKERMANN_HYBRID_ASTAR_PLAN.md
+  REAL_ROBOT_RUNBOOK.md
+  REAL_ROBOT_TEST_PLAN_ZH.md
+  操作手册.md
   DESTINATION share/${PROJECT_NAME}
 )
 

src/planner/README.md

@@ -102,8 +102,8 @@ treats that segment as reverse and publishes negative `/cmd_vel.linear.x`.
   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
+- If the base expects Ackermann commands, run
-  `cmd_vel_to_ackermann_drive.py` bridge and configure the base driver
+  `scripts/ackermann_cmd_bridge.py` and configure the base driver
   accordingly.
 
 ## Planner Semantics

src/planner/REAL_ROBOT_RUNBOOK.md

@@ -0,0 +1,160 @@
+# Real Robot Custom Hybrid A* Runbook
+
+The dedicated launch file is `real_hybrid_astar.launch.py`. It starts only the
+nodes required by the custom stack:
+
+`origincar_base -> EKF -> AMCL/map_server -> LSLIDAR -> grid_astar_theta_node -> Pure Pursuit -> Ackermann bridge`
+
+Nav2 `controller_server` is intentionally not started. Pure Pursuit is the only
+node publishing `/cmd_vel`.
+
+The repository also contains `planner/maps/real_5x5.yaml`, a synthetic empty
+5x5 m room with boundary walls. Use it to validate the software chain only; it
+must be replaced by a map created from the real environment before driving.
+
+## Confirm before running
+
+1. The map YAML and image are the real map. The repository example `zhihui` map is
+   109x115 pixels at 0.05 m/pixel, or 5.45x5.75 m, not exactly 5x5 m.
+2. The LSLIDAR model, interface and serial device match the selected parameter
+   file. The example N10 configuration uses `/dev/ttyCH343USB0`.
+3. The STM32 serial device and baud rate are correct. The default is
+   `/dev/ttyACM0` and 115200.
+4. The robot is Ackermann configured and the measured minimum turning radius is
+   close to 0.40 m.
+5. The initial robot pose in the map is known and the start cell is free.
+6. The goal is position-only by default. Set `require_goal_yaw: true` in the
+   planner parameters when a final docking heading is required.
+
+## Build on the robot
+
+```bash
+cd ~/liao_ws
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install \
+  --packages-select \
+  origincar_msg origincar_description origincar_base \
+  lslidar_msgs lslidar_driver planner
+source install/setup.bash
+```
+
+Before launching ROS, validate the map on any machine with Python and PyYAML:
+
+```bash
+python3 src/planner/scripts/map_info.py /actual/map.yaml
+```
+
+The check must report `PASS` for a 5x5 m map. Use `--no-check` only when the
+measured map size is intentional and pass those dimensions to `preflight_check`.
+
+## Start the stack
+
+Use one terminal. Replace the map and lidar parameter paths when necessary.
+
+```bash
+ros2 launch planner real_hybrid_astar.launch.py \
+  map:=$HOME/liao_ws/src/planner/maps/real_5x5.yaml \
+  lidar_params:=$HOME/liao_ws/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml \
+  lidar_serial_port:=/dev/ttyCH343USB0 \
+  lidar_model:=N10 \
+  start_lidar:=true \
+  serial_port:=/dev/ttyACM0 \
+  serial_baud:=115200 \
+  enable_motion:=false
+```
+
+The command above uses the repository's synthetic 5x5 m room for a software
+chain test. For real driving, replace `map:=...` with the YAML generated from
+the actual environment and verify its dimensions and origin first.
+
+Set `start_lidar:=false` when a standalone LSLIDAR launch already publishes
+`/scan`; never let two driver processes open the same serial device.
+
+Do not also launch `origincar_bringup`, Nav2 `navigation_launch`, another
+controller, `path_follower_demo.py`, or `udp_to_cmdvel.py`; those can publish a
+competing `/cmd_vel`.
+
+## Verify interfaces
+
+```bash
+ros2 topic echo /scan --once
+ros2 topic echo /map --once
+ros2 topic echo /odom_combined --once
+ros2 run tf2_ros tf2_echo map base_footprint
+ros2 topic echo /hybrid_astar_status
+ros2 topic echo /pure_pursuit_status
+```
+
+AMCL must be initialized before the preflight gate. Publish the measured initial
+pose in the map:
+
+```bash
+ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
+  "{header: {frame_id: map}, pose: {pose: {position: {x: 0.54, y: 0.20}, orientation: {w: 1.0}}, covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.10]}}"
+```
+
+Run the preflight gate before sending any goal. It checks the 5x5m map size,
+`/scan`, `/odom_combined`, `map -> base_footprint`, and that exactly one node
+publishes `/cmd_vel`. It also checks the `/ackermann_cmd` bridge and base
+subscriber:
+
+```bash
+ros2 run planner preflight_check.py
+```
+
+For a deliberately different map size, override the expected dimensions rather
+than ignoring the check:
+
+```bash
+ros2 run planner preflight_check.py --ros-args \
+  -p expected_map_width:=5.45 -p expected_map_height:=5.75
+```
+
+## Send one goal
+
+Start the status and `/plan` monitors in separate terminals before publishing;
+these topics are not transient-local and an observer started later can miss the
+message.
+
+```bash
+ros2 run planner send_goal.py 1.00 1.00
+```
+
+The optional third argument is the final yaw in radians. In the real-robot
+configuration only x/y are required; the planner stops within the position
+tolerance without forcing yaw to zero.
+
+Equivalent raw command:
+
+```bash
+ros2 topic pub --once /goal_pose geometry_msgs/msg/PoseStamped \
+  "{header: {frame_id: map}, pose: {position: {x: 1.00, y: 1.00}, orientation: {w: 1.0}}}"
+```
+
+For the repository map, use a point strictly inside `[-2.5, 2.5)` and clear of
+the inflated boundary walls. The `(1.0, 1.0)` example is valid; `(2.5, 2.5)` is
+outside the usable grid.
+
+The first launch uses `enable_motion:=false`, so it only validates planning.
+After `/plan` and the steering direction have been checked, cancel the goal,
+stop the launch, and restart with `enable_motion:=true` for low-speed driving.
+
+Expected sequence:
+
+1. `/hybrid_astar_status` reports `plan_ok`.
+2. `/plan` contains the generated path.
+3. With `enable_motion:=false`, the robot remains stopped for path inspection.
+4. After restarting with `enable_motion:=true`, `/cmd_vel` is converted to
+   `/ackermann_cmd` and the robot stops within `goal_tolerance` (0.15 m).
+
+Cancel the active goal and stop with:
+
+```bash
+ros2 topic pub --once /navigation_cancel std_msgs/msg/Bool "{data: true}"
+```
+
+This clears the planner goal and tracker path. Publishing one zero `/cmd_vel`
+message is not a reliable stop because the tracker can overwrite it at 20 Hz.
+
+If AMCL localization is not available yet, do not drive autonomously. The
+planner requires a valid `map -> odom` transform and recent `/odom_combined`.

src/planner/REAL_ROBOT_TEST_PLAN_ZH.md

@@ -0,0 +1,238 @@
+# Hybrid A* 与 Pure Pursuit 实车测试方案
+
+## 1. 测试目标
+
+分别验证两个对象：
+
+1. Hybrid A* 是否能在规定时间内生成无碰撞、满足最小转弯半径的路径。
+2. Pure Pursuit 是否能稳定跟踪该路径，在到点、取消、定位失效时可靠停车。
+
+测试必须按阶段执行。前一阶段未通过，不进入下一阶段。
+
+## 2. 安全规则
+
+- 至少两人参与：一人操作电脑，一人负责硬件急停。
+- 第一次运动测试必须架起驱动轮。
+- 落地测试速度保持 `linear_speed <= 0.20 m/s`、`reverse_speed <= 0.10 m/s`。
+- 禁止同时启动 Nav2 controller、`origincar_bringup`、巡线、UDP 控制或其他 `/cmd_vel` 发布节点。
+- 合成 `real_5x5.yaml` 只允许做不运动的规划测试。除非真实场地就是对应的 5×5 米空房间，否则禁止用它进行自动驾驶。
+- ROS 取消不是硬件急停。任何异常首先使用硬件急停。
+
+## 3. 测试输出
+
+启动记录器：
+
+```bash
+ros2 run planner navigation_test_recorder.py --ros-args \
+  -p test_name:=P1_straight \
+  -p output_dir:=$HOME/yiliao_ws/test_results
+```
+
+测试结束后按 `Ctrl+C`，得到：
+
+```text
+test_results/P1_straight_时间.csv
+test_results/P1_straight_时间.json
+```
+
+JSON 中的关键指标：
+
+| 字段 | 含义 |
+|---|---|
+| `planning_latency_sec` | 从目标到收到路径的延迟 |
+| `path_occupied_points_max` | 路径落入原始占用/未知栅格的点数 |
+| `minimum_planned_turning_radius_m` | 路径估计最小转弯半径 |
+| `cross_track_error_m` | 实车到当前路径的横向误差 |
+| `final_goal_error_m` | 记录结束时的目标距离 |
+| `cancel_to_zero_command_sec` | 取消到收到零 `/cmd_vel` 的延迟 |
+| `transform_failures` | 记录期间 TF 查询失败次数 |
+
+每个落地用例至少重复 5 次，保留全部 CSV/JSON，不只保留最好结果。
+
+自动评估单个或多个结果：
+
+```bash
+ros2 run planner evaluate_navigation_test.py planning \
+  $HOME/yiliao_ws/test_results/P[1-4]*.json
+
+ros2 run planner evaluate_navigation_test.py tracking \
+  $HOME/yiliao_ws/test_results/T*.json
+```
+
+评估器按本手册的默认阈值返回 `PASS` 或 `FAIL`，退出码分别为 0 或 1。需要调整验收指标时使用脚本的命令行参数，不要修改原始测试结果。
+
+## 4. 阶段 0：硬件与定位门槛
+
+### 4.1 雷达
+
+当前 N10 参数文件默认使用 `/dev/ttyCH343USB0`。先确认：
+
+```bash
+grep -E "lidar_name|interface_selection|serial_port_" \
+  $HOME/yiliao_ws/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml
+
+ls -l /dev/ttyACM* /dev/ttyUSB* /dev/ttyCH343USB* 2>/dev/null
+id -nG
+```
+
+任何时刻只允许一个 `lslidar_driver_node`。如果雷达已单独启动，启动 planner 实车入口时使用 `start_lidar:=false`。
+
+启动后检查：
+
+```bash
+ros2 param get /lslidar_driver_node lidar_name
+ros2 param get /lslidar_driver_node interface_selection
+ros2 param get /lslidar_driver_node serial_port_
+ros2 topic hz /scan
+ros2 run planner lidar_health_check.py
+```
+
+通过标准：连续 60 秒有 `/scan`，频率不低于雷达标称频率的 80%，无持续设备断连。
+
+旧版雷达驱动可能反复打印笼统的 `error`。是否阻塞测试以 `/scan` 健康检查为准：健康检查通过时可继续只规划测试；没有 `/scan` 或健康检查失败时必须先处理设备路径、权限、占用或供电问题。
+
+### 4.2 底盘、地图与 TF
+
+```bash
+ros2 topic hz /odom
+ros2 topic hz /odom_combined
+ros2 run tf2_ros tf2_echo odom base_footprint
+ros2 run tf2_ros tf2_echo map base_footprint
+ros2 run planner preflight_check.py
+```
+
+通过标准：
+
+- `/odom_combined` 稳定接近 20 Hz；
+- `odom -> base_footprint` 连续；
+- 设置 AMCL 初始位姿后 `map -> base_footprint` 连续；
+- `preflight_check.py` 返回成功；
+- `/cmd_vel`、`/ackermann_cmd` 的发布/订阅数量均符合预检要求。
+
+## 5. 阶段 1：只规划测试
+
+启动时必须使用：
+
+```text
+enable_motion:=false
+```
+
+启动记录器和三个监视终端：
+
+```bash
+ros2 topic echo /hybrid_astar_status
+ros2 topic echo /pure_pursuit_status
+ros2 topic echo /plan
+```
+
+以测试地图中心附近 `(0, 0, 0)` 为起点，执行以下用例：
+
+| 用例 | 操作 | 预期 |
+|---|---|---|
+| P1 直线路径 | `send_goal.py 1.0 0.0` | 非空路径，无倒车段 |
+| P2 转弯路径 | `send_goal.py 1.0 1.0` | 曲率连续且无占用点 |
+| P3 后方目标 | `send_goal.py -0.8 0.0` | 允许出现倒车段，不频繁换挡 |
+| P4 锥桶重规划 | 路径发布后在路径中部发布锥桶 | 2 Hz 内重规划或安全清空路径 |
+| P5 目标占用 | 把目标放在边界墙上 | 拒绝规划并发布空路径 |
+| P6 取消 | 路径生成后发布取消 | 规划器和跟踪器都报告取消 |
+
+锥桶命令示例：
+
+```bash
+ros2 topic pub --once /random_obstacles \
+  std_msgs/msg/Float32MultiArray "{data: [0.5, 0.0, 0.15]}"
+
+ros2 topic pub --once /clear_random_obstacles \
+  std_msgs/msg/Bool "{data: true}"
+```
+
+规划器通过标准：
+
+- 自由起终点成功率 >= 95%（至少 20 次）；
+- 规划延迟 p95 <= 0.50 秒，最大值 <= 1.0 秒；
+- `path_occupied_points_max == 0`；
+- 规划最小转弯半径 >= 配置值的 90%（当前为 0.36 米）；
+- 目标占用或无路时必须清空危险旧路径；
+- 锥桶更新到新路径/空路径的时间 <= 0.75 秒；
+- `enable_motion:=false` 时底盘不得运动。
+
+## 6. 阶段 2：架空轮控制测试
+
+架起驱动轮并准备硬件急停，然后用 `enable_motion:=true` 重启。重新初始化 AMCL 并运行预检。
+
+依次测试短距离前进、左转、右转、倒车，观察：
+
+```bash
+ros2 topic echo /cmd_vel
+ros2 topic echo /ackermann_cmd
+```
+
+通过标准：
+
+- 正速度对应车轮前进，负速度对应车轮倒转；
+- 正转角和负转角分别对应预期左右方向；
+- `/ackermann_cmd.drive.steering_angle` 不超过 `0.60 rad`；
+- 无命令时不持续转动；
+- 停止 `/cmd_vel` 后，planner 内的桥接看门狗在 0.25 秒左右将 `/ackermann_cmd` 归零；
+- 发布 `/navigation_cancel` 后 0.25 秒内命令归零。
+
+任何符号相反都必须先修正，禁止通过交换测试目标来掩盖问题。
+
+## 7. 阶段 3：低速落地跟踪
+
+必须换成与真实环境匹配的地图。每次用例流程：
+
+1. 将机器人放在标记起点。
+2. 设置 AMCL 初始位姿并等待扫描与地图重合。
+3. 运行 `preflight_check.py`。
+4. 启动记录器。
+5. 发布目标，保持急停人员就位。
+6. 到达或失败后立即取消，停止记录器。
+7. 重复 5 次。
+
+推荐用例：
+
+| 用例 | 路径特征 | 距离 |
+|---|---|---|
+| T1 | 直线 | 1.0 米 |
+| T2 | 单个 90 度转弯 | 1.0 至 1.5 米 |
+| T3 | S 形连续转弯 | 2.0 至 3.0 米 |
+| T4 | 包含倒车 | 0.5 至 1.0 米 |
+| T5 | 锥桶触发重规划 | 1.5 至 2.0 米 |
+
+跟踪器基线通过标准：
+
+- 到点成功率 >= 90%（每类至少 5 次，总计至少 25 次）；
+- 横向误差 RMS <= 0.12 米；
+- 横向误差 p95 <= 0.20 米；
+- 最大横向误差 <= 0.30 米；
+- 最终位置误差 <= 0.15 米；
+- 无碰撞、压线、失控或错误换挡；
+- 定位/里程计过期时能停车；
+- 取消到零命令 p95 <= 0.25 秒；
+- TF 查询失败为 0。
+
+这些是当前 0.20 m/s 低速配置的基线。如果场地测量精度不足，应先改善定位，不应直接放宽安全指标。
+
+## 8. 阶段 4：故障与恢复
+
+| 故障注入 | 合格行为 |
+|---|---|
+| 拔掉雷达或停止 `/scan` | AMCL 不可信，不再发送新目标；人工取消 |
+| 暂停 `/odom_combined` | 跟踪器在 `max_odom_age` 后发布零速度 |
+| 发布路径上的锥桶 | 安全重规划；无安全路径则停车 |
+| 发布 `/navigation_cancel` | 清目标、清路径并停车 |
+| 启动第二个 `/cmd_vel` 发布者 | 预检失败，禁止测试 |
+| 目标在地图外或障碍物内 | 空路径并报告拒绝/失败 |
+
+关键安全项出现一次失败，即本轮版本不合格，修复后从阶段 1 重新回归。
+
+## 9. 最终判定
+
+只有同时满足以下条件，才能认为规划与跟踪方案适合当前低速实车：
+
+1. 阶段 0 全部通过。
+2. 规划成功率、延迟、碰撞和转弯半径全部达标。
+3. 25 次以上落地测试满足成功率与误差指标。
+4. 取消、定位过期和无路场景均能可靠停车。
+5. 没有任何碰撞、失控、错误方向或多控制器竞争。

src/planner/config/odom_hybrid_astar.yaml

@@ -0,0 +1,66 @@
+grid_astar_theta_planner:
+  ros__parameters:
+    use_sim_time: false
+
+    # Odom-only real-robot test:
+    # - /odom is the only localization source.
+    # - planner_odom_map_tf publishes map->odom from the provided start pose.
+    # - /map is optional static geometry from nav2_map_server; no lidar/AMCL is used.
+    map_frame: map
+    odom_topic: /odom
+    goal_pose_topic: /goal_pose
+    obstacles_topic: /random_obstacles
+    clear_obstacles_topic: /clear_random_obstacles
+    cancel_topic: /navigation_cancel
+    costmap_topic: /map
+    plan_topic: /plan
+    status_topic: /hybrid_astar_status
+
+    # real_10x10.yaml fallback bounds. When /map arrives, exact map bounds are
+    # applied because fit_map_to_costmap is true.
+    map_min_x: -5.0
+    map_min_y: -5.0
+    map_max_x: 5.0
+    map_max_y: 5.0
+    # A 10 cm planning grid keeps odom-only real-vehicle planning responsive
+    # over the full 10 x 10 m test area. The map remains used for collision
+    # checks at its native resolution.
+    resolution: 0.10
+    fit_map_to_costmap: true
+
+    obstacle_format: xyr
+    default_obstacle_radius: 0.15
+    unknown_obstacle_radius: 0.15
+    accumulate_obstacles: false
+    obstacle_merge_distance: 0.20
+    robot_radius: 0.14
+    localization_error: 0.05
+    latency_margin: 0.03
+    safety_margin: 0.06
+    costmap_occupied_threshold: 50
+    unknown_is_obstacle: true
+    outside_costmap_is_obstacle: true
+    inflate_static_map: true
+
+    min_turning_radius: 0.25
+    yaw_bins: 36
+    primitive_length: 0.12
+    collision_sample_step: 0.03
+    goal_xy_tolerance: 0.06
+    goal_yaw_tolerance: 0.174533
+    require_goal_yaw: false
+    use_grid_heuristic: true
+    heuristic_weight: 1.35
+    goal_yaw_heuristic_weight: 0.0
+    reverse_penalty: 1.5
+    turn_penalty: 0.05
+    gear_change_penalty: 0.25
+    steering_change_penalty: 0.02
+    max_iterations: 200000
+    planning_timeout_ms: 1500.0
+    replan_rate: 2.0
+    plan_on_obstacle_update: true
+    max_odom_age: 0.80
+    tf_timeout: 0.10
+    state_log_period: 1.0
+    search_progress_log_period_ms: 500.0

src/planner/config/odom_pure_pursuit.yaml

@@ -0,0 +1,35 @@
+topology_pure_pursuit:
+  ros__parameters:
+    use_sim_time: false
+    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: /planner_cmd_vel
+    status_topic: /pure_pursuit_status
+    cancel_topic: /navigation_cancel
+
+    control_rate: 20.0
+    lookahead_distance: 0.40
+    goal_tolerance: 0.06
+    goal_yaw_tolerance: 0.174533
+    align_goal_yaw: false
+    final_alignment_offset: 0.25
+    final_alignment_speed: 0.06
+    linear_speed: 0.60
+    min_linear_speed: 0.08
+    reverse_speed: 0.25
+    slowdown_distance: 0.35
+    curvature_slowdown_gain: 0.10
+    max_angular_speed: 2.80
+    min_turning_radius: 0.25
+
+    latency_compensation: true
+    max_latency_compensation: 0.15
+    max_odom_age: 0.80
+    tf_timeout: 0.10
+    state_log_period: 1.0

src/planner/config/real_amcl.yaml

@@ -0,0 +1,41 @@
+amcl:
+  ros__parameters:
+    use_sim_time: false
+    alpha1: 0.20
+    alpha2: 0.20
+    alpha3: 0.20
+    alpha4: 0.20
+    alpha5: 0.20
+    base_frame_id: base_footprint
+    global_frame_id: map
+    odom_frame_id: odom
+    scan_topic: /scan
+    laser_model_type: likelihood_field
+    laser_min_range: 0.15
+    laser_max_range: 20.0
+    laser_likelihood_max_dist: 2.0
+    max_beams: 60
+    min_particles: 500
+    max_particles: 2000
+    pf_err: 0.05
+    pf_z: 0.99
+    resample_interval: 1
+    robot_model_type: nav2_amcl::DifferentialMotionModel
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 0.5
+    update_min_a: 0.15
+    update_min_d: 0.10
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: false
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false

src/planner/config/real_hybrid_astar.yaml

@@ -0,0 +1,64 @@
+grid_astar_theta_planner:
+  ros__parameters:
+    use_sim_time: false
+
+    # Real-robot frames and interfaces.
+    map_frame: map
+    odom_topic: /odom_combined
+    goal_pose_topic: /goal_pose
+    obstacles_topic: /random_obstacles
+    clear_obstacles_topic: /clear_random_obstacles
+    cancel_topic: /navigation_cancel
+    # nav2_map_server publishes an OccupancyGrid on /map. The planner can
+    # consume it directly, so a second Nav2 costmap is not required.
+    costmap_topic: /map
+    plan_topic: /plan
+    status_topic: /hybrid_astar_status
+
+    # Fallback is a 5 x 5 m square. When /map arrives, its origin and size are
+    # used because fit_map_to_costmap is enabled.
+    map_min_x: 0.0
+    map_min_y: 0.0
+    map_max_x: 5.0
+    map_max_y: 5.0
+    resolution: 0.05
+    fit_map_to_costmap: true
+
+    obstacle_format: xyr
+    default_obstacle_radius: 0.15
+    unknown_obstacle_radius: 0.15
+    accumulate_obstacles: false
+    obstacle_merge_distance: 0.20
+    robot_radius: 0.14
+    localization_error: 0.05
+    latency_margin: 0.03
+    safety_margin: 0.06
+    costmap_occupied_threshold: 50
+    unknown_is_obstacle: true
+    outside_costmap_is_obstacle: true
+    inflate_static_map: true
+
+    min_turning_radius: 0.40
+    yaw_bins: 72
+    primitive_length: 0.12
+    collision_sample_step: 0.03
+    goal_xy_tolerance: 0.15
+    goal_yaw_tolerance: 0.174533
+    # Point navigation does not require the car to finish at a specific yaw.
+    # Set true when final docking orientation is required.
+    require_goal_yaw: false
+    use_grid_heuristic: true
+    heuristic_weight: 1.25
+    goal_yaw_heuristic_weight: 0.0
+    reverse_penalty: 1.5
+    turn_penalty: 0.05
+    gear_change_penalty: 0.25
+    steering_change_penalty: 0.02
+    max_iterations: 100000
+    planning_timeout_ms: 500.0
+    replan_rate: 2.0
+    plan_on_obstacle_update: true
+    max_odom_age: 0.50
+    tf_timeout: 0.10
+    state_log_period: 1.0
+    search_progress_log_period_ms: 500.0

src/planner/config/real_pure_pursuit.yaml

@@ -0,0 +1,33 @@
+topology_pure_pursuit:
+  ros__parameters:
+    use_sim_time: false
+    enable_topology_planning: false
+    accept_external_plan: true
+    publish_cmd_vel: true
+    stop_without_plan: true
+
+    map_frame: map
+    odom_topic: /odom_combined
+    external_plan_topic: /plan
+    cmd_vel_topic: /planner_cmd_vel
+    status_topic: /pure_pursuit_status
+    cancel_topic: /navigation_cancel
+
+    control_rate: 20.0
+    lookahead_distance: 0.45
+    goal_tolerance: 0.15
+    goal_yaw_tolerance: 0.174533
+    align_goal_yaw: false
+    linear_speed: 0.20
+    min_linear_speed: 0.06
+    reverse_speed: 0.10
+    slowdown_distance: 0.70
+    curvature_slowdown_gain: 0.25
+    max_angular_speed: 1.20
+    min_turning_radius: 0.40
+
+    latency_compensation: true
+    max_latency_compensation: 0.15
+    max_odom_age: 0.50
+    tf_timeout: 0.10
+    state_log_period: 1.0

src/planner/launch/odom_hybrid_astar.launch.py

@@ -0,0 +1,394 @@
+#!/usr/bin/env python3
+"""Odom-only bringup for Hybrid A* planning and Pure Pursuit tracking.
+
+This launch does not start lidar or AMCL. The user provides the known current
+map pose; planner_odom_map_tf aligns map->odom once, then localization is
+wheel odom only.
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    planner_share = get_package_share_directory("planner")
+    description_share = get_package_share_directory("origincar_description")
+
+    map_yaml = LaunchConfiguration("map")
+    use_map = LaunchConfiguration("use_map")
+    start_base = LaunchConfiguration("start_base")
+    start_robot_state_publisher = LaunchConfiguration("start_robot_state_publisher")
+    serial_port = LaunchConfiguration("serial_port")
+    serial_baud = LaunchConfiguration("serial_baud")
+    enable_motion = LaunchConfiguration("enable_motion")
+    wheelbase = LaunchConfiguration("wheelbase")
+    max_steering_angle = LaunchConfiguration("max_steering_angle")
+    map_frame = LaunchConfiguration("map_frame")
+    odom_frame = LaunchConfiguration("odom_frame")
+    base_frame = LaunchConfiguration("base_frame")
+    odom_topic = LaunchConfiguration("odom_topic")
+    initial_x = LaunchConfiguration("initial_x")
+    initial_y = LaunchConfiguration("initial_y")
+    initial_yaw = LaunchConfiguration("initial_yaw")
+    initialize_on_first_odom = LaunchConfiguration("initialize_on_first_odom")
+    planner_min_turning_radius = LaunchConfiguration("planner_min_turning_radius")
+    tracker_min_turning_radius = LaunchConfiguration("tracker_min_turning_radius")
+    lookahead_distance = LaunchConfiguration("lookahead_distance")
+    linear_speed = LaunchConfiguration("linear_speed")
+    min_linear_speed = LaunchConfiguration("min_linear_speed")
+    reverse_speed = LaunchConfiguration("reverse_speed")
+    slowdown_distance = LaunchConfiguration("slowdown_distance")
+    curvature_slowdown_gain = LaunchConfiguration("curvature_slowdown_gain")
+    max_angular_speed = LaunchConfiguration("max_angular_speed")
+    goal_tolerance = LaunchConfiguration("goal_tolerance")
+    goal_yaw_tolerance = LaunchConfiguration("goal_yaw_tolerance")
+    align_goal_yaw = LaunchConfiguration("align_goal_yaw")
+    final_alignment_offset = LaunchConfiguration("final_alignment_offset")
+    final_alignment_speed = LaunchConfiguration("final_alignment_speed")
+    require_goal_yaw = LaunchConfiguration("require_goal_yaw")
+    planner_goal_xy_tolerance = LaunchConfiguration("planner_goal_xy_tolerance")
+    use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
+    heuristic_weight = LaunchConfiguration("heuristic_weight")
+    goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
+    planner_resolution = LaunchConfiguration("planner_resolution")
+    yaw_bins = LaunchConfiguration("yaw_bins")
+    primitive_length = LaunchConfiguration("primitive_length")
+    max_iterations = LaunchConfiguration("max_iterations")
+    planning_timeout_ms = LaunchConfiguration("planning_timeout_ms")
+    state_log_period = LaunchConfiguration("state_log_period")
+    search_progress_log_period_ms = LaunchConfiguration("search_progress_log_period_ms")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+
+    serial_baud_value = ParameterValue(serial_baud, value_type=int)
+    enable_motion_value = ParameterValue(enable_motion, value_type=bool)
+    wheelbase_value = ParameterValue(wheelbase, value_type=float)
+    max_steering_angle_value = ParameterValue(max_steering_angle, value_type=float)
+    initial_x_value = ParameterValue(initial_x, value_type=float)
+    initial_y_value = ParameterValue(initial_y, value_type=float)
+    initial_yaw_value = ParameterValue(initial_yaw, value_type=float)
+    initialize_on_first_odom_value = ParameterValue(
+        initialize_on_first_odom, value_type=bool
+    )
+    planner_min_turning_radius_value = ParameterValue(
+        planner_min_turning_radius, value_type=float
+    )
+    tracker_min_turning_radius_value = ParameterValue(
+        tracker_min_turning_radius, value_type=float
+    )
+    lookahead_distance_value = ParameterValue(lookahead_distance, value_type=float)
+    linear_speed_value = ParameterValue(linear_speed, value_type=float)
+    min_linear_speed_value = ParameterValue(min_linear_speed, value_type=float)
+    reverse_speed_value = ParameterValue(reverse_speed, value_type=float)
+    slowdown_distance_value = ParameterValue(slowdown_distance, value_type=float)
+    curvature_slowdown_gain_value = ParameterValue(
+        curvature_slowdown_gain, value_type=float
+    )
+    max_angular_speed_value = ParameterValue(max_angular_speed, value_type=float)
+    goal_tolerance_value = ParameterValue(goal_tolerance, value_type=float)
+    goal_yaw_tolerance_value = ParameterValue(goal_yaw_tolerance, value_type=float)
+    align_goal_yaw_value = ParameterValue(align_goal_yaw, value_type=bool)
+    final_alignment_offset_value = ParameterValue(
+        final_alignment_offset, value_type=float
+    )
+    final_alignment_speed_value = ParameterValue(
+        final_alignment_speed, value_type=float
+    )
+    require_goal_yaw_value = ParameterValue(require_goal_yaw, value_type=bool)
+    planner_goal_xy_tolerance_value = ParameterValue(
+        planner_goal_xy_tolerance, value_type=float
+    )
+    use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
+    heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
+    goal_yaw_heuristic_weight_value = ParameterValue(
+        goal_yaw_heuristic_weight, value_type=float
+    )
+    planner_resolution_value = ParameterValue(planner_resolution, value_type=float)
+    yaw_bins_value = ParameterValue(yaw_bins, value_type=int)
+    primitive_length_value = ParameterValue(primitive_length, value_type=float)
+    max_iterations_value = ParameterValue(max_iterations, value_type=int)
+    planning_timeout_ms_value = ParameterValue(planning_timeout_ms, value_type=float)
+    state_log_period_value = ParameterValue(state_log_period, value_type=float)
+    search_progress_log_period_ms_value = ParameterValue(
+        search_progress_log_period_ms, value_type=float
+    )
+    use_sim_time_value = ParameterValue(use_sim_time, value_type=bool)
+
+    urdf_path = os.path.join(description_share, "urdf", "origincar.urdf")
+    planner_path = os.path.join(planner_share, "config", "odom_hybrid_astar.yaml")
+    tracker_path = os.path.join(planner_share, "config", "odom_pure_pursuit.yaml")
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        condition=IfCondition(start_robot_state_publisher),
+        parameters=[{
+            "robot_description": open(urdf_path, encoding="utf-8").read(),
+            "use_sim_time": use_sim_time_value,
+        }],
+    )
+
+    base_node = Node(
+        package="origincar_base",
+        executable="origincar_base_node",
+        name="origincar_base",
+        output="screen",
+        condition=IfCondition(start_base),
+        parameters=[{
+            "usart_port_name": serial_port,
+            "serial_baud_rate": serial_baud_value,
+            "robot_frame_id": base_frame,
+            "odom_frame_id": odom_frame,
+            "gyro_frame_id": "gyro_link",
+            "cmd_vel": "/planner_disabled_cmd_vel",
+            "akm_cmd_vel": "/ackermann_cmd",
+            "publish_tf": True,
+            "gyro_z_low_pass_alpha": 0.6,
+        }],
+    )
+
+    cmd_vel_bridge = Node(
+        package="planner",
+        executable="ackermann_cmd_bridge.py",
+        name="planner_ackermann_cmd_bridge",
+        output="screen",
+        condition=IfCondition(enable_motion),
+        parameters=[{
+            "input_topic": "/planner_cmd_vel",
+            "output_topic": "/ackermann_cmd",
+            "wheelbase": wheelbase_value,
+            "frame_id": base_frame,
+            "max_steering_angle": max_steering_angle_value,
+            "command_timeout": 0.25,
+            "watchdog_rate": 10.0,
+            "state_log_period": state_log_period_value,
+        }],
+    )
+
+    map_server = Node(
+        package="nav2_map_server",
+        executable="map_server",
+        name="map_server",
+        output="screen",
+        condition=IfCondition(use_map),
+        parameters=[{"yaml_filename": map_yaml, "use_sim_time": use_sim_time_value}],
+    )
+
+    lifecycle_manager = Node(
+        package="nav2_lifecycle_manager",
+        executable="lifecycle_manager",
+        name="lifecycle_manager_odom_map",
+        output="screen",
+        condition=IfCondition(use_map),
+        parameters=[{
+            "use_sim_time": use_sim_time_value,
+            "autostart": True,
+            "node_names": ["map_server"],
+        }],
+    )
+
+    odom_map_tf = Node(
+        package="planner",
+        executable="odom_map_tf.py",
+        name="planner_odom_map_tf",
+        output="screen",
+        parameters=[{
+            "use_sim_time": use_sim_time_value,
+            "map_frame": map_frame,
+            "odom_frame": odom_frame,
+            "odom_topic": odom_topic,
+            "initial_x": initial_x_value,
+            "initial_y": initial_y_value,
+            "initial_yaw": initial_yaw_value,
+            "initialize_on_first_odom": initialize_on_first_odom_value,
+            "state_log_period": state_log_period_value,
+        }],
+    )
+
+    planner_node = Node(
+        package="planner",
+        executable="grid_astar_theta_node",
+        name="grid_astar_theta_planner",
+        output="screen",
+        parameters=[planner_path, {
+            "use_sim_time": use_sim_time_value,
+            "map_frame": map_frame,
+            "odom_topic": odom_topic,
+            "min_turning_radius": planner_min_turning_radius_value,
+            "require_goal_yaw": require_goal_yaw_value,
+            "goal_xy_tolerance": planner_goal_xy_tolerance_value,
+            "use_grid_heuristic": use_grid_heuristic_value,
+            "heuristic_weight": heuristic_weight_value,
+            "goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
+            "resolution": planner_resolution_value,
+            "yaw_bins": yaw_bins_value,
+            "primitive_length": primitive_length_value,
+            "max_iterations": max_iterations_value,
+            "planning_timeout_ms": planning_timeout_ms_value,
+            "state_log_period": state_log_period_value,
+            "search_progress_log_period_ms": search_progress_log_period_ms_value,
+        }],
+    )
+
+    tracker_node = Node(
+        package="planner",
+        executable="topology_pure_pursuit_node.py",
+        name="topology_pure_pursuit",
+        output="screen",
+        parameters=[tracker_path, {
+            "use_sim_time": use_sim_time_value,
+            "map_frame": map_frame,
+            "odom_topic": odom_topic,
+            "publish_cmd_vel": enable_motion_value,
+            "cmd_vel_topic": "/planner_cmd_vel",
+            "min_turning_radius": tracker_min_turning_radius_value,
+            "lookahead_distance": lookahead_distance_value,
+            "linear_speed": linear_speed_value,
+            "min_linear_speed": min_linear_speed_value,
+            "reverse_speed": reverse_speed_value,
+            "slowdown_distance": slowdown_distance_value,
+            "curvature_slowdown_gain": curvature_slowdown_gain_value,
+            "max_angular_speed": max_angular_speed_value,
+            "goal_tolerance": goal_tolerance_value,
+            "goal_yaw_tolerance": goal_yaw_tolerance_value,
+            "align_goal_yaw": align_goal_yaw_value,
+            "final_alignment_offset": final_alignment_offset_value,
+            "final_alignment_speed": final_alignment_speed_value,
+            "state_log_period": state_log_period_value,
+        }],
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "map",
+            default_value=os.path.join(planner_share, "maps", "real_10x10.yaml"),
+            description="Static map YAML. Used only when use_map=true.",
+        ),
+        DeclareLaunchArgument("use_map", default_value="true"),
+        DeclareLaunchArgument("start_base", default_value="true"),
+        DeclareLaunchArgument("start_robot_state_publisher", default_value="true"),
+        DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
+        DeclareLaunchArgument("serial_baud", default_value="115200"),
+        DeclareLaunchArgument(
+            "enable_motion",
+            default_value="false",
+            description="Publish motion commands. Keep false for planner-only tests.",
+        ),
+        DeclareLaunchArgument("wheelbase", default_value="0.143"),
+        DeclareLaunchArgument("max_steering_angle", default_value="0.60"),
+        DeclareLaunchArgument("map_frame", default_value="map"),
+        DeclareLaunchArgument("odom_frame", default_value="odom"),
+        DeclareLaunchArgument("base_frame", default_value="base_link"),
+        DeclareLaunchArgument("odom_topic", default_value="/odom"),
+        DeclareLaunchArgument("initial_x", default_value="0.0"),
+        DeclareLaunchArgument("initial_y", default_value="0.0"),
+        DeclareLaunchArgument("initial_yaw", default_value="0.0"),
+        DeclareLaunchArgument(
+            "initialize_on_first_odom",
+            default_value="true",
+            description="Use launch initial pose when the first odom message arrives.",
+        ),
+        DeclareLaunchArgument(
+            "planner_min_turning_radius",
+            default_value="0.25",
+            description="Hybrid A* minimum turning radius in meters.",
+        ),
+        DeclareLaunchArgument(
+            "tracker_min_turning_radius",
+            default_value="0.25",
+            description="Pure Pursuit curvature limit in meters.",
+        ),
+        DeclareLaunchArgument(
+            "lookahead_distance",
+            default_value="0.40",
+            description="Pure Pursuit lookahead in meters.",
+        ),
+        DeclareLaunchArgument("linear_speed", default_value="0.60"),
+        DeclareLaunchArgument("min_linear_speed", default_value="0.08"),
+        DeclareLaunchArgument("reverse_speed", default_value="0.25"),
+        DeclareLaunchArgument("slowdown_distance", default_value="0.35"),
+        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.10"),
+        DeclareLaunchArgument("max_angular_speed", default_value="2.80"),
+        DeclareLaunchArgument("goal_tolerance", default_value="0.06"),
+        DeclareLaunchArgument("goal_yaw_tolerance", default_value="0.174533"),
+        DeclareLaunchArgument("align_goal_yaw", default_value="false"),
+        DeclareLaunchArgument("final_alignment_offset", default_value="0.25"),
+        DeclareLaunchArgument("final_alignment_speed", default_value="0.06"),
+        DeclareLaunchArgument(
+            "require_goal_yaw",
+            default_value="false",
+            description="Require Hybrid A* to match the goal yaw.",
+        ),
+        DeclareLaunchArgument(
+            "planner_goal_xy_tolerance",
+            default_value="0.06",
+            description="Hybrid A* final XY tolerance in meters.",
+        ),
+        DeclareLaunchArgument(
+            "use_grid_heuristic",
+            default_value="true",
+            description="Use obstacle-aware 2D grid distance as the Hybrid A* heuristic.",
+        ),
+        DeclareLaunchArgument(
+            "heuristic_weight",
+            default_value="1.35",
+            description="Weighted A* heuristic multiplier. Higher is faster but less optimal.",
+        ),
+        DeclareLaunchArgument(
+            "goal_yaw_heuristic_weight",
+            default_value="0.0",
+            description="Weight for final yaw guidance in the Hybrid A* heuristic.",
+        ),
+        DeclareLaunchArgument(
+            "planner_resolution",
+            default_value="0.10",
+            description="Hybrid A* search grid resolution in meters.",
+        ),
+        DeclareLaunchArgument(
+            "yaw_bins",
+            default_value="36",
+            description="Number of discrete Hybrid A* heading states.",
+        ),
+        DeclareLaunchArgument(
+            "primitive_length",
+            default_value="0.12",
+            description="Length of each Hybrid A* motion primitive in meters.",
+        ),
+        DeclareLaunchArgument(
+            "max_iterations",
+            default_value="200000",
+            description="Maximum Hybrid A* state expansions per plan.",
+        ),
+        DeclareLaunchArgument(
+            "planning_timeout_ms",
+            default_value="1500.0",
+            description="Hybrid A* planning time limit in milliseconds.",
+        ),
+        DeclareLaunchArgument(
+            "state_log_period",
+            default_value="1.0",
+            description="Seconds between runtime state logs. Set 0.0 to disable.",
+        ),
+        DeclareLaunchArgument(
+            "search_progress_log_period_ms",
+            default_value="500.0",
+            description="Milliseconds between Hybrid A* search progress logs. Set 0.0 to disable.",
+        ),
+        DeclareLaunchArgument("use_sim_time", default_value="false"),
+        robot_state_publisher,
+        base_node,
+        cmd_vel_bridge,
+        map_server,
+        lifecycle_manager,
+        odom_map_tf,
+        planner_node,
+        tracker_node,
+    ])

src/planner/launch/real_hybrid_astar.launch.py

@@ -0,0 +1,250 @@
+#!/usr/bin/env python3
+"""Real-robot bringup for the custom Hybrid A* and Pure Pursuit stack.
+
+This launch intentionally does not start Nav2 controller_server. The custom
+planner publishes /plan and the tracker publishes an internal Twist command
+that is converted to /ackermann_cmd by planner_ackermann_cmd_bridge.
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    planner_share = get_package_share_directory("planner")
+    base_share = get_package_share_directory("origincar_base")
+    description_share = get_package_share_directory("origincar_description")
+
+    map_yaml = LaunchConfiguration("map")
+    lidar_params = LaunchConfiguration("lidar_params")
+    lidar_serial_port = LaunchConfiguration("lidar_serial_port")
+    lidar_model = LaunchConfiguration("lidar_model")
+    start_lidar = LaunchConfiguration("start_lidar")
+    serial_port = LaunchConfiguration("serial_port")
+    serial_baud = LaunchConfiguration("serial_baud")
+    enable_motion = LaunchConfiguration("enable_motion")
+    wheelbase = LaunchConfiguration("wheelbase")
+    max_steering_angle = LaunchConfiguration("max_steering_angle")
+    use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
+    heuristic_weight = LaunchConfiguration("heuristic_weight")
+    goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
+    state_log_period = LaunchConfiguration("state_log_period")
+    search_progress_log_period_ms = LaunchConfiguration("search_progress_log_period_ms")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    serial_baud_value = ParameterValue(serial_baud, value_type=int)
+    enable_motion_value = ParameterValue(enable_motion, value_type=bool)
+    wheelbase_value = ParameterValue(wheelbase, value_type=float)
+    max_steering_angle_value = ParameterValue(max_steering_angle, value_type=float)
+    use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
+    heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
+    goal_yaw_heuristic_weight_value = ParameterValue(
+        goal_yaw_heuristic_weight, value_type=float
+    )
+    state_log_period_value = ParameterValue(state_log_period, value_type=float)
+    search_progress_log_period_ms_value = ParameterValue(
+        search_progress_log_period_ms, value_type=float
+    )
+    use_sim_time_value = ParameterValue(use_sim_time, value_type=bool)
+
+    urdf_path = os.path.join(description_share, "urdf", "origincar.urdf")
+    ekf_path = os.path.join(base_share, "config", "ekf.yaml")
+    amcl_path = os.path.join(planner_share, "config", "real_amcl.yaml")
+    planner_path = os.path.join(planner_share, "config", "real_hybrid_astar.yaml")
+    tracker_path = os.path.join(planner_share, "config", "real_pure_pursuit.yaml")
+
+    # xacro is not needed here because origincar.urdf is already generated and
+    # is the file used by the existing real-robot launch files.
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{"robot_description": open(urdf_path, encoding="utf-8").read(),
+                     "use_sim_time": use_sim_time_value}],
+    )
+
+    base_node = Node(
+        package="origincar_base",
+        executable="origincar_base_node",
+        name="origincar_base",
+        output="screen",
+        parameters=[{
+            "usart_port_name": serial_port,
+            "serial_baud_rate": serial_baud_value,
+            "robot_frame_id": "base_link",
+            "odom_frame_id": "odom",
+            "gyro_frame_id": "gyro_link",
+            # The bridge consumes /planner_cmd_vel. This prevents the same
+            # command from reaching the serial driver in two formats.
+            "cmd_vel": "/planner_disabled_cmd_vel",
+            "akm_cmd_vel": "/ackermann_cmd",
+            "publish_tf": False,
+            "gyro_z_low_pass_alpha": 0.6,
+        }],
+    )
+
+    cmd_vel_bridge = Node(
+        package="planner",
+        executable="ackermann_cmd_bridge.py",
+        name="planner_ackermann_cmd_bridge",
+        output="screen",
+        condition=IfCondition(enable_motion),
+        parameters=[{
+            "input_topic": "/planner_cmd_vel",
+            "output_topic": "/ackermann_cmd",
+            "wheelbase": wheelbase_value,
+            "frame_id": "base_link",
+            "max_steering_angle": max_steering_angle_value,
+            "command_timeout": 0.25,
+            "watchdog_rate": 10.0,
+            "state_log_period": state_log_period_value,
+        }],
+    )
+
+    ekf_node = Node(
+        package="robot_localization",
+        executable="ekf_node",
+        name="ekf_filter_node",
+        output="screen",
+        parameters=[ekf_path, {"use_sim_time": use_sim_time_value}],
+        remappings=[("odometry/filtered", "/odom_combined")],
+    )
+
+    lidar_node = Node(
+        package="lslidar_driver",
+        executable="lslidar_driver_node",
+        name="lslidar_driver_node",
+        output="screen",
+        condition=IfCondition(start_lidar),
+        parameters=[lidar_params, {
+            "interface_selection": "serial",
+            "serial_port_": lidar_serial_port,
+            "lidar_name": lidar_model,
+        }],
+    )
+
+    map_server = Node(
+        package="nav2_map_server",
+        executable="map_server",
+        name="map_server",
+        output="screen",
+        parameters=[{"yaml_filename": map_yaml, "use_sim_time": use_sim_time_value}],
+    )
+
+    amcl_node = Node(
+        package="nav2_amcl",
+        executable="amcl",
+        name="amcl",
+        output="screen",
+        parameters=[amcl_path, {"use_sim_time": use_sim_time_value}],
+    )
+
+    lifecycle_manager = Node(
+        package="nav2_lifecycle_manager",
+        executable="lifecycle_manager",
+        name="lifecycle_manager_localization",
+        output="screen",
+        parameters=[{
+            "use_sim_time": use_sim_time_value,
+            "autostart": True,
+            "node_names": ["map_server", "amcl"],
+        }],
+    )
+
+    planner_node = Node(
+        package="planner",
+        executable="grid_astar_theta_node",
+        name="grid_astar_theta_planner",
+        output="screen",
+        parameters=[planner_path, {
+            "use_sim_time": use_sim_time_value,
+            "use_grid_heuristic": use_grid_heuristic_value,
+            "heuristic_weight": heuristic_weight_value,
+            "goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
+            "state_log_period": state_log_period_value,
+            "search_progress_log_period_ms": search_progress_log_period_ms_value,
+        }],
+    )
+
+    tracker_node = Node(
+        package="planner",
+        executable="topology_pure_pursuit_node.py",
+        name="topology_pure_pursuit",
+        output="screen",
+        parameters=[tracker_path, {
+            "use_sim_time": use_sim_time_value,
+            "publish_cmd_vel": enable_motion_value,
+            "cmd_vel_topic": "/planner_cmd_vel",
+            "state_log_period": state_log_period_value,
+        }],
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "map",
+            description="Required: real 5x5m map YAML used by map_server.",
+        ),
+        DeclareLaunchArgument(
+            "lidar_params",
+            description="Required: LSLIDAR YAML matching the actual sensor/interface.",
+        ),
+        DeclareLaunchArgument("lidar_serial_port", default_value="/dev/ttyCH343USB0"),
+        DeclareLaunchArgument("lidar_model", default_value="N10"),
+        DeclareLaunchArgument(
+            "start_lidar",
+            default_value="true",
+            description="Start LSLIDAR. Set false when /scan is provided externally.",
+        ),
+        DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
+        DeclareLaunchArgument("serial_baud", default_value="115200"),
+        DeclareLaunchArgument(
+            "enable_motion",
+            default_value="false",
+            description="Publish motion commands. Keep false for the first planner-only test.",
+        ),
+        DeclareLaunchArgument("wheelbase", default_value="0.143"),
+        DeclareLaunchArgument("max_steering_angle", default_value="0.60"),
+        DeclareLaunchArgument(
+            "use_grid_heuristic",
+            default_value="true",
+            description="Use obstacle-aware 2D grid distance as the Hybrid A* heuristic.",
+        ),
+        DeclareLaunchArgument(
+            "heuristic_weight",
+            default_value="1.25",
+            description="Weighted A* heuristic multiplier. Higher is faster but less optimal.",
+        ),
+        DeclareLaunchArgument(
+            "goal_yaw_heuristic_weight",
+            default_value="0.0",
+            description="Weight for final yaw guidance in the Hybrid A* heuristic.",
+        ),
+        DeclareLaunchArgument(
+            "state_log_period",
+            default_value="1.0",
+            description="Seconds between runtime state logs. Set 0.0 to disable.",
+        ),
+        DeclareLaunchArgument(
+            "search_progress_log_period_ms",
+            default_value="500.0",
+            description="Milliseconds between Hybrid A* search progress logs. Set 0.0 to disable.",
+        ),
+        DeclareLaunchArgument("use_sim_time", default_value="false"),
+        robot_state_publisher,
+        base_node,
+        cmd_vel_bridge,
+        ekf_node,
+        lidar_node,
+        map_server,
+        amcl_node,
+        lifecycle_manager,
+        planner_node,
+        tracker_node,
+    ])

src/planner/maps/real_10x10.yaml

@@ -0,0 +1,7 @@
+image: real_5x5.pgm
+mode: trinary
+resolution: 0.10
+origin: [-5.0, -5.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/planner/maps/real_5x5.yaml

@@ -0,0 +1,7 @@
+image: real_5x5.pgm
+mode: trinary
+resolution: 0.05
+origin: [-2.5, -2.5, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/planner/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
   <name>planner</name>
   <version>0.0.0</version>
-  <description>Minimal Nav2 global planning stack: slam_toolbox + planner_server + costmaps + lifecycle_manager</description>
+  <description>Lightweight Ackermann Hybrid A* planner, path tracker, and real-robot test tools</description>
   <maintainer email="2314753575@qq.com">sunrise</maintainer>
   <license>TODO: License declaration</license>
 
@@ -16,13 +16,25 @@
   <depend>slam_toolbox</depend>
   <depend>nav2_planner</depend>
   <depend>nav2_costmap_2d</depend>
-  <depend>nav2_lifecycle_manager</depend>
   <depend>nav2_common</depend>
   <depend>nav2_util</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>ackermann_msgs</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>origincar_base</exec_depend>
+  <exec_depend>origincar_description</exec_depend>
+  <exec_depend>robot_localization</exec_depend>
+  <exec_depend>nav2_map_server</exec_depend>
+  <exec_depend>nav2_amcl</exec_depend>
+  <exec_depend>nav2_lifecycle_manager</exec_depend>
+  <exec_depend>lslidar_driver</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>rcl_interfaces</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/planner/scripts/ackermann_cmd_bridge.py

@@ -0,0 +1,148 @@
+#!/usr/bin/env python3
+"""Convert Twist yaw-rate commands to Ackermann steering commands."""
+
+import math
+import time
+
+import rclpy
+from ackermann_msgs.msg import AckermannDriveStamped
+from geometry_msgs.msg import Twist
+from rclpy.node import Node
+
+
+class AckermannCmdBridge(Node):
+    def __init__(self):
+        super().__init__("planner_ackermann_cmd_bridge")
+        self.declare_parameter("input_topic", "/cmd_vel")
+        self.declare_parameter("output_topic", "/ackermann_cmd")
+        self.declare_parameter("wheelbase", 0.143)
+        self.declare_parameter("max_steering_angle", 0.60)
+        self.declare_parameter("frame_id", "base_link")
+        self.declare_parameter("command_timeout", 0.25)
+        self.declare_parameter("watchdog_rate", 10.0)
+        self.declare_parameter("state_log_period", 1.0)
+
+        input_topic = str(self.get_parameter("input_topic").value)
+        output_topic = str(self.get_parameter("output_topic").value)
+        self.wheelbase = float(self.get_parameter("wheelbase").value)
+        self.max_steering_angle = abs(
+            float(self.get_parameter("max_steering_angle").value)
+        )
+        self.frame_id = str(self.get_parameter("frame_id").value)
+        self.command_timeout = max(
+            0.05, float(self.get_parameter("command_timeout").value)
+        )
+        self.state_log_period = max(
+            0.0, float(self.get_parameter("state_log_period").value)
+        )
+        if self.wheelbase <= 0.0:
+            raise ValueError("wheelbase must be positive")
+        if self.max_steering_angle <= 0.0:
+            raise ValueError("max_steering_angle must be positive")
+
+        self.publisher = self.create_publisher(
+            AckermannDriveStamped, output_topic, 10
+        )
+        self.subscription = self.create_subscription(
+            Twist, input_topic, self._cmd_callback, 10
+        )
+        self.last_command_time = None
+        self.last_idle_publish_time = 0.0
+        self.last_state_log_time = 0.0
+        self.last_input_linear = 0.0
+        self.last_input_angular = 0.0
+        self.last_output_velocity = 0.0
+        self.last_output_steering = 0.0
+        self.last_steering_saturated = False
+        watchdog_rate = max(2.0, float(self.get_parameter("watchdog_rate").value))
+        self.watchdog_timer = self.create_timer(1.0 / watchdog_rate, self._watchdog)
+        self.get_logger().info(
+            f"bridge {input_topic} -> {output_topic}, "
+            f"wheelbase={self.wheelbase:.3f} m, "
+            f"steering_limit={self.max_steering_angle:.3f} rad, "
+            f"command_timeout={self.command_timeout:.3f} s"
+        )
+
+    def _cmd_callback(self, command):
+        self.last_command_time = time.monotonic()
+        velocity = command.linear.x
+        self.last_input_linear = velocity
+        self.last_input_angular = command.angular.z
+        if abs(velocity) <= 1e-6 or abs(command.angular.z) <= 1e-6:
+            steering = 0.0
+            raw_steering = 0.0
+        else:
+            raw_steering = math.atan(self.wheelbase * command.angular.z / velocity)
+            steering = raw_steering
+        steering = max(
+            -self.max_steering_angle,
+            min(self.max_steering_angle, steering),
+        )
+        self.last_steering_saturated = abs(raw_steering - steering) > 1e-6
+
+        self._publish(velocity, steering)
+
+    def _watchdog(self):
+        now = time.monotonic()
+        command_stale = (
+            self.last_command_time is None
+            or now - self.last_command_time > self.command_timeout
+        )
+        if command_stale and now - self.last_idle_publish_time >= 0.5:
+            self._publish(0.0, 0.0)
+            self.last_idle_publish_time = now
+
+    def _publish(self, velocity, steering):
+        self.last_output_velocity = float(velocity)
+        self.last_output_steering = float(steering)
+        output = AckermannDriveStamped()
+        output.header.stamp = self.get_clock().now().to_msg()
+        output.header.frame_id = self.frame_id
+        output.drive.speed = self.last_output_velocity
+        output.drive.steering_angle = self.last_output_steering
+        self.publisher.publish(output)
+        self._maybe_log_state()
+
+    def _maybe_log_state(self):
+        if self.state_log_period <= 0.0:
+            return
+        now = time.monotonic()
+        if now - self.last_state_log_time < self.state_log_period:
+            return
+        self.last_state_log_time = now
+        if self.last_command_time is None:
+            command_age = None
+            stale = True
+        else:
+            command_age = max(0.0, now - self.last_command_time)
+            stale = command_age > self.command_timeout
+        age_text = "none" if command_age is None else f"{command_age:.3f}"
+        self.get_logger().info(
+            "ackermann_state "
+            f"input_twist=({self.last_input_linear:.3f},{self.last_input_angular:.3f}) "
+            f"output=(speed={self.last_output_velocity:.3f},"
+            f"steering={self.last_output_steering:.3f}) "
+            f"command_age={age_text} stale={stale} "
+            f"steering_saturated={self.last_steering_saturated}"
+        )
+
+    def stop(self):
+        for _ in range(3):
+            self._publish(0.0, 0.0)
+
+
+def main():
+    rclpy.init()
+    node = AckermannCmdBridge()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.stop()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/planner/scripts/evaluate_navigation_test.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+"""Evaluate navigation test summaries against explicit acceptance thresholds."""
+
+import argparse
+import glob
+import json
+from pathlib import Path
+import sys
+
+
+def nested_value(data, *keys):
+    value = data
+    for key in keys:
+        if not isinstance(value, dict) or key not in value:
+            return None
+        value = value[key]
+    return value
+
+
+def check_max(failures, label, value, limit):
+    if value is None:
+        failures.append(f"{label}: missing")
+    elif value > limit:
+        failures.append(f"{label}: {value:.4f} > {limit:.4f}")
+
+
+def check_min(failures, label, value, limit):
+    if value is None:
+        failures.append(f"{label}: missing")
+    elif value < limit:
+        failures.append(f"{label}: {value:.4f} < {limit:.4f}")
+
+
+def evaluate_planning(data, args):
+    failures = []
+    ok_count = int(data.get("plan_ok_status_count", 0))
+    fail_count = int(data.get("plan_fail_status_count", 0))
+    attempts = ok_count + fail_count
+    success_rate = ok_count / attempts if attempts else None
+    check_min(failures, "planning success rate", success_rate, args.min_plan_success)
+    check_max(
+        failures,
+        "planning latency p95 (s)",
+        nested_value(data, "planning_latency_sec", "p95"),
+        args.max_planning_p95,
+    )
+    check_max(
+        failures,
+        "occupied path points",
+        data.get("path_occupied_points_max"),
+        args.max_occupied_points,
+    )
+    minimum_radius = data.get("minimum_planned_turning_radius_m")
+    if minimum_radius is not None:
+        check_min(
+            failures,
+            "minimum turning radius (m)",
+            minimum_radius,
+            args.min_turning_radius,
+        )
+    elif int(data.get("plan_count", 0)) <= 0:
+        failures.append("minimum turning radius (m): missing because no path was recorded")
+    return failures
+
+
+def evaluate_tracking(data, args):
+    failures = []
+    check_max(
+        failures,
+        "cross-track RMS (m)",
+        nested_value(data, "cross_track_error_m", "rms"),
+        args.max_cross_track_rms,
+    )
+    check_max(
+        failures,
+        "cross-track p95 (m)",
+        nested_value(data, "cross_track_error_m", "p95"),
+        args.max_cross_track_p95,
+    )
+    check_max(
+        failures,
+        "cross-track max (m)",
+        nested_value(data, "cross_track_error_m", "max"),
+        args.max_cross_track,
+    )
+    check_max(
+        failures,
+        "final goal error (m)",
+        data.get("final_goal_error_m"),
+        args.max_goal_error,
+    )
+    check_max(
+        failures,
+        "cancel-to-zero p95 (s)",
+        nested_value(data, "cancel_to_zero_command_sec", "p95"),
+        args.max_cancel_latency,
+    )
+    check_max(
+        failures,
+        "Ackermann drive speed max (m/s)",
+        data.get("max_abs_drive_speed"),
+        args.max_drive_speed,
+    )
+    check_max(
+        failures,
+        "Ackermann steering max (rad)",
+        data.get("max_abs_steering_angle"),
+        args.max_steering_angle,
+    )
+    transform_failures = int(data.get("transform_failures", 0))
+    if transform_failures > args.max_transform_failures:
+        failures.append(
+            f"transform failures: {transform_failures} > {args.max_transform_failures}"
+        )
+    return failures
+
+
+def resolve_summary_paths(values):
+    paths = []
+    unmatched = []
+    for value in values:
+        expanded_value = str(Path(value).expanduser())
+        matches = sorted(glob.glob(expanded_value))
+        if not matches:
+            unmatched.append(value)
+            continue
+        paths.extend(Path(match) for match in matches if Path(match).is_file())
+    return paths, unmatched
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("mode", choices=("planning", "tracking"))
+    parser.add_argument("summaries", nargs="+")
+    parser.add_argument("--min-plan-success", type=float, default=0.95)
+    parser.add_argument("--max-planning-p95", type=float, default=0.50)
+    parser.add_argument("--max-occupied-points", type=int, default=0)
+    parser.add_argument("--min-turning-radius", type=float, default=0.36)
+    parser.add_argument("--max-cross-track-rms", type=float, default=0.12)
+    parser.add_argument("--max-cross-track-p95", type=float, default=0.20)
+    parser.add_argument("--max-cross-track", type=float, default=0.30)
+    parser.add_argument("--max-goal-error", type=float, default=0.15)
+    parser.add_argument("--max-cancel-latency", type=float, default=0.25)
+    parser.add_argument("--max-drive-speed", type=float, default=0.21)
+    parser.add_argument("--max-steering-angle", type=float, default=0.601)
+    parser.add_argument("--max-transform-failures", type=int, default=0)
+    args = parser.parse_args()
+
+    summary_paths, unmatched = resolve_summary_paths(args.summaries)
+    if unmatched:
+        for pattern in unmatched:
+            print(f"ERROR no summary files matched: {pattern}", file=sys.stderr)
+    if not summary_paths:
+        return 2
+
+    overall_success = True
+    for summary_path in summary_paths:
+        try:
+            data = json.loads(summary_path.read_text(encoding="utf-8"))
+        except (OSError, json.JSONDecodeError) as exc:
+            overall_success = False
+            print(f"FAIL {summary_path}")
+            print(f"  - unable to read valid JSON: {exc}")
+            continue
+        if args.mode == "planning":
+            failures = evaluate_planning(data, args)
+        else:
+            failures = evaluate_tracking(data, args)
+        if failures:
+            overall_success = False
+            print(f"FAIL {summary_path}")
+            for failure in failures:
+                print(f"  - {failure}")
+        else:
+            print(f"PASS {summary_path}")
+    return 0 if overall_success and not unmatched else 1
+
+
+if __name__ == "__main__":
+    sys.exit(main())

src/planner/scripts/goal_reach_debug_collector.py

@@ -0,0 +1,717 @@
+#!/usr/bin/env python3
+"""Collect data needed to diagnose why the robot does not reach a goal.
+
+This node is read-only. It subscribes to planner/tracker topics, queries key
+parameters, samples odom/command state, and writes one JSON report.
+"""
+
+import json
+import math
+from pathlib import Path
+import re
+import time
+
+import rclpy
+from geometry_msgs.msg import PoseStamped, Twist
+from nav_msgs.msg import Odometry, Path as NavPath
+from rcl_interfaces.msg import ParameterType
+from rcl_interfaces.srv import GetParameters
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.time import Time
+from std_msgs.msg import String
+from tf2_ros import Buffer, TransformException, TransformListener
+
+try:
+    from ackermann_msgs.msg import AckermannDriveStamped
+except ImportError:  # pragma: no cover - ackermann_msgs is a runtime dependency.
+    AckermannDriveStamped = None
+
+
+PLAN_OK_RE = re.compile(
+    r"plan_ok.*points=(?P<points>\d+).*iterations=(?P<iterations>\d+)"
+    r".*planning_ms=(?P<planning_ms>[-+]?\d+(?:\.\d+)?)"
+    r".*goal_error=(?P<goal_error>[-+]?\d+(?:\.\d+)?)"
+    r".*yaw_error=(?P<yaw_error>[-+]?\d+(?:\.\d+)?)"
+)
+
+
+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 finite_or_none(value):
+    if value is None:
+        return None
+    if isinstance(value, float) and not math.isfinite(value):
+        return None
+    return value
+
+
+def endpoint_to_dict(endpoint):
+    return {
+        "node_name": getattr(endpoint, "node_name", ""),
+        "node_namespace": getattr(endpoint, "node_namespace", ""),
+        "topic_type": getattr(endpoint, "topic_type", ""),
+    }
+
+
+def parameter_value_to_python_compatible(value):
+    """Convert rcl_interfaces/msg/ParameterValue on ROS 2 Humble and later."""
+    if value.type == ParameterType.PARAMETER_BOOL:
+        return bool(value.bool_value)
+    if value.type == ParameterType.PARAMETER_INTEGER:
+        return int(value.integer_value)
+    if value.type == ParameterType.PARAMETER_DOUBLE:
+        return float(value.double_value)
+    if value.type == ParameterType.PARAMETER_STRING:
+        return str(value.string_value)
+    if value.type == ParameterType.PARAMETER_BYTE_ARRAY:
+        return list(value.byte_array_value)
+    if value.type == ParameterType.PARAMETER_BOOL_ARRAY:
+        return list(value.bool_array_value)
+    if value.type == ParameterType.PARAMETER_INTEGER_ARRAY:
+        return list(value.integer_array_value)
+    if value.type == ParameterType.PARAMETER_DOUBLE_ARRAY:
+        return list(value.double_array_value)
+    if value.type == ParameterType.PARAMETER_STRING_ARRAY:
+        return list(value.string_array_value)
+    return None
+
+
+class GoalReachDebugCollector(Node):
+    def __init__(self):
+        super().__init__("goal_reach_debug_collector")
+
+        self.declare_parameter("duration", 30.0)
+        self.declare_parameter("sample_rate", 10.0)
+        self.declare_parameter("output_dir", "~/yiliao_ws/test_results")
+        self.declare_parameter("test_name", "goal_reach_debug")
+        self.declare_parameter("map_frame", "map")
+        self.declare_parameter("odom_topic", "/odom")
+        self.declare_parameter("goal_pose_topic", "/goal_pose")
+        self.declare_parameter("plan_topic", "/plan")
+        self.declare_parameter("cmd_vel_topic", "/planner_cmd_vel")
+        self.declare_parameter("ackermann_cmd_topic", "/ackermann_cmd")
+        self.declare_parameter("planner_status_topic", "/hybrid_astar_status")
+        self.declare_parameter("tracker_status_topic", "/pure_pursuit_status")
+        self.declare_parameter("planner_node", "/grid_astar_theta_planner")
+        self.declare_parameter("tracker_node", "/topology_pure_pursuit")
+        self.declare_parameter("tf_timeout", 0.10)
+        self.declare_parameter("max_odom_age", 2.0)
+        self.declare_parameter("include_plan_points", True)
+
+        self.duration = max(1.0, float(self.get_parameter("duration").value))
+        self.sample_rate = max(1.0, float(self.get_parameter("sample_rate").value))
+        self.map_frame = str(self.get_parameter("map_frame").value)
+        self.odom_topic = str(self.get_parameter("odom_topic").value)
+        self.goal_pose_topic = str(self.get_parameter("goal_pose_topic").value)
+        self.plan_topic = str(self.get_parameter("plan_topic").value)
+        self.cmd_vel_topic = str(self.get_parameter("cmd_vel_topic").value)
+        self.ackermann_cmd_topic = str(self.get_parameter("ackermann_cmd_topic").value)
+        self.planner_status_topic = str(
+            self.get_parameter("planner_status_topic").value
+        )
+        self.tracker_status_topic = str(
+            self.get_parameter("tracker_status_topic").value
+        )
+        self.planner_node = str(self.get_parameter("planner_node").value)
+        self.tracker_node = str(self.get_parameter("tracker_node").value)
+        self.tf_timeout = float(self.get_parameter("tf_timeout").value)
+        self.max_odom_age = float(self.get_parameter("max_odom_age").value)
+        self.include_plan_points = bool(
+            self.get_parameter("include_plan_points").value
+        )
+
+        output_dir = Path(str(self.get_parameter("output_dir").value)).expanduser()
+        output_dir.mkdir(parents=True, exist_ok=True)
+        timestamp = time.strftime("%Y%m%d_%H%M%S")
+        test_name = str(self.get_parameter("test_name").value)
+        self.output_path = output_dir / f"{test_name}_{timestamp}.json"
+
+        self.started_wall = time.time()
+        self.started_mono = time.monotonic()
+        self.finished = False
+
+        self.latest_odom = None
+        self.latest_goal = None
+        self.latest_plan_points = []
+        self.latest_cmd = None
+        self.latest_ackermann = None
+        self.latest_planner_status = ""
+        self.latest_tracker_status = ""
+        self.last_plan_ok = None
+        self.remote_parameters = {}
+
+        self.samples = []
+        self.planner_status_events = []
+        self.tracker_status_events = []
+        self.plan_events = []
+        self.goal_events = []
+
+        self.min_distance_to_goal = None
+        self.min_distance_to_plan_end = None
+        self.max_abs_cmd_linear = 0.0
+        self.max_abs_cmd_angular = 0.0
+        self.max_abs_ackermann_speed = 0.0
+        self.max_abs_steering_angle = 0.0
+        self.tf_failures = 0
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        goal_qos = QoSProfile(
+            depth=10,
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.subscriptions_keepalive = [
+            self.create_subscription(Odometry, self.odom_topic, self._odom_cb, 50),
+            self.create_subscription(
+                PoseStamped, self.goal_pose_topic, self._goal_cb, goal_qos
+            ),
+            self.create_subscription(NavPath, self.plan_topic, self._plan_cb, 10),
+            self.create_subscription(Twist, self.cmd_vel_topic, self._cmd_cb, 50),
+            self.create_subscription(
+                String, self.planner_status_topic, self._planner_status_cb, 50
+            ),
+            self.create_subscription(
+                String, self.tracker_status_topic, self._tracker_status_cb, 50
+            ),
+        ]
+        if AckermannDriveStamped is not None:
+            self.subscriptions_keepalive.append(
+                self.create_subscription(
+                    AckermannDriveStamped,
+                    self.ackermann_cmd_topic,
+                    self._ackermann_cb,
+                    50,
+                )
+            )
+
+        self.sample_timer = self.create_timer(1.0 / self.sample_rate, self._sample)
+        self.done_timer = self.create_timer(0.20, self._maybe_finish)
+        self.get_logger().info(
+            f"collecting goal reach debug data for {self.duration:.1f}s -> {self.output_path}"
+        )
+
+    def elapsed(self):
+        return max(0.0, time.monotonic() - self.started_mono)
+
+    def _odom_cb(self, msg):
+        self.latest_odom = msg
+
+    def _goal_cb(self, msg):
+        goal = {
+            "elapsed_sec": self.elapsed(),
+            "frame_id": 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),
+        }
+        self.latest_goal = goal
+        self.goal_events.append(goal)
+
+    def _plan_cb(self, msg):
+        points = [
+            {
+                "x": pose.pose.position.x,
+                "y": pose.pose.position.y,
+                "yaw": yaw_from_quaternion(pose.pose.orientation),
+            }
+            for pose in msg.poses
+        ]
+        self.latest_plan_points = points
+        summary = self._plan_summary(points, msg.header.frame_id or self.map_frame)
+        summary["elapsed_sec"] = self.elapsed()
+        self.plan_events.append(summary)
+
+    def _cmd_cb(self, msg):
+        self.latest_cmd = {
+            "linear_x": msg.linear.x,
+            "angular_z": msg.angular.z,
+        }
+        self.max_abs_cmd_linear = max(self.max_abs_cmd_linear, abs(msg.linear.x))
+        self.max_abs_cmd_angular = max(self.max_abs_cmd_angular, abs(msg.angular.z))
+
+    def _ackermann_cb(self, msg):
+        self.latest_ackermann = {
+            "speed": msg.drive.speed,
+            "steering_angle": msg.drive.steering_angle,
+        }
+        self.max_abs_ackermann_speed = max(
+            self.max_abs_ackermann_speed, abs(msg.drive.speed)
+        )
+        self.max_abs_steering_angle = max(
+            self.max_abs_steering_angle, abs(msg.drive.steering_angle)
+        )
+
+    def _planner_status_cb(self, msg):
+        self.latest_planner_status = msg.data
+        event = {"elapsed_sec": self.elapsed(), "data": msg.data}
+        parsed = self._parse_plan_ok(msg.data)
+        if parsed is not None:
+            event["parsed_plan_ok"] = parsed
+            self.last_plan_ok = parsed
+        self.planner_status_events.append(event)
+
+    def _tracker_status_cb(self, msg):
+        self.latest_tracker_status = msg.data
+        self.tracker_status_events.append(
+            {"elapsed_sec": self.elapsed(), "data": msg.data}
+        )
+
+    def _parse_plan_ok(self, text):
+        match = PLAN_OK_RE.search(text)
+        if not match:
+            return None
+        parsed = {}
+        for key, value in match.groupdict().items():
+            parsed[key] = int(value) if key in {"points", "iterations"} else float(value)
+        return parsed
+
+    def _sample(self):
+        pose = self._current_pose_map()
+        pose_available = self._pose_available(pose)
+        goal = self.latest_goal
+        plan_end = self.latest_plan_points[-1] if self.latest_plan_points else None
+
+        distance_to_goal = None
+        yaw_error_to_goal = None
+        if pose_available and goal is not None:
+            distance_to_goal = math.hypot(goal["x"] - pose["x"], goal["y"] - pose["y"])
+            yaw_error_to_goal = abs(normalize_angle(goal["yaw"] - pose["yaw"]))
+            self.min_distance_to_goal = self._min_or_value(
+                self.min_distance_to_goal, distance_to_goal
+            )
+
+        distance_to_plan_end = None
+        yaw_error_to_plan_end = None
+        if pose_available and plan_end is not None:
+            distance_to_plan_end = math.hypot(
+                plan_end["x"] - pose["x"], plan_end["y"] - pose["y"]
+            )
+            yaw_error_to_plan_end = abs(normalize_angle(plan_end["yaw"] - pose["yaw"]))
+            self.min_distance_to_plan_end = self._min_or_value(
+                self.min_distance_to_plan_end, distance_to_plan_end
+            )
+
+        plan_end_to_goal = None
+        plan_end_yaw_to_goal = None
+        if plan_end is not None and goal is not None:
+            plan_end_to_goal = math.hypot(plan_end["x"] - goal["x"], plan_end["y"] - goal["y"])
+            plan_end_yaw_to_goal = abs(normalize_angle(plan_end["yaw"] - goal["yaw"]))
+
+        self.samples.append({
+            "elapsed_sec": self.elapsed(),
+            "pose_map": pose,
+            "goal": goal,
+            "plan_end": plan_end,
+            "distance_to_goal": finite_or_none(distance_to_goal),
+            "yaw_error_to_goal": finite_or_none(yaw_error_to_goal),
+            "distance_to_plan_end": finite_or_none(distance_to_plan_end),
+            "yaw_error_to_plan_end": finite_or_none(yaw_error_to_plan_end),
+            "plan_end_to_goal": finite_or_none(plan_end_to_goal),
+            "plan_end_yaw_to_goal": finite_or_none(plan_end_yaw_to_goal),
+            "cmd_vel": self.latest_cmd,
+            "ackermann_cmd": self.latest_ackermann,
+            "planner_status": self.latest_planner_status,
+            "tracker_status": self.latest_tracker_status,
+        })
+
+    def _min_or_value(self, old, new):
+        if new is None:
+            return old
+        if old is None:
+            return new
+        return min(old, new)
+
+    def _pose_available(self, pose):
+        return (
+            isinstance(pose, dict)
+            and pose.get("available") is True
+            and "x" in pose
+            and "y" in pose
+            and "yaw" in pose
+        )
+
+    def _current_pose_map(self):
+        if self.latest_odom is None:
+            return None
+
+        msg = self.latest_odom
+        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 {
+                "available": False,
+                "reason": "odom_too_old",
+                "age_sec": age,
+            }
+
+        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)
+        transformed = self._transform_pose_2d(x, y, yaw, source_frame)
+        if transformed is None:
+            return {
+                "available": False,
+                "reason": "tf_unavailable",
+                "source_frame": source_frame,
+            }
+        x, y, yaw = transformed
+        return {
+            "available": True,
+            "frame_id": self.map_frame,
+            "source_frame": source_frame,
+            "x": x,
+            "y": y,
+            "yaw": yaw,
+            "odom_age_sec": age,
+            "odom_linear_x": msg.twist.twist.linear.x,
+            "odom_angular_z": msg.twist.twist.angular.z,
+        }
+
+    def _transform_pose_2d(self, x, y, yaw, source_frame):
+        if source_frame == self.map_frame:
+            return x, y, yaw
+        try:
+            transform = self.tf_buffer.lookup_transform(
+                self.map_frame,
+                source_frame,
+                Time(),
+                timeout=Duration(seconds=self.tf_timeout),
+            )
+        except TransformException:
+            self.tf_failures += 1
+            return None
+
+        t = transform.transform.translation
+        transform_yaw = yaw_from_quaternion(transform.transform.rotation)
+        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 _plan_summary(self, points, frame_id):
+        if not points:
+            return {
+                "frame_id": frame_id,
+                "points": 0,
+                "length": 0.0,
+                "start": None,
+                "end": None,
+            }
+        length = 0.0
+        reverse_segments = 0
+        for a, b in zip(points, points[1:]):
+            length += math.hypot(b["x"] - a["x"], b["y"] - a["y"])
+            heading = math.atan2(b["y"] - a["y"], b["x"] - a["x"])
+            if math.cos(normalize_angle(a["yaw"] - heading)) < 0.0:
+                reverse_segments += 1
+        return {
+            "frame_id": frame_id,
+            "points": len(points),
+            "length": length,
+            "start": points[0],
+            "end": points[-1],
+            "reverse_segment_fraction": (
+                reverse_segments / max(1, len(points) - 1)
+            ),
+        }
+
+    def _maybe_finish(self):
+        if self.elapsed() >= self.duration:
+            self.finished = True
+
+    def collect_remote_parameters(self):
+        parameter_targets = {
+            "planner": (
+                self.planner_node,
+                [
+                    "goal_xy_tolerance",
+                    "goal_yaw_tolerance",
+                    "require_goal_yaw",
+                    "resolution",
+                    "yaw_bins",
+                    "primitive_length",
+                    "min_turning_radius",
+                    "heuristic_weight",
+                    "goal_yaw_heuristic_weight",
+                    "planning_timeout_ms",
+                    "max_iterations",
+                ],
+            ),
+            "tracker": (
+                self.tracker_node,
+                [
+                    "goal_tolerance",
+                    "goal_yaw_tolerance",
+                    "align_goal_yaw",
+                    "publish_cmd_vel",
+                    "cmd_vel_topic",
+                    "lookahead_distance",
+                    "linear_speed",
+                    "min_linear_speed",
+                    "reverse_speed",
+                    "slowdown_distance",
+                    "curvature_slowdown_gain",
+                    "min_turning_radius",
+                    "max_angular_speed",
+                ],
+            ),
+        }
+        for label, (node_name, names) in parameter_targets.items():
+            normalized_node_name = node_name if node_name.startswith("/") else f"/{node_name}"
+            service_name = f"{normalized_node_name}/get_parameters"
+            client = self.create_client(GetParameters, service_name)
+            if not client.wait_for_service(timeout_sec=1.0):
+                self.remote_parameters[label] = {
+                    "node": node_name,
+                    "service": service_name,
+                    "error": "parameter_service_unavailable",
+                }
+                continue
+            request = GetParameters.Request()
+            request.names = list(names)
+            future = client.call_async(request)
+            rclpy.spin_until_future_complete(self, future, timeout_sec=2.0)
+            if not future.done() or future.result() is None:
+                self.remote_parameters[label] = {
+                    "node": node_name,
+                    "service": service_name,
+                    "error": "parameter_query_timeout",
+                }
+                continue
+            response = future.result()
+            self.remote_parameters[label] = {
+                "node": node_name,
+                "service": service_name,
+                "values": {
+                    name: parameter_value_to_python_compatible(value)
+                    for name, value in zip(names, response.values)
+                },
+            }
+
+    def write_report(self):
+        report = {
+            "metadata": {
+                "started_wall_time": self.started_wall,
+                "duration_sec": self.elapsed(),
+                "output_path": str(self.output_path),
+            },
+            "topics": {
+                "odom": self.odom_topic,
+                "goal_pose": self.goal_pose_topic,
+                "plan": self.plan_topic,
+                "cmd_vel": self.cmd_vel_topic,
+                "ackermann_cmd": self.ackermann_cmd_topic,
+                "planner_status": self.planner_status_topic,
+                "tracker_status": self.tracker_status_topic,
+            },
+            "topic_endpoints": self._topic_endpoints_report(),
+            "remote_parameters": self.remote_parameters,
+            "latest": {
+                "goal": self.latest_goal,
+                "plan": self._plan_summary(self.latest_plan_points, self.map_frame),
+                "pose_map": self._current_pose_map(),
+                "cmd_vel": self.latest_cmd,
+                "ackermann_cmd": self.latest_ackermann,
+                "planner_status": self.latest_planner_status,
+                "tracker_status": self.latest_tracker_status,
+                "last_plan_ok": self.last_plan_ok,
+            },
+            "metrics": self._metrics(),
+            "diagnostic_hints": self._diagnostic_hints(),
+            "events": {
+                "goals": self.goal_events,
+                "plans": self.plan_events,
+                "planner_status": self.planner_status_events,
+                "tracker_status": self.tracker_status_events,
+            },
+            "samples": self.samples,
+        }
+        if self.include_plan_points:
+            report["latest"]["plan_points"] = self.latest_plan_points
+
+        self.output_path.write_text(
+            json.dumps(report, indent=2, ensure_ascii=False),
+            encoding="utf-8",
+        )
+        self.get_logger().info(f"wrote goal reach debug report: {self.output_path}")
+        for hint in report["diagnostic_hints"]:
+            self.get_logger().warn(hint)
+
+    def _topic_endpoints_report(self):
+        topics = [
+            self.odom_topic,
+            self.goal_pose_topic,
+            self.plan_topic,
+            self.cmd_vel_topic,
+            self.ackermann_cmd_topic,
+            self.planner_status_topic,
+            self.tracker_status_topic,
+        ]
+        report = {}
+        for topic in topics:
+            publishers = self.get_publishers_info_by_topic(topic)
+            subscribers = self.get_subscriptions_info_by_topic(topic)
+            report[topic] = {
+                "publisher_count": len(publishers),
+                "subscriber_count": len(subscribers),
+                "publishers": [endpoint_to_dict(endpoint) for endpoint in publishers],
+                "subscribers": [endpoint_to_dict(endpoint) for endpoint in subscribers],
+            }
+        return report
+
+    def _metrics(self):
+        final = self.samples[-1] if self.samples else {}
+        planner_texts = [event["data"] for event in self.planner_status_events]
+        tracker_texts = [event["data"] for event in self.tracker_status_events]
+        return {
+            "sample_count": len(self.samples),
+            "goal_count": len(self.goal_events),
+            "plan_count": len(self.plan_events),
+            "planner_status_count": len(self.planner_status_events),
+            "tracker_status_count": len(self.tracker_status_events),
+            "min_distance_to_goal": finite_or_none(self.min_distance_to_goal),
+            "min_distance_to_plan_end": finite_or_none(self.min_distance_to_plan_end),
+            "final_distance_to_goal": final.get("distance_to_goal"),
+            "final_distance_to_plan_end": final.get("distance_to_plan_end"),
+            "final_plan_end_to_goal": final.get("plan_end_to_goal"),
+            "final_plan_end_yaw_to_goal": final.get("plan_end_yaw_to_goal"),
+            "max_abs_cmd_linear": self.max_abs_cmd_linear,
+            "max_abs_cmd_angular": self.max_abs_cmd_angular,
+            "max_abs_ackermann_speed": self.max_abs_ackermann_speed,
+            "max_abs_steering_angle": self.max_abs_steering_angle,
+            "tf_failures": self.tf_failures,
+            "saw_plan_ok": any("plan_ok" in text for text in planner_texts),
+            "saw_plan_failed": any("plan_failed" in text for text in planner_texts),
+            "saw_goal_reached": any("goal_reached" in text for text in tracker_texts),
+            "saw_align_goal_yaw": any("aligning_goal_yaw" in text for text in tracker_texts),
+            "saw_no_recent_odom": any("no_recent_odom" in text for text in tracker_texts + planner_texts),
+        }
+
+    def _diagnostic_hints(self):
+        hints = []
+        params = self.remote_parameters
+        planner = params.get("planner", {}).get("values", {})
+        tracker = params.get("tracker", {}).get("values", {})
+        metrics = self._metrics()
+
+        if not self.latest_goal:
+            hints.append("没有采集到 /goal_pose；请先运行采集脚本，再发送目标。")
+        if not self.latest_plan_points:
+            hints.append("没有采集到 /plan；planner 可能未规划成功或采集脚本启动太晚。")
+        if self.latest_odom is None:
+            hints.append(f"没有采集到 {self.odom_topic}；tracker 无法闭环到点。")
+        if metrics["tf_failures"] > 0:
+            hints.append(
+                f"TF 查询失败 {metrics['tf_failures']} 次；需要检查 map->odom 和 odom frame。"
+            )
+
+        planner_xy = planner.get("goal_xy_tolerance")
+        tracker_xy = tracker.get("goal_tolerance")
+        if isinstance(planner_xy, (int, float)) and isinstance(tracker_xy, (int, float)):
+            if planner_xy > tracker_xy:
+                hints.append(
+                    "planner 的 goal_xy_tolerance 大于 tracker 的 goal_tolerance；"
+                    "planner 可能发布一个离原始目标较远的路径终点。"
+                )
+
+        planner_yaw = planner.get("goal_yaw_tolerance")
+        tracker_yaw = tracker.get("goal_yaw_tolerance")
+        planner_requires_yaw = bool(planner.get("require_goal_yaw"))
+        tracker_aligns_yaw = bool(tracker.get("align_goal_yaw"))
+        if (
+            planner_requires_yaw
+            and tracker_aligns_yaw
+            and isinstance(planner_yaw, (int, float))
+            and isinstance(tracker_yaw, (int, float))
+        ):
+            if planner_yaw > tracker_yaw:
+                hints.append(
+                    "planner 的 goal_yaw_tolerance 大于 tracker 的 goal_yaw_tolerance；"
+                    "planner 认为合格的终点 yaw，tracker 可能继续调整。"
+                )
+
+        min_speed = tracker.get("min_linear_speed")
+        if isinstance(min_speed, (int, float)) and isinstance(tracker_xy, (int, float)):
+            if min_speed > tracker_xy:
+                hints.append(
+                    "tracker 的 min_linear_speed 数值大于 goal_tolerance；"
+                    "接近终点时可能仍以较高速度越过目标。"
+                )
+
+        plan_goal_error = metrics.get("final_plan_end_to_goal")
+        if isinstance(plan_goal_error, (int, float)) and isinstance(tracker_xy, (int, float)):
+            if plan_goal_error > tracker_xy:
+                hints.append(
+                    "最新 /plan 的终点到 /goal_pose 的距离大于 tracker 到点半径；"
+                    "tracker 跟随 /plan 时不会到达你原始输入的目标点。"
+                )
+
+        if metrics["saw_align_goal_yaw"] and not metrics["saw_goal_reached"]:
+            hints.append(
+                "采集期间出现 aligning_goal_yaw 但没有 goal_reached；"
+                "终点 yaw 对齐可能把车辆带离目标圆。"
+            )
+
+        if (
+            not metrics["saw_goal_reached"]
+            and metrics["max_abs_cmd_linear"] <= 1e-6
+            and metrics["max_abs_cmd_angular"] <= 1e-6
+        ):
+            hints.append(
+                "采集期间没有观察到速度命令；如果正在做实车到点测试，"
+                "请确认 launch 使用 enable_motion:=true，且采集的是 tracker 实际输出话题。"
+            )
+
+        ack_info = self._topic_endpoints_report().get(self.ackermann_cmd_topic, {})
+        if ack_info.get("publisher_count", 0) == 0:
+            hints.append(
+                f"{self.ackermann_cmd_topic} 没有发布者；阿克曼桥可能未启动，"
+                "通常是 enable_motion:=false。"
+            )
+
+        if not hints:
+            hints.append("未发现明显配置矛盾；请把 JSON 报告发给我继续分析。")
+        return hints
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = GoalReachDebugCollector()
+    try:
+        node.collect_remote_parameters()
+        while rclpy.ok() and not node.finished:
+            rclpy.spin_once(node, timeout_sec=0.10)
+        node.write_report()
+    except KeyboardInterrupt:
+        node.get_logger().warn("interrupted; writing partial report")
+        node.write_report()
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/planner/scripts/lidar_health_check.py

@@ -0,0 +1,98 @@
+#!/usr/bin/env python3
+"""Check whether LaserScan remains usable despite driver console warnings."""
+
+import math
+import sys
+import time
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import LaserScan
+
+
+class LidarHealthCheck(Node):
+    def __init__(self):
+        super().__init__("planner_lidar_health_check")
+        self.declare_parameter("scan_topic", "/scan")
+        self.declare_parameter("duration_sec", 10.0)
+        self.declare_parameter("min_rate_hz", 5.0)
+        self.declare_parameter("min_valid_fraction", 0.02)
+        self.scan_topic = str(self.get_parameter("scan_topic").value)
+        self.duration_sec = max(2.0, float(self.get_parameter("duration_sec").value))
+        self.min_rate_hz = max(0.1, float(self.get_parameter("min_rate_hz").value))
+        self.min_valid_fraction = max(
+            0.0, min(1.0, float(self.get_parameter("min_valid_fraction").value))
+        )
+        self.start_monotonic = time.monotonic()
+        self.receipt_times = []
+        self.valid_fractions = []
+        self.frame_ids = set()
+        self.subscription = self.create_subscription(
+            LaserScan, self.scan_topic, self._scan_cb, 20
+        )
+
+    def _scan_cb(self, msg):
+        self.receipt_times.append(time.monotonic())
+        self.frame_ids.add(msg.header.frame_id)
+        if msg.ranges:
+            valid = sum(
+                1
+                for value in msg.ranges
+                if math.isfinite(value) and msg.range_min <= value <= msg.range_max
+            )
+            self.valid_fractions.append(valid / len(msg.ranges))
+
+    def complete(self):
+        return time.monotonic() - self.start_monotonic >= self.duration_sec
+
+    def evaluate(self):
+        elapsed = time.monotonic() - self.start_monotonic
+        if len(self.receipt_times) >= 2:
+            span = self.receipt_times[-1] - self.receipt_times[0]
+            rate = (len(self.receipt_times) - 1) / span if span > 0.0 else 0.0
+        else:
+            rate = 0.0
+        valid_mean = (
+            sum(self.valid_fractions) / len(self.valid_fractions)
+            if self.valid_fractions
+            else 0.0
+        )
+        errors = []
+        if not self.receipt_times:
+            errors.append(f"no {self.scan_topic} received in {elapsed:.1f} s")
+        if rate < self.min_rate_hz:
+            errors.append(f"scan rate {rate:.2f} Hz is below {self.min_rate_hz:.2f} Hz")
+        if valid_mean < self.min_valid_fraction:
+            errors.append(
+                f"valid range fraction {valid_mean:.3f} is below "
+                f"{self.min_valid_fraction:.3f}"
+            )
+        if not self.frame_ids or "" in self.frame_ids:
+            errors.append("LaserScan frame_id is empty")
+
+        self.get_logger().info(
+            f"scans={len(self.receipt_times)} rate={rate:.2f} Hz "
+            f"valid_fraction={valid_mean:.3f} frames={sorted(self.frame_ids)}"
+        )
+        for error in errors:
+            self.get_logger().error(error)
+        if not errors:
+            self.get_logger().info("PASS: lidar data is usable by AMCL")
+        return not errors
+
+
+def main():
+    rclpy.init()
+    node = LidarHealthCheck()
+    try:
+        while rclpy.ok() and not node.complete():
+            rclpy.spin_once(node, timeout_sec=0.1)
+        success = node.evaluate()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+    sys.exit(0 if success else 1)
+
+
+if __name__ == "__main__":
+    main()

src/planner/scripts/map_info.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python3
+"""Inspect a ROS map YAML/PGM pair and validate its metric dimensions."""
+
+import argparse
+from pathlib import Path
+import sys
+
+import yaml
+
+
+def _pgm_token(stream):
+    while True:
+        byte = stream.read(1)
+        if not byte:
+            raise ValueError("unexpected end of PGM header")
+        if byte.isspace():
+            continue
+        if byte == b"#":
+            stream.readline()
+            continue
+        token = byte
+        while True:
+            byte = stream.read(1)
+            if not byte or byte.isspace():
+                return token.decode("ascii")
+            token += byte
+
+
+def read_pgm_header(path):
+    with path.open("rb") as stream:
+        magic = _pgm_token(stream)
+        if magic not in ("P2", "P5"):
+            raise ValueError(f"unsupported image format {magic}, expected P2/P5")
+        width = int(_pgm_token(stream))
+        height = int(_pgm_token(stream))
+        max_value = int(_pgm_token(stream))
+    return magic, width, height, max_value
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("map_yaml", type=Path)
+    parser.add_argument("--expected-width", type=float, default=5.0)
+    parser.add_argument("--expected-height", type=float, default=5.0)
+    parser.add_argument("--tolerance", type=float, default=0.15)
+    parser.add_argument("--no-check", action="store_true")
+    args = parser.parse_args()
+
+    data = yaml.safe_load(args.map_yaml.read_text(encoding="utf-8")) or {}
+    image = Path(data["image"])
+    if not image.is_absolute():
+        image = args.map_yaml.parent / image
+    magic, pixels_x, pixels_y, max_value = read_pgm_header(image)
+    resolution = float(data["resolution"])
+    metric_x = pixels_x * resolution
+    metric_y = pixels_y * resolution
+    origin = data.get("origin", [0.0, 0.0, 0.0])
+
+    print(f"yaml:       {args.map_yaml}")
+    print(f"image:      {image}")
+    print(f"format:     {magic}, max_value={max_value}")
+    print(f"pixels:     {pixels_x} x {pixels_y}")
+    print(f"resolution: {resolution:.6f} m/pixel")
+    print(f"size:       {metric_x:.3f} x {metric_y:.3f} m")
+    print(f"origin:     {origin}")
+
+    if args.no_check:
+        return 0
+    if (
+        abs(metric_x - args.expected_width) > args.tolerance
+        or abs(metric_y - args.expected_height) > args.tolerance
+    ):
+        print(
+            "FAIL: expected %.3f x %.3f m +/- %.3f m"
+            % (args.expected_width, args.expected_height, args.tolerance)
+        )
+        return 1
+    print("PASS: map dimensions match the expected metric size")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/planner/scripts/navigation_test_recorder.py

@@ -0,0 +1,460 @@
+#!/usr/bin/env python3
+"""Record and summarize real-robot planner and tracker performance."""
+
+import csv
+import json
+import math
+from pathlib import Path
+import statistics
+import time
+
+import rclpy
+from ackermann_msgs.msg import AckermannDriveStamped
+from geometry_msgs.msg import PoseStamped, Twist
+from nav_msgs.msg import OccupancyGrid, Odometry, Path as NavPath
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.time import Time
+from std_msgs.msg import Bool, 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 percentile(values, fraction):
+    if not values:
+        return None
+    ordered = sorted(values)
+    index = min(len(ordered) - 1, max(0, math.ceil(fraction * len(ordered)) - 1))
+    return ordered[index]
+
+
+def point_segment_distance(px, py, ax, ay, bx, by):
+    dx = bx - ax
+    dy = by - ay
+    length_sq = dx * dx + dy * dy
+    if length_sq <= 1e-12:
+        return math.hypot(px - ax, py - ay)
+    ratio = ((px - ax) * dx + (py - ay) * dy) / length_sq
+    ratio = max(0.0, min(1.0, ratio))
+    cx = ax + ratio * dx
+    cy = ay + ratio * dy
+    return math.hypot(px - cx, py - cy)
+
+
+class NavigationTestRecorder(Node):
+    def __init__(self):
+        super().__init__("navigation_test_recorder")
+        self.declare_parameter("test_name", "navigation_test")
+        self.declare_parameter("output_dir", "~/.ros/navigation_tests")
+        self.declare_parameter("map_frame", "map")
+        self.declare_parameter("map_topic", "/map")
+        self.declare_parameter("odom_topic", "/odom_combined")
+        self.declare_parameter("goal_pose_topic", "/goal_pose")
+        self.declare_parameter("plan_topic", "/plan")
+        self.declare_parameter("cmd_vel_topic", "/cmd_vel")
+        self.declare_parameter("drive_command_topic", "/ackermann_cmd")
+        self.declare_parameter("cancel_topic", "/navigation_cancel")
+        self.declare_parameter("planner_status_topic", "/hybrid_astar_status")
+        self.declare_parameter("tracker_status_topic", "/pure_pursuit_status")
+        self.declare_parameter("sample_rate", 20.0)
+        self.declare_parameter("tf_timeout", 0.10)
+        self.declare_parameter("occupied_threshold", 50)
+
+        self.test_name = str(self.get_parameter("test_name").value)
+        output_dir = Path(str(self.get_parameter("output_dir").value)).expanduser()
+        output_dir.mkdir(parents=True, exist_ok=True)
+        timestamp = time.strftime("%Y%m%d_%H%M%S")
+        self.output_base = output_dir / f"{self.test_name}_{timestamp}"
+        self.csv_path = self.output_base.with_suffix(".csv")
+        self.summary_path = self.output_base.with_suffix(".json")
+        self.map_frame = str(self.get_parameter("map_frame").value)
+        self.map_topic = str(self.get_parameter("map_topic").value)
+        self.odom_topic = str(self.get_parameter("odom_topic").value)
+        self.goal_pose_topic = str(self.get_parameter("goal_pose_topic").value)
+        self.plan_topic = str(self.get_parameter("plan_topic").value)
+        self.cmd_vel_topic = str(self.get_parameter("cmd_vel_topic").value)
+        self.drive_command_topic = str(
+            self.get_parameter("drive_command_topic").value
+        )
+        self.cancel_topic = str(self.get_parameter("cancel_topic").value)
+        self.planner_status_topic = str(
+            self.get_parameter("planner_status_topic").value
+        )
+        self.tracker_status_topic = str(
+            self.get_parameter("tracker_status_topic").value
+        )
+        self.tf_timeout = float(self.get_parameter("tf_timeout").value)
+        self.occupied_threshold = int(self.get_parameter("occupied_threshold").value)
+
+        self.started_at = self.get_clock().now()
+        self.latest_odom = None
+        self.latest_cmd = Twist()
+        self.latest_drive = AckermannDriveStamped()
+        self.latest_path = []
+        self.latest_goal = None
+        self.map_msg = None
+        self.planner_status = ""
+        self.tracker_status = ""
+        self.pending_goal_time = None
+        self.pending_goal_xy = None
+        self.plan_latencies = []
+        self.cancel_stop_latencies = []
+        self.cancel_pending_time = None
+        self.plan_count = 0
+        self.plan_ok_count = 0
+        self.plan_fail_count = 0
+        self.plan_collision_cells = []
+        self.plan_min_turning_radii = []
+        self.plan_lengths = []
+        self.reverse_fractions = []
+        self.cross_track_errors = []
+        self.goal_errors = []
+        self.samples = 0
+        self.transform_failures = 0
+        self.max_abs_cmd_linear = 0.0
+        self.max_abs_cmd_angular = 0.0
+        self.max_abs_drive_speed = 0.0
+        self.max_abs_steering_angle = 0.0
+        self.max_abs_measured_linear = 0.0
+        self.max_abs_measured_angular = 0.0
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.csv_file = self.csv_path.open("w", newline="", encoding="utf-8")
+        self.csv_writer = csv.writer(self.csv_file)
+        self.csv_writer.writerow([
+            "elapsed_sec", "map_x", "map_y", "map_yaw",
+            "measured_linear", "measured_angular",
+            "cmd_linear", "cmd_angular", "drive_speed", "steering_angle",
+            "cross_track_error",
+            "goal_error", "plan_points", "planner_status", "tracker_status",
+        ])
+
+        map_qos = QoSProfile(
+            depth=1,
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.input_subscriptions = [
+            self.create_subscription(
+                OccupancyGrid, self.map_topic, self._map_cb, map_qos
+            ),
+            self.create_subscription(Odometry, self.odom_topic, self._odom_cb, 20),
+            self.create_subscription(
+                PoseStamped, self.goal_pose_topic, self._goal_cb, map_qos
+            ),
+            self.create_subscription(NavPath, self.plan_topic, self._plan_cb, 10),
+            self.create_subscription(Twist, self.cmd_vel_topic, self._cmd_cb, 20),
+            self.create_subscription(
+                AckermannDriveStamped, self.drive_command_topic, self._drive_cb, 20
+            ),
+            self.create_subscription(Bool, self.cancel_topic, self._cancel_cb, 10),
+            self.create_subscription(
+                String, self.planner_status_topic, self._planner_status_cb, 20
+            ),
+            self.create_subscription(
+                String, self.tracker_status_topic, self._tracker_status_cb, 20
+            ),
+        ]
+        sample_rate = max(1.0, float(self.get_parameter("sample_rate").value))
+        self.timer = self.create_timer(1.0 / sample_rate, self._sample)
+        self.get_logger().info(f"recording navigation test to {self.csv_path}")
+
+    def _map_cb(self, msg):
+        self.map_msg = msg
+
+    def _odom_cb(self, msg):
+        self.latest_odom = msg
+
+    def _cmd_cb(self, msg):
+        self.latest_cmd = msg
+        self.max_abs_cmd_linear = max(self.max_abs_cmd_linear, abs(msg.linear.x))
+        self.max_abs_cmd_angular = max(self.max_abs_cmd_angular, abs(msg.angular.z))
+        if (
+            self.cancel_pending_time is not None
+            and abs(msg.linear.x) <= 0.01
+            and abs(msg.angular.z) <= 0.01
+        ):
+            latency = (self.get_clock().now() - self.cancel_pending_time).nanoseconds * 1e-9
+            self.cancel_stop_latencies.append(max(0.0, latency))
+            self.cancel_pending_time = None
+
+    def _cancel_cb(self, msg):
+        if not msg.data:
+            return
+        if abs(self.latest_cmd.linear.x) <= 0.01 and abs(self.latest_cmd.angular.z) <= 0.01:
+            self.cancel_stop_latencies.append(0.0)
+        else:
+            self.cancel_pending_time = self.get_clock().now()
+
+    def _drive_cb(self, msg):
+        self.latest_drive = msg
+        self.max_abs_drive_speed = max(
+            self.max_abs_drive_speed, abs(msg.drive.speed)
+        )
+        self.max_abs_steering_angle = max(
+            self.max_abs_steering_angle, abs(msg.drive.steering_angle)
+        )
+
+    def _goal_cb(self, msg):
+        goal_xy = (msg.pose.position.x, msg.pose.position.y)
+        if self.pending_goal_xy is None or math.hypot(
+            goal_xy[0] - self.pending_goal_xy[0],
+            goal_xy[1] - self.pending_goal_xy[1],
+        ) > 1e-4:
+            self.pending_goal_time = self.get_clock().now()
+            self.pending_goal_xy = goal_xy
+        self.latest_goal = goal_xy
+
+    def _planner_status_cb(self, msg):
+        self.planner_status = msg.data
+        if msg.data.startswith("plan_ok"):
+            self.plan_ok_count += 1
+        elif msg.data.startswith("plan_failed") or msg.data.startswith("plan_rejected"):
+            self.plan_fail_count += 1
+
+    def _tracker_status_cb(self, msg):
+        self.tracker_status = msg.data
+
+    def _plan_cb(self, msg):
+        if len(msg.poses) < 2:
+            self.latest_path = []
+            return
+        self.latest_path = [
+            (
+                pose.pose.position.x,
+                pose.pose.position.y,
+                yaw_from_quaternion(pose.pose.orientation),
+            )
+            for pose in msg.poses
+        ]
+        self.plan_count += 1
+        if self.pending_goal_time is not None:
+            latency = (self.get_clock().now() - self.pending_goal_time).nanoseconds * 1e-9
+            self.plan_latencies.append(max(0.0, latency))
+            self.pending_goal_time = None
+        self.plan_lengths.append(self._path_length(self.latest_path))
+        self.plan_min_turning_radii.append(self._minimum_turning_radius(self.latest_path))
+        self.reverse_fractions.append(self._reverse_fraction(self.latest_path))
+        self.plan_collision_cells.append(self._occupied_path_points(self.latest_path))
+
+    def _current_map_pose(self):
+        if self.latest_odom is None:
+            return None
+        msg = self.latest_odom
+        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 x, y, yaw
+        try:
+            transform = self.tf_buffer.lookup_transform(
+                self.map_frame,
+                source_frame,
+                Time(),
+                timeout=Duration(seconds=self.tf_timeout),
+            )
+        except TransformException:
+            self.transform_failures += 1
+            return None
+        t = transform.transform.translation
+        transform_yaw = yaw_from_quaternion(transform.transform.rotation)
+        cos_yaw = math.cos(transform_yaw)
+        sin_yaw = math.sin(transform_yaw)
+        return (
+            t.x + cos_yaw * x - sin_yaw * y,
+            t.y + sin_yaw * x + cos_yaw * y,
+            normalize_angle(transform_yaw + yaw),
+        )
+
+    def _sample(self):
+        pose = self._current_map_pose()
+        if pose is None or self.latest_odom is None:
+            return
+        x, y, yaw = pose
+        measured_linear = self.latest_odom.twist.twist.linear.x
+        measured_angular = self.latest_odom.twist.twist.angular.z
+        self.max_abs_measured_linear = max(
+            self.max_abs_measured_linear, abs(measured_linear)
+        )
+        self.max_abs_measured_angular = max(
+            self.max_abs_measured_angular, abs(measured_angular)
+        )
+        cross_track = self._cross_track_error(x, y)
+        goal_error = None
+        if self.latest_goal is not None:
+            goal_error = math.hypot(self.latest_goal[0] - x, self.latest_goal[1] - y)
+            self.goal_errors.append(goal_error)
+        if cross_track is not None:
+            self.cross_track_errors.append(cross_track)
+        elapsed = (self.get_clock().now() - self.started_at).nanoseconds * 1e-9
+        self.csv_writer.writerow([
+            f"{elapsed:.6f}", f"{x:.6f}", f"{y:.6f}", f"{yaw:.6f}",
+            f"{measured_linear:.6f}", f"{measured_angular:.6f}",
+            f"{self.latest_cmd.linear.x:.6f}", f"{self.latest_cmd.angular.z:.6f}",
+            f"{self.latest_drive.drive.speed:.6f}",
+            f"{self.latest_drive.drive.steering_angle:.6f}",
+            "" if cross_track is None else f"{cross_track:.6f}",
+            "" if goal_error is None else f"{goal_error:.6f}",
+            len(self.latest_path), self.planner_status, self.tracker_status,
+        ])
+        self.samples += 1
+        if self.samples % 20 == 0:
+            self.csv_file.flush()
+
+    def _cross_track_error(self, x, y):
+        if len(self.latest_path) < 2:
+            return None
+        return min(
+            point_segment_distance(x, y, a[0], a[1], b[0], b[1])
+            for a, b in zip(self.latest_path, self.latest_path[1:])
+        )
+
+    @staticmethod
+    def _path_length(path):
+        return sum(
+            math.hypot(b[0] - a[0], b[1] - a[1])
+            for a, b in zip(path, path[1:])
+        )
+
+    @staticmethod
+    def _minimum_turning_radius(path):
+        max_curvature = 0.0
+        for a, b in zip(path, path[1:]):
+            distance = math.hypot(b[0] - a[0], b[1] - a[1])
+            if distance <= 1e-6:
+                continue
+            curvature = abs(normalize_angle(b[2] - a[2])) / distance
+            max_curvature = max(max_curvature, curvature)
+        return None if max_curvature <= 1e-9 else 1.0 / max_curvature
+
+    @staticmethod
+    def _reverse_fraction(path):
+        reverse_length = 0.0
+        total_length = 0.0
+        for a, b in zip(path, path[1:]):
+            dx = b[0] - a[0]
+            dy = b[1] - a[1]
+            distance = math.hypot(dx, dy)
+            if distance <= 1e-6:
+                continue
+            segment_yaw = math.atan2(dy, dx)
+            if math.cos(normalize_angle(a[2] - segment_yaw)) < 0.0:
+                reverse_length += distance
+            total_length += distance
+        return 0.0 if total_length <= 1e-9 else reverse_length / total_length
+
+    def _occupied_path_points(self, path):
+        msg = self.map_msg
+        if msg is None or msg.info.resolution <= 0.0:
+            return None
+        origin_yaw = yaw_from_quaternion(msg.info.origin.orientation)
+        cos_yaw = math.cos(origin_yaw)
+        sin_yaw = math.sin(origin_yaw)
+        collisions = 0
+        for x, y, _ in path:
+            dx = x - msg.info.origin.position.x
+            dy = y - msg.info.origin.position.y
+            mx = cos_yaw * dx + sin_yaw * dy
+            my = -sin_yaw * dx + cos_yaw * dy
+            gx = math.floor(mx / msg.info.resolution)
+            gy = math.floor(my / msg.info.resolution)
+            if gx < 0 or gy < 0 or gx >= msg.info.width or gy >= msg.info.height:
+                collisions += 1
+                continue
+            value = msg.data[gy * msg.info.width + gx]
+            if value < 0 or value >= self.occupied_threshold:
+                collisions += 1
+        return collisions
+
+    @staticmethod
+    def _stats(values):
+        if not values:
+            return {"count": 0, "mean": None, "rms": None, "p95": None, "max": None}
+        return {
+            "count": len(values),
+            "mean": statistics.fmean(values),
+            "rms": math.sqrt(statistics.fmean(value * value for value in values)),
+            "p95": percentile(values, 0.95),
+            "max": max(values),
+        }
+
+    def write_summary(self):
+        duration = (self.get_clock().now() - self.started_at).nanoseconds * 1e-9
+        valid_radii = [value for value in self.plan_min_turning_radii if value is not None]
+        valid_collisions = [value for value in self.plan_collision_cells if value is not None]
+        summary = {
+            "test_name": self.test_name,
+            "duration_sec": duration,
+            "samples": self.samples,
+            "plan_count": self.plan_count,
+            "plan_ok_status_count": self.plan_ok_count,
+            "plan_fail_status_count": self.plan_fail_count,
+            "planning_latency_sec": self._stats(self.plan_latencies),
+            "cancel_to_zero_command_sec": self._stats(self.cancel_stop_latencies),
+            "path_length_m": self._stats(self.plan_lengths),
+            "path_occupied_points_max": max(valid_collisions) if valid_collisions else None,
+            "minimum_planned_turning_radius_m": min(valid_radii) if valid_radii else None,
+            "reverse_fraction": self._stats(self.reverse_fractions),
+            "cross_track_error_m": self._stats(self.cross_track_errors),
+            "goal_error_m": self._stats(self.goal_errors),
+            "final_goal_error_m": self.goal_errors[-1] if self.goal_errors else None,
+            "minimum_goal_error_m": min(self.goal_errors) if self.goal_errors else None,
+            "transform_failures": self.transform_failures,
+            "max_abs_cmd_linear": self.max_abs_cmd_linear,
+            "max_abs_cmd_angular": self.max_abs_cmd_angular,
+            "max_abs_drive_speed": self.max_abs_drive_speed,
+            "max_abs_steering_angle": self.max_abs_steering_angle,
+            "max_abs_measured_linear": self.max_abs_measured_linear,
+            "max_abs_measured_angular": self.max_abs_measured_angular,
+            "last_planner_status": self.planner_status,
+            "last_tracker_status": self.tracker_status,
+            "csv": str(self.csv_path),
+        }
+        self.csv_file.flush()
+        self.summary_path.write_text(
+            json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8"
+        )
+        self.get_logger().info(f"wrote navigation summary to {self.summary_path}")
+
+    def close(self):
+        if not self.csv_file.closed:
+            self.write_summary()
+            self.csv_file.close()
+
+
+def main():
+    rclpy.init()
+    node = NavigationTestRecorder()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.close()
+        try:
+            if rclpy.ok():
+                node.destroy_node()
+        except Exception:
+            pass
+        try:
+            if rclpy.ok():
+                rclpy.shutdown()
+        except Exception:
+            pass
+
+
+if __name__ == "__main__":
+    main()

src/planner/scripts/odom_map_tf.py

@@ -0,0 +1,249 @@
+#!/usr/bin/env python3
+"""Align map to wheel odom using a user-provided initial pose.
+
+This node replaces AMCL for controlled short-range tests. It publishes a fixed
+map->odom transform computed from:
+
+  known current map pose * inverse(current odom pose)
+
+After initialization, pose updates come only from /odom.
+"""
+
+import math
+
+import rclpy
+from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from std_msgs.msg import String
+from tf2_ros import TransformBroadcaster
+
+
+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 fill_yaw_quaternion(q, yaw):
+    q.x = 0.0
+    q.y = 0.0
+    q.z = math.sin(0.5 * yaw)
+    q.w = math.cos(0.5 * yaw)
+
+
+class OdomMapTf(Node):
+    def __init__(self):
+        super().__init__("planner_odom_map_tf")
+        self.declare_parameter("map_frame", "map")
+        self.declare_parameter("odom_frame", "odom")
+        self.declare_parameter("odom_topic", "/odom")
+        self.declare_parameter("initial_pose_topic", "/initialpose")
+        self.declare_parameter("status_topic", "/odom_localization_status")
+        self.declare_parameter("initial_x", 0.0)
+        self.declare_parameter("initial_y", 0.0)
+        self.declare_parameter("initial_yaw", 0.0)
+        self.declare_parameter("initialize_on_first_odom", True)
+        self.declare_parameter("publish_rate", 30.0)
+        self.declare_parameter("state_log_period", 1.0)
+
+        self.map_frame = str(self.get_parameter("map_frame").value)
+        self.default_odom_frame = str(self.get_parameter("odom_frame").value)
+        self.active_odom_frame = self.default_odom_frame
+        self.initialize_on_first_odom = bool(
+            self.get_parameter("initialize_on_first_odom").value
+        )
+        self.initial_pose = (
+            float(self.get_parameter("initial_x").value),
+            float(self.get_parameter("initial_y").value),
+            float(self.get_parameter("initial_yaw").value),
+        )
+        self.state_log_period = max(
+            0.0, float(self.get_parameter("state_log_period").value)
+        )
+
+        self.latest_odom_pose = None
+        self.pending_initial_pose = None
+        self.have_transform = False
+        self.last_state_log_ns = 0
+        self.map_to_odom_x = 0.0
+        self.map_to_odom_y = 0.0
+        self.map_to_odom_yaw = 0.0
+
+        self.tf_broadcaster = TransformBroadcaster(self)
+        self.status_pub = self.create_publisher(
+            String, str(self.get_parameter("status_topic").value), 10
+        )
+        self.odom_sub = self.create_subscription(
+            Odometry,
+            str(self.get_parameter("odom_topic").value),
+            self._odom_callback,
+            20,
+        )
+        self.initial_pose_sub = self.create_subscription(
+            PoseWithCovarianceStamped,
+            str(self.get_parameter("initial_pose_topic").value),
+            self._initial_pose_callback,
+            10,
+        )
+
+        publish_rate = max(1.0, float(self.get_parameter("publish_rate").value))
+        self.timer = self.create_timer(1.0 / publish_rate, self._publish_transform)
+        self._publish_status(
+            "waiting_for_odom initial_pose=(%.3f, %.3f, %.3f)"
+            % self.initial_pose
+        )
+
+    def _odom_callback(self, msg):
+        source_frame = msg.header.frame_id or self.default_odom_frame
+        self.active_odom_frame = source_frame
+        self.latest_odom_pose = (
+            msg.pose.pose.position.x,
+            msg.pose.pose.position.y,
+            yaw_from_quaternion(msg.pose.pose.orientation),
+        )
+
+        if self.pending_initial_pose is not None:
+            pending = self.pending_initial_pose
+            self.pending_initial_pose = None
+            self._initialize_from_pose(*pending, reason="initialpose")
+        elif not self.have_transform and self.initialize_on_first_odom:
+            self._initialize_from_pose(*self.initial_pose, reason="launch_initial")
+
+    def _initial_pose_callback(self, msg):
+        frame_id = msg.header.frame_id or self.map_frame
+        if frame_id != self.map_frame:
+            self._publish_status(
+                "initialpose_rejected frame=%s expected=%s"
+                % (frame_id, self.map_frame)
+            )
+            return
+
+        pose = msg.pose.pose
+        initial_pose = (
+            pose.position.x,
+            pose.position.y,
+            yaw_from_quaternion(pose.orientation),
+        )
+        if self.latest_odom_pose is None:
+            self.pending_initial_pose = initial_pose
+            self._publish_status("initialpose_queued waiting_for_odom")
+            return
+        self._initialize_from_pose(*initial_pose, reason="initialpose")
+
+    def _initialize_from_pose(self, map_x, map_y, map_yaw, reason):
+        if self.latest_odom_pose is None:
+            self.pending_initial_pose = (map_x, map_y, map_yaw)
+            return
+
+        odom_x, odom_y, odom_yaw = self.latest_odom_pose
+        delta_yaw = normalize_angle(map_yaw - odom_yaw)
+        cos_yaw = math.cos(delta_yaw)
+        sin_yaw = math.sin(delta_yaw)
+        self.map_to_odom_x = map_x - (cos_yaw * odom_x - sin_yaw * odom_y)
+        self.map_to_odom_y = map_y - (sin_yaw * odom_x + cos_yaw * odom_y)
+        self.map_to_odom_yaw = delta_yaw
+        self.have_transform = True
+        self._publish_status(
+            "%s_aligned %s->%s x=%.3f y=%.3f yaw=%.3f"
+            % (
+                reason,
+                self.map_frame,
+                self.active_odom_frame,
+                self.map_to_odom_x,
+                self.map_to_odom_y,
+                self.map_to_odom_yaw,
+            )
+        )
+
+    def _publish_transform(self):
+        if not self.have_transform:
+            self._maybe_log_state()
+            return
+        msg = TransformStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self.map_frame
+        msg.child_frame_id = self.active_odom_frame
+        msg.transform.translation.x = self.map_to_odom_x
+        msg.transform.translation.y = self.map_to_odom_y
+        msg.transform.translation.z = 0.0
+        fill_yaw_quaternion(msg.transform.rotation, self.map_to_odom_yaw)
+        self.tf_broadcaster.sendTransform(msg)
+        self._maybe_log_state()
+
+    def _maybe_log_state(self):
+        if self.state_log_period <= 0.0:
+            return
+        now_ns = self.get_clock().now().nanoseconds
+        min_period_ns = int(self.state_log_period * 1_000_000_000)
+        if now_ns - self.last_state_log_ns < min_period_ns:
+            return
+        self.last_state_log_ns = now_ns
+
+        if self.latest_odom_pose is None:
+            self.get_logger().info(
+                "odom_tf_state waiting_for_odom "
+                "initial_pose=(%.3f,%.3f,%.3f)"
+                % self.initial_pose
+            )
+            return
+
+        odom_x, odom_y, odom_yaw = self.latest_odom_pose
+        if not self.have_transform:
+            self.get_logger().info(
+                "odom_tf_state waiting_for_alignment "
+                "odom=(%.3f,%.3f,%.3f) pending_initialpose=%s"
+                % (odom_x, odom_y, odom_yaw, self.pending_initial_pose is not None)
+            )
+            return
+
+        cos_yaw = math.cos(self.map_to_odom_yaw)
+        sin_yaw = math.sin(self.map_to_odom_yaw)
+        map_x = self.map_to_odom_x + cos_yaw * odom_x - sin_yaw * odom_y
+        map_y = self.map_to_odom_y + sin_yaw * odom_x + cos_yaw * odom_y
+        map_yaw = normalize_angle(self.map_to_odom_yaw + odom_yaw)
+        self.get_logger().info(
+            "odom_tf_state aligned=%s odom=(%.3f,%.3f,%.3f) "
+            "map_pose=(%.3f,%.3f,%.3f) map_to_odom=(%.3f,%.3f,%.3f)"
+            % (
+                self.have_transform,
+                odom_x,
+                odom_y,
+                odom_yaw,
+                map_x,
+                map_y,
+                map_yaw,
+                self.map_to_odom_x,
+                self.map_to_odom_y,
+                self.map_to_odom_yaw,
+            )
+        )
+
+    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 = OdomMapTf()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/planner/scripts/preflight_check.py

@@ -0,0 +1,235 @@
+#!/usr/bin/env python3
+"""Validate the real-robot navigation interfaces before sending a goal."""
+
+import sys
+import time
+
+import rclpy
+from nav_msgs.msg import OccupancyGrid, Odometry
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.time import Time
+from sensor_msgs.msg import LaserScan
+from tf2_ros import Buffer, TransformListener
+
+
+class PreflightCheck(Node):
+    def __init__(self):
+        super().__init__("hybrid_astar_preflight")
+        self.declare_parameter("expected_map_width", 5.0)
+        self.declare_parameter("expected_map_height", 5.0)
+        self.declare_parameter("map_size_tolerance", 0.15)
+        self.declare_parameter("timeout_sec", 10.0)
+        self.declare_parameter("require_map", True)
+        self.declare_parameter("require_scan", True)
+        self.declare_parameter("require_tf", True)
+        self.declare_parameter("require_odom_tf", False)
+        self.declare_parameter("require_base_tf", True)
+        self.declare_parameter("map_frame", "map")
+        self.declare_parameter("odom_frame", "odom")
+        self.declare_parameter("base_frame", "base_footprint")
+        self.declare_parameter("map_topic", "/map")
+        self.declare_parameter("odom_topic", "/odom_combined")
+        self.declare_parameter("scan_topic", "/scan")
+        self.declare_parameter("cmd_vel_topic", "/cmd_vel")
+        self.declare_parameter("expected_scan_frame", "laser_link")
+        self.declare_parameter("tf_wait_sec", 3.0)
+        self.declare_parameter("drive_command_topic", "/ackermann_cmd")
+
+        self.require_map = bool(self.get_parameter("require_map").value)
+        self.require_scan = bool(self.get_parameter("require_scan").value)
+        self.require_tf = bool(self.get_parameter("require_tf").value)
+        self.require_odom_tf = bool(self.get_parameter("require_odom_tf").value)
+        self.require_base_tf = bool(self.get_parameter("require_base_tf").value)
+        self.map_topic = str(self.get_parameter("map_topic").value)
+        self.odom_topic = str(self.get_parameter("odom_topic").value)
+        self.scan_topic = str(self.get_parameter("scan_topic").value)
+        self.cmd_vel_topic = str(self.get_parameter("cmd_vel_topic").value)
+
+        map_qos = QoSProfile(
+            depth=1,
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+
+        self.map_msg = None
+        self.odom_msg = None
+        self.scan_msg = None
+        self.create_subscription(OccupancyGrid, self.map_topic, self._map_cb, map_qos)
+        self.create_subscription(Odometry, self.odom_topic, self._odom_cb, 10)
+        self.create_subscription(LaserScan, self.scan_topic, self._scan_cb, 10)
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+    def _map_cb(self, msg):
+        self.map_msg = msg
+
+    def _odom_cb(self, msg):
+        self.odom_msg = msg
+
+    def _scan_cb(self, msg):
+        self.scan_msg = msg
+
+    def wait_for_inputs(self):
+        deadline = time.monotonic() + float(self.get_parameter("timeout_sec").value)
+        while rclpy.ok() and time.monotonic() < deadline:
+            rclpy.spin_once(self, timeout_sec=0.1)
+            map_ready = self.map_msg is not None or not self.require_map
+            odom_ready = self.odom_msg is not None
+            scan_ready = self.scan_msg is not None or not self.require_scan
+            if map_ready and odom_ready and scan_ready:
+                return
+
+    def evaluate(self):
+        errors = []
+        if self.map_msg is None:
+            if self.require_map:
+                errors.append("no %s OccupancyGrid received" % self.map_topic)
+        else:
+            map_frame = str(self.get_parameter("map_frame").value)
+            if (
+                self.map_msg.header.frame_id
+                and self.map_msg.header.frame_id != map_frame
+            ):
+                errors.append(
+                    "/map frame is %s, expected %s"
+                    % (self.map_msg.header.frame_id, map_frame)
+                )
+            width = self.map_msg.info.width * self.map_msg.info.resolution
+            height = self.map_msg.info.height * self.map_msg.info.resolution
+            expected_width = float(self.get_parameter("expected_map_width").value)
+            expected_height = float(self.get_parameter("expected_map_height").value)
+            tolerance = float(self.get_parameter("map_size_tolerance").value)
+            self.get_logger().info(
+                f"map size {width:.3f} x {height:.3f} m, "
+                f"resolution {self.map_msg.info.resolution:.3f} m"
+            )
+            if (
+                abs(width - expected_width) > tolerance
+                or abs(height - expected_height) > tolerance
+            ):
+                errors.append(
+                    "map is not the expected %.2f x %.2f m (tolerance %.2f m)"
+                    % (expected_width, expected_height, tolerance)
+                )
+
+        if self.odom_msg is None:
+            errors.append("no %s received" % self.odom_topic)
+        if self.scan_msg is None:
+            if self.require_scan:
+                errors.append("no %s received" % self.scan_topic)
+        elif not self.scan_msg.header.frame_id:
+            errors.append("%s frame_id is empty" % self.scan_topic)
+        else:
+            expected_scan_frame = str(
+                self.get_parameter("expected_scan_frame").value
+            )
+            if (
+                expected_scan_frame
+                and self.scan_msg.header.frame_id != expected_scan_frame
+            ):
+                errors.append(
+                    "%s frame is %s, expected %s"
+                    % (self.scan_topic, self.scan_msg.header.frame_id, expected_scan_frame)
+                )
+
+        map_frame = str(self.get_parameter("map_frame").value)
+        odom_frame = str(self.get_parameter("odom_frame").value)
+        base_frame = str(self.get_parameter("base_frame").value)
+        if self.require_tf:
+            if self.require_odom_tf:
+                if not self._wait_for_tf(map_frame, odom_frame):
+                    errors.append("TF unavailable: %s -> %s" % (map_frame, odom_frame))
+
+            if self.require_base_tf and not self._wait_for_tf(map_frame, base_frame):
+                errors.append("TF unavailable: %s -> %s" % (map_frame, base_frame))
+            elif (
+                self.require_base_tf
+                and self.scan_msg is not None
+                and self.scan_msg.header.frame_id
+            ):
+                scan_frame = self.scan_msg.header.frame_id
+                if not self._wait_for_tf(base_frame, scan_frame):
+                    errors.append("TF unavailable: %s -> %s" % (base_frame, scan_frame))
+        elif self.require_scan and self.scan_msg is not None and self.scan_msg.header.frame_id:
+            scan_frame = self.scan_msg.header.frame_id
+            self.get_logger().info("scan frame check only: %s" % scan_frame)
+
+        publisher_deadline = time.monotonic() + 1.5
+        cmd_publishers = self.get_publishers_info_by_topic(self.cmd_vel_topic)
+        while not cmd_publishers and time.monotonic() < publisher_deadline:
+            rclpy.spin_once(self, timeout_sec=0.1)
+            cmd_publishers = self.get_publishers_info_by_topic(self.cmd_vel_topic)
+        if len(cmd_publishers) != 1:
+            names = [
+                "%s/%s" % (info.node_namespace, info.node_name)
+                for info in cmd_publishers
+            ]
+            errors.append(
+                "expected exactly one %s publisher, found %d: %s"
+                % (self.cmd_vel_topic, len(cmd_publishers), ", ".join(names) or "none")
+            )
+
+        drive_topic = str(self.get_parameter("drive_command_topic").value)
+        drive_publishers = self.get_publishers_info_by_topic(drive_topic)
+        drive_subscribers = self.get_subscriptions_info_by_topic(drive_topic)
+        if len(drive_publishers) != 1:
+            names = [
+                "%s/%s" % (info.node_namespace, info.node_name)
+                for info in drive_publishers
+            ]
+            errors.append(
+                "expected exactly one %s publisher, found %d: %s"
+                % (drive_topic, len(drive_publishers), ", ".join(names) or "none")
+            )
+        if len(drive_subscribers) != 1:
+            names = [
+                "%s/%s" % (info.node_namespace, info.node_name)
+                for info in drive_subscribers
+            ]
+            errors.append(
+                "expected exactly one %s subscriber, found %d: %s"
+                % (drive_topic, len(drive_subscribers), ", ".join(names) or "none")
+            )
+
+        for error in errors:
+            self.get_logger().error(error)
+        if errors:
+            return False
+
+        self.get_logger().info("preflight passed: navigation stack is ready for a goal")
+        return True
+
+    def _wait_for_tf(self, target_frame, source_frame):
+        tf_deadline = time.monotonic() + max(
+            0.5, float(self.get_parameter("tf_wait_sec").value)
+        )
+        while rclpy.ok() and time.monotonic() < tf_deadline:
+            if self.tf_buffer.can_transform(
+                target_frame,
+                source_frame,
+                Time(),
+                timeout=Duration(seconds=0.2),
+            ):
+                return True
+            rclpy.spin_once(self, timeout_sec=0.05)
+        return False
+
+
+def main():
+    rclpy.init()
+    node = PreflightCheck()
+    success = False
+    try:
+        node.wait_for_inputs()
+        success = node.evaluate()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+    if not success:
+        sys.exit(1)
+
+
+if __name__ == "__main__":
+    main()

src/planner/scripts/send_goal.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+"""Publish one map-frame goal for the real-robot Hybrid A* stack."""
+
+import argparse
+import math
+import time
+
+import rclpy
+from geometry_msgs.msg import PoseStamped
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+
+
+class GoalPublisher(Node):
+    def __init__(self, x, y, yaw, topic, frame_id):
+        super().__init__("send_goal")
+        goal_qos = QoSProfile(
+            depth=10,
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.publisher = self.create_publisher(PoseStamped, topic, goal_qos)
+        self.goal = PoseStamped()
+        self.goal.header.frame_id = frame_id
+        self.goal.pose.position.x = x
+        self.goal.pose.position.y = y
+        self.goal.pose.orientation.z = math.sin(yaw * 0.5)
+        self.goal.pose.orientation.w = math.cos(yaw * 0.5)
+
+    def publish_once(self):
+        self.goal.header.stamp = self.get_clock().now().to_msg()
+        self.publisher.publish(self.goal)
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("x", type=float)
+    parser.add_argument("y", type=float)
+    parser.add_argument("yaw", type=float, nargs="?", default=0.0)
+    parser.add_argument("--topic", default="/goal_pose")
+    parser.add_argument("--frame", default="map")
+    parser.add_argument("--timeout", type=float, default=5.0)
+    parser.add_argument("--repeat", type=int, default=1)
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = GoalPublisher(args.x, args.y, args.yaw, args.topic, args.frame)
+    deadline = time.monotonic() + max(0.5, args.timeout)
+    published = 0
+    while time.monotonic() < deadline:
+        rclpy.spin_once(node, timeout_sec=0.05)
+        if node.publisher.get_subscription_count() == 0:
+            continue
+        node.publish_once()
+        published += 1
+        time.sleep(0.10)
+        if published >= max(1, args.repeat):
+            break
+    if published == 0:
+        node.get_logger().error("no /goal_pose subscriber discovered before timeout")
+        node.destroy_node()
+        rclpy.shutdown()
+        return 1
+    node.get_logger().info(
+        f"published goal {published} times "
+        f"{args.frame}=({args.x:.3f}, {args.y:.3f}, {args.yaw:.3f} rad)"
+    )
+    node.destroy_node()
+    if rclpy.ok():
+        rclpy.shutdown()
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/planner/scripts/send_start_goal.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+"""Publish a known current pose and then a navigation goal."""
+
+import argparse
+import math
+import time
+
+import rclpy
+from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+
+
+def fill_yaw_quaternion(q, yaw):
+    q.x = 0.0
+    q.y = 0.0
+    q.z = math.sin(0.5 * yaw)
+    q.w = math.cos(0.5 * yaw)
+
+
+class StartGoalPublisher(Node):
+    def __init__(self, args):
+        super().__init__("send_start_goal")
+        qos = QoSProfile(
+            depth=10,
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.start_pub = self.create_publisher(
+            PoseWithCovarianceStamped, args.initialpose_topic, qos
+        )
+        self.goal_pub = self.create_publisher(PoseStamped, args.goal_topic, qos)
+        self.frame = args.frame
+
+        self.start = PoseWithCovarianceStamped()
+        self.start.header.frame_id = args.frame
+        self.start.pose.pose.position.x = args.start_x
+        self.start.pose.pose.position.y = args.start_y
+        fill_yaw_quaternion(self.start.pose.pose.orientation, args.start_yaw)
+        self.start.pose.covariance[0] = args.xy_covariance
+        self.start.pose.covariance[7] = args.xy_covariance
+        self.start.pose.covariance[35] = args.yaw_covariance
+
+        self.goal = PoseStamped()
+        self.goal.header.frame_id = args.frame
+        self.goal.pose.position.x = args.goal_x
+        self.goal.pose.position.y = args.goal_y
+        fill_yaw_quaternion(self.goal.pose.orientation, args.goal_yaw)
+
+    def publish_start(self):
+        self.start.header.stamp = self.get_clock().now().to_msg()
+        self.start_pub.publish(self.start)
+
+    def publish_goal(self):
+        self.goal.header.stamp = self.get_clock().now().to_msg()
+        self.goal_pub.publish(self.goal)
+
+
+def wait_for_subscribers(node, timeout):
+    deadline = time.monotonic() + max(0.5, timeout)
+    while time.monotonic() < deadline:
+        rclpy.spin_once(node, timeout_sec=0.05)
+        if (
+            node.start_pub.get_subscription_count() > 0
+            and node.goal_pub.get_subscription_count() > 0
+        ):
+            return True
+    return False
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("start_x", type=float)
+    parser.add_argument("start_y", type=float)
+    parser.add_argument("start_yaw", type=float)
+    parser.add_argument("goal_x", type=float)
+    parser.add_argument("goal_y", type=float)
+    parser.add_argument("goal_yaw", type=float, nargs="?", default=0.0)
+    parser.add_argument("--frame", default="map")
+    parser.add_argument("--initialpose-topic", default="/initialpose")
+    parser.add_argument("--goal-topic", default="/goal_pose")
+    parser.add_argument("--timeout", type=float, default=5.0)
+    parser.add_argument("--repeat", type=int, default=5)
+    parser.add_argument("--interval", type=float, default=0.10)
+    parser.add_argument("--goal-delay", type=float, default=0.30)
+    parser.add_argument("--xy-covariance", type=float, default=0.0025)
+    parser.add_argument("--yaw-covariance", type=float, default=0.0076)
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = StartGoalPublisher(args)
+    try:
+        if not wait_for_subscribers(node, args.timeout):
+            node.get_logger().error(
+                "missing subscribers: /initialpose=%d /goal_pose=%d"
+                % (
+                    node.start_pub.get_subscription_count(),
+                    node.goal_pub.get_subscription_count(),
+                )
+            )
+            return 1
+
+        for _ in range(max(1, args.repeat)):
+            node.publish_start()
+            rclpy.spin_once(node, timeout_sec=0.02)
+            time.sleep(max(0.02, args.interval))
+
+        time.sleep(max(0.0, args.goal_delay))
+        for _ in range(max(1, args.repeat)):
+            node.publish_goal()
+            rclpy.spin_once(node, timeout_sec=0.02)
+            time.sleep(max(0.02, args.interval))
+
+        node.get_logger().info(
+            "published start %s=(%.3f, %.3f, %.3f) and goal %s=(%.3f, %.3f, %.3f)"
+            % (
+                args.frame,
+                args.start_x,
+                args.start_y,
+                args.start_yaw,
+                args.frame,
+                args.goal_x,
+                args.goal_y,
+                args.goal_yaw,
+            )
+        )
+        return 0
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/planner/scripts/topology_pure_pursuit_node.py

@@ -10,7 +10,7 @@ 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
+from std_msgs.msg import Bool, String
 import yaml
 
 from rclpy.duration import Duration
@@ -53,6 +53,7 @@ class TopologyPurePursuit(Node):
         self.declare_parameter("plan_topic", "/plan")
         self.declare_parameter("cmd_vel_topic", "/cmd_vel")
         self.declare_parameter("status_topic", "/topology_status")
+        self.declare_parameter("cancel_topic", "/navigation_cancel")
         self.declare_parameter("enable_topology_planning", True)
         self.declare_parameter("accept_external_plan", False)
         self.declare_parameter("publish_cmd_vel", True)
@@ -69,11 +70,16 @@ class TopologyPurePursuit(Node):
         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("goal_yaw_tolerance", 0.10)
+        self.declare_parameter("align_goal_yaw", True)
+        self.declare_parameter("final_alignment_offset", 0.25)
+        self.declare_parameter("final_alignment_speed", 0.10)
         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.declare_parameter("state_log_period", 1.0)
 
         self.map_frame = self.get_parameter("map_frame").value
         self.enable_topology_planning = bool(
@@ -107,6 +113,16 @@ class TopologyPurePursuit(Node):
         self.min_turning_radius = float(
             self.get_parameter("min_turning_radius").value
         )
+        self.goal_yaw_tolerance = float(
+            self.get_parameter("goal_yaw_tolerance").value
+        )
+        self.align_goal_yaw = bool(self.get_parameter("align_goal_yaw").value)
+        self.final_alignment_offset = max(
+            0.05, float(self.get_parameter("final_alignment_offset").value)
+        )
+        self.final_alignment_speed = max(
+            self.min_linear_speed, float(self.get_parameter("final_alignment_speed").value)
+        )
         self.latency_compensation = bool(
             self.get_parameter("latency_compensation").value
         )
@@ -116,6 +132,9 @@ class TopologyPurePursuit(Node):
         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.state_log_period = max(
+            0.0, float(self.get_parameter("state_log_period").value)
+        )
 
         self.nodes = {}
         self.edges = {}
@@ -139,6 +158,9 @@ class TopologyPurePursuit(Node):
         self.completed = True
         self.tried_default_goal = False
         self.last_tf_warn_ns = 0
+        self.last_state_log_ns = 0
+        self.last_cmd_linear = 0.0
+        self.last_cmd_angular = 0.0
 
         self.plan_pub = self.create_publisher(
             NavPath, self.get_parameter("plan_topic").value, 10
@@ -156,6 +178,12 @@ class TopologyPurePursuit(Node):
             self._odom_callback,
             10,
         )
+        self.cancel_sub = self.create_subscription(
+            Bool,
+            self.get_parameter("cancel_topic").value,
+            self._cancel_callback,
+            10,
+        )
         self.goal_pose_sub = None
         self.goal_node_sub = None
         self.external_plan_sub = None
@@ -183,7 +211,9 @@ class TopologyPurePursuit(Node):
         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())}"
+            f"ready graph_nodes={len(self.nodes)} "
+            f"graph_edges={sum(len(v) for v in self.edges.values())} "
+            f"motion_enabled={self.publish_cmd_vel}"
         )
 
     def _load_graph(self, graph_file_value):
@@ -295,7 +325,23 @@ class TopologyPurePursuit(Node):
         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)}")
+        path_length = self._path_length(self.active_path)
+        goal_x, goal_y, goal_yaw = self.active_path[-1]
+        self._publish_status(
+            f"external_plan_ok points={len(self.active_path)} "
+            f"length={path_length:.3f} goal=({goal_x:.3f},{goal_y:.3f},{goal_yaw:.3f})"
+        )
+
+    def _cancel_callback(self, msg):
+        if not msg.data:
+            return
+        self.active_path = []
+        self.active_node_path = []
+        self.active_goal_node = ""
+        self.current_path_index = 0
+        self.completed = True
+        self._publish_cmd(0.0, 0.0)
+        self._publish_status("navigation_cancelled")
 
     def plan_to_node(self, goal_node):
         if goal_node not in self.nodes:
@@ -496,31 +542,57 @@ class TopologyPurePursuit(Node):
 
     def _control_loop(self):
         if not self.publish_cmd_vel:
+            pose = self._current_pose(compensate=False)
+            state_args = {"pose": pose, "mode": "motion_disabled"}
+            if pose is not None and self.active_path:
+                x, y, yaw, _, _ = pose
+                goal_x, goal_y, goal_yaw = self.active_path[-1]
+                state_args["goal_distance"] = math.hypot(goal_x - x, goal_y - y)
+                state_args["goal_yaw_error"] = abs(normalize_angle(goal_yaw - yaw))
+                nearest_index = self._nearest_path_index(x, y)
+                nearest_x, nearest_y, _ = self.active_path[nearest_index]
+                state_args["nearest_index"] = nearest_index
+                state_args["cross_track_error"] = math.hypot(nearest_x - x, nearest_y - y)
+            self._maybe_log_state(**state_args)
             return
 
         pose = self._current_pose(compensate=True)
         if pose is None:
             self._stop_if_needed("stop no_recent_odom")
+            self._maybe_log_state(pose=None, mode="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)
+            self._maybe_log_state(pose=pose, mode="idle")
             return
 
         x, y, yaw, _, _ = pose
         goal_x, goal_y, _ = self.active_path[-1]
+        goal_yaw = self.active_path[-1][2]
         goal_distance = math.hypot(goal_x - x, goal_y - y)
+        goal_yaw_error = abs(normalize_angle(goal_yaw - yaw))
         if goal_distance <= self.goal_tolerance:
+            if self.align_goal_yaw and goal_yaw_error > self.goal_yaw_tolerance:
+                self._align_final_yaw(
+                    x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error
+                )
+                return
             self.completed = True
             self._publish_cmd(0.0, 0.0)
-            self._publish_status(f"goal_reached {self.active_goal_node}")
+            self._publish_status(
+                f"goal_reached {self.active_goal_node} "
+                f"goal_error={goal_distance:.3f} yaw_error={goal_yaw_error:.3f}"
+            )
             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]
+        nearest_x, nearest_y, _ = self.active_path[nearest_index]
+        cross_track_error = math.hypot(nearest_x - x, nearest_y - y)
 
         dx = target_x - x
         dy = target_y - y
@@ -540,6 +612,51 @@ class TopologyPurePursuit(Node):
         angular = speed * curvature
         angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
         self._publish_cmd(speed, angular)
+        self._maybe_log_state(
+            pose=pose,
+            mode="tracking",
+            goal_distance=goal_distance,
+            goal_yaw_error=goal_yaw_error,
+            nearest_index=nearest_index,
+            target_index=target_index,
+            cross_track_error=cross_track_error,
+            reverse=reverse,
+            curvature=curvature,
+            lookahead=lookahead,
+        )
+
+    def _align_final_yaw(self, x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error):
+        reverse = math.cos(normalize_angle(goal_yaw - yaw)) < 0.0
+        target_x = goal_x - self.final_alignment_offset * math.cos(goal_yaw) if reverse else goal_x + self.final_alignment_offset * math.cos(goal_yaw)
+        target_y = goal_y - self.final_alignment_offset * math.sin(goal_yaw) if reverse else goal_y + self.final_alignment_offset * math.sin(goal_yaw)
+        control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
+        dx = target_x - x
+        dy = target_y - y
+        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.final_alignment_speed if reverse else self.final_alignment_speed
+        angular = speed * curvature
+        angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
+        self._publish_cmd(speed, angular)
+        self._maybe_log_state(
+            pose=(x, y, yaw, 0.0, 0.0),
+            mode="align_goal_yaw",
+            goal_distance=math.hypot(goal_x - x, goal_y - y),
+            goal_yaw_error=goal_yaw_error,
+            nearest_index=self.current_path_index,
+            target_index=len(self.active_path) - 1 if self.active_path else 0,
+            reverse=reverse,
+            curvature=curvature,
+            lookahead=lookahead,
+        )
+        self._publish_status(
+            f"aligning_goal_yaw error={goal_yaw_error:.3f} reverse={reverse}"
+        )
 
     def _nearest_path_index(self, x, y):
         start = max(0, self.current_path_index - 5)
@@ -583,6 +700,62 @@ class TopologyPurePursuit(Node):
             return fallback_yaw
         return math.atan2(ty - sy, tx - sx)
 
+    @staticmethod
+    def _path_length(path):
+        return sum(
+            math.hypot(b[0] - a[0], b[1] - a[1])
+            for a, b in zip(path, path[1:])
+        )
+
+    def _maybe_log_state(
+        self,
+        pose=None,
+        mode="idle",
+        goal_distance=None,
+        goal_yaw_error=None,
+        nearest_index=None,
+        target_index=None,
+        cross_track_error=None,
+        reverse=False,
+        curvature=None,
+        lookahead=None,
+    ):
+        if self.state_log_period <= 0.0:
+            return
+        now_ns = self.get_clock().now().nanoseconds
+        min_period_ns = int(self.state_log_period * 1_000_000_000)
+        if now_ns - self.last_state_log_ns < min_period_ns:
+            return
+        self.last_state_log_ns = now_ns
+
+        parts = [f"tracker_state mode={mode}"]
+        if pose is None:
+            parts.append("pose=unavailable")
+        else:
+            x, y, yaw, vx, wz = pose
+            parts.append(f"pose=({x:.3f},{y:.3f},{yaw:.3f})")
+            parts.append(f"odom_twist=({vx:.3f},{wz:.3f})")
+
+        parts.append(f"path_points={len(self.active_path)}")
+        parts.append(f"completed={self.completed}")
+        if goal_distance is not None:
+            parts.append(f"goal_distance={goal_distance:.3f}")
+        if goal_yaw_error is not None:
+            parts.append(f"yaw_error={goal_yaw_error:.3f}")
+        if nearest_index is not None:
+            parts.append(f"nearest_index={nearest_index}")
+        if target_index is not None:
+            parts.append(f"target_index={target_index}")
+        if cross_track_error is not None:
+            parts.append(f"cross_track={cross_track_error:.3f}")
+        if curvature is not None:
+            parts.append(f"curvature={curvature:.3f}")
+        if lookahead is not None:
+            parts.append(f"lookahead={lookahead:.3f}")
+        parts.append(f"reverse={reverse}")
+        parts.append(f"cmd=({self.last_cmd_linear:.3f},{self.last_cmd_angular:.3f})")
+        self.get_logger().info(" ".join(parts))
+
     def _target_speed(self, abs_curvature, goal_distance, reverse):
         base_speed = self.reverse_speed if reverse else self.linear_speed
         speed = base_speed
@@ -594,9 +767,11 @@ class TopologyPurePursuit(Node):
         return -speed if reverse else speed
 
     def _publish_cmd(self, linear, angular):
+        self.last_cmd_linear = float(linear)
+        self.last_cmd_angular = float(angular)
         cmd = Twist()
-        cmd.linear.x = float(linear)
+        cmd.linear.x = self.last_cmd_linear
-        cmd.angular.z = float(angular)
+        cmd.angular.z = self.last_cmd_angular
         self.cmd_pub.publish(cmd)
 
     def _stop_if_needed(self, reason):

src/planner/src/planner_version.cpp

@@ -1,6 +1,6 @@
   #include <iostream>
 
-int main(int argc, char* argv[])
+  int main()
   {
     std::cout << "planner package: Nav2 minimal global planning stack" << std::endl;
     std::cout << "  - slam_toolbox (online_async)" << std::endl;

src/planner/操作手册.md

@@ -0,0 +1,238 @@
+# 自定义 Hybrid A* 导航操作手册
+
+本手册适用于仓库中的实车启动入口 `real_hybrid_astar.launch.py`。系统链路为：
+
+`STM32 底盘 -> EKF 里程计 -> AMCL/地图 -> 激光雷达 -> Hybrid A* -> Pure Pursuit -> 阿克曼命令转换`
+
+## 1. 安全和前置条件
+
+开始前确认：
+
+1. 机器人架起或周围有足够空旷区域，首次测试使用低速参数。
+2. 底盘串口、波特率和雷达设备路径正确。
+3. 真实地图 YAML 与对应 PGM/PNG 文件存在，地图坐标系为 `map`。
+4. 已测得机器人最小转弯半径，并在 `real_hybrid_astar.yaml` 中配置。
+5. 机器人初始位置在地图的可通行区域内。
+6. 只启动本手册中的导航链路。不要同时启动 Nav2 `controller_server`、
+   `origincar_bringup`、`path_follower_demo.py` 或 `udp_to_cmdvel.py`，否则会有多个
+   节点同时发布 `/cmd_vel`。
+
+仓库中的 `maps/real_5x5.yaml` 是空房间合成地图，只用于软件链路测试，不能作为真实场地地图。
+
+## 2. 编译工作空间
+
+在机器人电脑上执行：
+
+```bash
+cd ~/liao_ws
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install \
+  --packages-select \
+  origincar_msg origincar_description origincar_base \
+  lslidar_msgs lslidar_driver planner
+source install/setup.bash
+```
+
+如果只修改了 `planner`，也可以只编译该包及其依赖。每个新终端都要重新执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+source ~/liao_ws/install/setup.bash
+```
+
+## 3. 检查地图
+
+地图 YAML 和图像必须成对存在。先检查尺寸、分辨率和原点：
+
+```bash
+cd ~/liao_ws
+python3 src/planner/scripts/map_info.py /实际路径/map.yaml
+```
+
+仓库测试地图的检查命令：
+
+```bash
+python3 src/planner/scripts/map_info.py \
+  src/planner/maps/real_5x5.yaml
+```
+
+5×5 米地图应显示 `size: 5.000 x 5.000 m` 和 `PASS`。真实地图尺寸不是 5×5 米时，启动前记录实际宽高，后面传给 `preflight_check.py`。
+
+## 4. 启动导航系统
+
+启动前替换地图、雷达参数和底盘串口：
+
+```bash
+ros2 launch planner real_hybrid_astar.launch.py \
+  map:=/实际地图/map.yaml \
+  lidar_params:=/实际雷达参数.yaml \
+  lidar_serial_port:=/dev/ttyCH343USB0 \
+  lidar_model:=N10 \
+  start_lidar:=true \
+  serial_port:=/dev/实际底盘串口 \
+  serial_baud:=115200 \
+  enable_motion:=false
+```
+
+使用仓库合成地图进行软件测试：
+
+```bash
+ros2 launch planner real_hybrid_astar.launch.py \
+  map:=$HOME/liao_ws/src/planner/maps/real_5x5.yaml \
+  lidar_params:=/实际雷达参数.yaml \
+  lidar_serial_port:=/dev/ttyCH343USB0 \
+  lidar_model:=N10 \
+  start_lidar:=true \
+  serial_port:=/dev/实际底盘串口 \
+  serial_baud:=115200 \
+  enable_motion:=false
+```
+
+保持该终端运行并观察日志。启动入口已经包含底盘、EKF、雷达、地图服务器、AMCL、规划器、Pure Pursuit 和 `/cmd_vel` 到 `/ackermann_cmd` 的转换节点。首次启动保持 `enable_motion:=false`，此时只验证规划，不执行路径。
+
+如果已经通过 `ros2 launch lslidar_driver ...` 单独启动雷达，必须把 `start_lidar:=true` 改为 `start_lidar:=false`，避免两个进程争抢同一串口。
+
+## 5. 检查 ROS 接口
+
+打开第二个终端并加载环境：
+
+```bash
+source /opt/ros/humble/setup.bash
+source ~/liao_ws/install/setup.bash
+```
+
+依次检查：
+
+```bash
+ros2 topic echo /scan --once
+ros2 topic echo /map --once
+ros2 topic echo /odom_combined --once
+ros2 run tf2_ros tf2_echo map base_footprint
+```
+
+确认以下条件：
+
+- `/scan` 持续有激光数据；
+- `/map` 的 `header.frame_id` 为 `map`；
+- `/odom_combined` 持续更新；
+- `map -> base_footprint` 和 `base_footprint -> laser` TF 可用。
+
+## 6. 初始化 AMCL 位姿
+
+在 RViz 中使用 **2D Pose Estimate** 设置机器人当前位置，或命令行发布（将坐标替换为实际值）：
+
+```bash
+ros2 topic pub --once /initialpose \
+  geometry_msgs/msg/PoseWithCovarianceStamped \
+  "{header: {frame_id: map}, pose: {pose: {position: {x: 0.54, y: 0.20}, orientation: {w: 1.0}}, covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.10]}}"
+```
+
+等待 AMCL 稳定，确认 `map -> base_footprint` TF 连续可查。
+
+## 7. 运行启动前检查
+
+默认按 5×5 米地图检查：
+
+```bash
+ros2 run planner preflight_check.py
+```
+
+检查通过后才允许发送目标。预检还会确认 `/cmd_vel` 只有一个发布者，并检查 `/ackermann_cmd` 转换节点与底盘订阅者。真实地图尺寸不同，例如 5.45×5.75 米：
+
+```bash
+ros2 run planner preflight_check.py --ros-args \
+  -p expected_map_width:=5.45 \
+  -p expected_map_height:=5.75
+```
+
+检查失败时，不要发送目标，先根据错误补齐地图、雷达、里程计或 TF。
+
+## 8. 发布目标点
+
+目标点使用 `map` 坐标系，单位为米：
+
+```bash
+ros2 run planner send_goal.py 1.00 1.00
+```
+
+第三个参数可指定终点航向角（弧度）：
+
+```bash
+ros2 run planner send_goal.py 1.00 1.00 1.57
+```
+
+默认只要求到达目标位置，位置误差小于约 0.15 米即停止。状态和路径不是持久话题，应先在其他终端启动监视，再发送目标：
+
+```bash
+ros2 topic echo /hybrid_astar_status
+ros2 topic echo /pure_pursuit_status
+ros2 topic echo /plan
+```
+
+只规划模式下，正常结果是规划状态 `plan_ok`、`/plan` 出现路径，且机器人不运动。`enable_motion:=true` 时，Pure Pursuit 才会发布实际速度，经过阿克曼转换后由底盘执行，机器人到达目标后停车。
+
+仓库地图的坐标范围是 `[-2.5, 2.5)`，目标必须严格位于范围内部并避开膨胀后的边界墙，不能使用 `(2.5, 2.5)`。
+
+首次只规划测试完成后，先取消目标并停止启动文件。确认转向方向、轴距和最大转角正确，再重新启动并显式允许低速运动：
+
+```bash
+ros2 launch planner real_hybrid_astar.launch.py \
+  map:=/实际地图/map.yaml \
+  lidar_params:=/实际雷达参数.yaml \
+  lidar_serial_port:=/dev/ttyCH343USB0 \
+  lidar_model:=N10 \
+  start_lidar:=true \
+  serial_port:=/dev/实际底盘串口 \
+  serial_baud:=115200 \
+  wheelbase:=0.143 \
+  max_steering_angle:=0.60 \
+  enable_motion:=true
+```
+
+重新启动后必须再次初始化 AMCL、运行 `preflight_check.py`，然后发布一个距离较近且无障碍的目标。第一次闭环测试应架起驱动轮或准备硬件急停，确认前进、倒车和左右转向符号正确后再落地运行。
+
+## 9. 取消导航和停车
+
+取消当前目标必须使用导航取消话题：
+
+```bash
+ros2 topic pub --once /navigation_cancel std_msgs/msg/Bool "{data: true}"
+```
+
+该命令会清除规划目标和跟踪路径，并持续保持停车。只发布一次零速度 `/cmd_vel` 不可靠，因为跟踪器可能在 20 Hz 下重新覆盖它。
+
+## 10. 常见问题
+
+### `preflight_check.py` 报无 `/map`
+
+检查 `map:=` 路径、YAML 引用的图像路径，以及 `map_server` 和生命周期管理器日志。
+
+### 地图尺寸检查失败
+
+用 `map_info.py` 查看实际尺寸，然后通过 `expected_map_width` 和 `expected_map_height` 传入真实尺寸。不要为了绕过检查而修改真实地图参数。
+
+### 无 `/scan` 或雷达 TF
+
+检查 `lidar_params` 是否匹配雷达型号、串口权限和 `base_footprint -> laser` 静态 TF。
+
+### 无 `map -> base_footprint`
+
+先在 RViz 设置 AMCL 初始位姿，确认 `/scan` 与地图大致重合；没有有效定位时禁止自动驾驶。
+
+### `/cmd_vel` 发布者数量不是 1
+
+执行：
+
+```bash
+ros2 topic info /cmd_vel -v
+```
+
+关闭重复启动的控制器，只保留 `topology_pure_pursuit` 发布速度指令。
+
+### 规划失败或机器人原地不动
+
+检查起点和目标是否在空闲区域，地图障碍物膨胀是否过大，机器人最小转弯半径是否填写正确，并查看 `/hybrid_astar_status` 的错误信息。
+
+## 11. 结束运行
+
+先发送 `/navigation_cancel`，确认机器人停止，再在启动终端按 `Ctrl+C` 关闭节点。实车测试结束后断开驱动电源或使底盘进入硬件急停状态。