forked from zbw/yiliao2026
目前实现较快速从原点到达某个点，但是然后回到原点时有较大问题
This commit is contained in:
@@ -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. 启动方式
|
||||
|
||||
|
||||
@@ -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}
|
||||
)
|
||||
|
||||
|
||||
@@ -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
|
||||
`cmd_vel_to_ackermann_drive.py` bridge and configure the base driver
|
||||
- If the base expects Ackermann commands, run
|
||||
`scripts/ackermann_cmd_bridge.py` and configure the base driver
|
||||
accordingly.
|
||||
|
||||
## Planner Semantics
|
||||
|
||||
160
src/planner/REAL_ROBOT_RUNBOOK.md
Normal file
160
src/planner/REAL_ROBOT_RUNBOOK.md
Normal file
@@ -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`.
|
||||
238
src/planner/REAL_ROBOT_TEST_PLAN_ZH.md
Normal file
238
src/planner/REAL_ROBOT_TEST_PLAN_ZH.md
Normal file
@@ -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. 没有任何碰撞、失控、错误方向或多控制器竞争。
|
||||
66
src/planner/config/odom_hybrid_astar.yaml
Normal file
66
src/planner/config/odom_hybrid_astar.yaml
Normal file
@@ -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
|
||||
35
src/planner/config/odom_pure_pursuit.yaml
Normal file
35
src/planner/config/odom_pure_pursuit.yaml
Normal file
@@ -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
|
||||
41
src/planner/config/real_amcl.yaml
Normal file
41
src/planner/config/real_amcl.yaml
Normal file
@@ -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
|
||||
64
src/planner/config/real_hybrid_astar.yaml
Normal file
64
src/planner/config/real_hybrid_astar.yaml
Normal file
@@ -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
|
||||
33
src/planner/config/real_pure_pursuit.yaml
Normal file
33
src/planner/config/real_pure_pursuit.yaml
Normal file
@@ -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
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
src/planner/launch/__pycache__/planner.launch.cpython-313.pyc
Normal file
BIN
src/planner/launch/__pycache__/planner.launch.cpython-313.pyc
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
394
src/planner/launch/odom_hybrid_astar.launch.py
Normal file
394
src/planner/launch/odom_hybrid_astar.launch.py
Normal file
@@ -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,
|
||||
])
|
||||
250
src/planner/launch/real_hybrid_astar.launch.py
Normal file
250
src/planner/launch/real_hybrid_astar.launch.py
Normal file
@@ -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,
|
||||
])
|
||||
7
src/planner/maps/real_10x10.yaml
Normal file
7
src/planner/maps/real_10x10.yaml
Normal file
@@ -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
|
||||
BIN
src/planner/maps/real_5x5.pgm
Normal file
BIN
src/planner/maps/real_5x5.pgm
Normal file
Binary file not shown.
7
src/planner/maps/real_5x5.yaml
Normal file
7
src/planner/maps/real_5x5.yaml
Normal file
@@ -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
|
||||
@@ -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>
|
||||
|
||||
148
src/planner/scripts/ackermann_cmd_bridge.py
Executable file
148
src/planner/scripts/ackermann_cmd_bridge.py
Executable file
@@ -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()
|
||||
180
src/planner/scripts/evaluate_navigation_test.py
Executable file
180
src/planner/scripts/evaluate_navigation_test.py
Executable file
@@ -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())
|
||||
717
src/planner/scripts/goal_reach_debug_collector.py
Executable file
717
src/planner/scripts/goal_reach_debug_collector.py
Executable file
@@ -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())
|
||||
0
src/planner/scripts/grid_astar_theta_node.py
Normal file → Executable file
0
src/planner/scripts/grid_astar_theta_node.py
Normal file → Executable file
98
src/planner/scripts/lidar_health_check.py
Executable file
98
src/planner/scripts/lidar_health_check.py
Executable file
@@ -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()
|
||||
83
src/planner/scripts/map_info.py
Executable file
83
src/planner/scripts/map_info.py
Executable file
@@ -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())
|
||||
460
src/planner/scripts/navigation_test_recorder.py
Executable file
460
src/planner/scripts/navigation_test_recorder.py
Executable file
@@ -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()
|
||||
249
src/planner/scripts/odom_map_tf.py
Executable file
249
src/planner/scripts/odom_map_tf.py
Executable file
@@ -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()
|
||||
235
src/planner/scripts/preflight_check.py
Executable file
235
src/planner/scripts/preflight_check.py
Executable file
@@ -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()
|
||||
75
src/planner/scripts/send_goal.py
Executable file
75
src/planner/scripts/send_goal.py
Executable file
@@ -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())
|
||||
135
src/planner/scripts/send_start_goal.py
Executable file
135
src/planner/scripts/send_start_goal.py
Executable file
@@ -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())
|
||||
187
src/planner/scripts/topology_pure_pursuit_node.py
Normal file → Executable file
187
src/planner/scripts/topology_pure_pursuit_node.py
Normal file → Executable file
@@ -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.angular.z = float(angular)
|
||||
cmd.linear.x = self.last_cmd_linear
|
||||
cmd.angular.z = self.last_cmd_angular
|
||||
self.cmd_pub.publish(cmd)
|
||||
|
||||
def _stop_if_needed(self, reason):
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,11 +1,11 @@
|
||||
#include <iostream>
|
||||
#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;
|
||||
std::cout << " - planner_server (SmacPlannerHybrid)" << std::endl;
|
||||
std::cout << " - global_costmap + local_costmap" << std::endl;
|
||||
std::cout << " - lifecycle_manager" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
238
src/planner/操作手册.md
Normal file
238
src/planner/操作手册.md
Normal file
@@ -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` 关闭节点。实车测试结束后断开驱动电源或使底盘进入硬件急停状态。
|
||||
12664
test_results/goal_reach_debug_20260722_172834.json
Normal file
12664
test_results/goal_reach_debug_20260722_172834.json
Normal file
File diff suppressed because it is too large
Load Diff
12680
test_results/goal_reach_debug_20260722_173109.json
Normal file
12680
test_results/goal_reach_debug_20260722_173109.json
Normal file
File diff suppressed because it is too large
Load Diff
5528
test_results/reach_fail_debug_20260722_163711.json
Normal file
5528
test_results/reach_fail_debug_20260722_163711.json
Normal file
File diff suppressed because it is too large
Load Diff
10717
test_results/reach_fail_debug_20260722_163934.json
Normal file
10717
test_results/reach_fail_debug_20260722_163934.json
Normal file
File diff suppressed because it is too large
Load Diff
11838
test_results/reach_fail_debug_20260722_165021.json
Normal file
11838
test_results/reach_fail_debug_20260722_165021.json
Normal file
File diff suppressed because it is too large
Load Diff
8478
test_results/reach_fail_debug_20260722_170425.json
Normal file
8478
test_results/reach_fail_debug_20260722_170425.json
Normal file
File diff suppressed because it is too large
Load Diff
10166
test_results/reach_fail_debug_20260722_170521.json
Normal file
10166
test_results/reach_fail_debug_20260722_170521.json
Normal file
File diff suppressed because it is too large
Load Diff
11103
test_results/reach_fail_debug_20260722_170728.json
Normal file
11103
test_results/reach_fail_debug_20260722_170728.json
Normal file
File diff suppressed because it is too large
Load Diff
11253
test_results/reach_fail_debug_20260722_170806.json
Normal file
11253
test_results/reach_fail_debug_20260722_170806.json
Normal file
File diff suppressed because it is too large
Load Diff
11103
test_results/reach_fail_debug_20260722_171435.json
Normal file
11103
test_results/reach_fail_debug_20260722_171435.json
Normal file
File diff suppressed because it is too large
Load Diff
9127
test_results/reach_fail_debug_20260722_172319.json
Normal file
9127
test_results/reach_fail_debug_20260722_172319.json
Normal file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user