企业官网建设流程全解析

• 机器人
• ROS
• 自动驾驶

本指南围绕 Navigation2（ROS 2 Navigation Framework）中全局规划器（Global Planner）的组件级测试方案展开，以 nav2_system_tests/src/planning/README.md 为核心骨架，深入剖析PlannerTester节点如何用代价地图表达世界、请求规划服务并校验路径质量。读完本文，你将掌握该测试体系的工作原理、PlannerTester的核心接口与生命周期、单次与随机化测试的判定逻辑，以及如何基于仓库源码将其复用于新的全局规划插件（NavFn、Smac、Theta* 等）的回归验证。

全局规划器随机测试输出示例：蓝色球体为起点、绿色球体为目标点、红色线段为计算出的路径、灰色格子为障碍物

一、测试架构总览：PlannerTester 的职责与定位

PlannerTester是一个为全局规划器专门设计的组件级测试节点，它承担三项核心职责（见 planner_tester.hpp）：

1. 提供世界表示：以代价地图（costmap）的形式表达环境，规划器据此搜索路径；
2. 发送规划请求：向全局规划器发起路径生成请求（ComputePathToPose语义的createPlan调用）；
3. 接收并校验路径：接收返回的nav_msgs::msg::Path，检查其是否与障碍物碰撞、起终点是否与请求一致。

该测试体系刻意采用"简化版世界模型 + 简化版代价地图"来隔离变量：不依赖完整仿真环境、不依赖传感器数据流，只验证"给定一张代价地图，规划器能否给出正确、无碰撞的路径"这一核心契约，因此非常适合作为全局规划器的单元级/组件级回归测试基线。

从源码结构看，测试目录 nav2_system_tests/src/planning 中共有两类参与者：

• PlannerTester（继承nav2::LifecycleNode）：扮演"测试驱动者"，负责构造世界、管理生命周期、发起请求并做断言；
• NavFnPlannerTester（继承nav2_planner::PlannerServer）：直接复用真实生产代码中的规划服务端（planner_server），确保测试对象与线上实现一致，而不是另写一套"测试专用规划器"。

其中NavFnPlannerTester通过公开onConfigure/onActivate/onDeactivate/onCleanup四个生命周期钩子，允许测试代码显式驱动规划服务端的状态机，这为隔离测试环境提供了便利。

二、世界模型：代价地图与六种内置测试场景

文档明确指出："目前世界被表示为一张代价地图，测试使用简化版世界模型与代价地图。"仓库中这一抽象由 nav2_util/include/nav2_util/costmap.hpp 的nav2_util::Costmap实现，它支持两种来源：

• 静态地图：通过set_static_map()从nav_msgs::msg::OccupancyGrid填充；
• 内置测试数据：通过set_test_costmap()加载硬编码的测试代价地图。

内置测试代价地图共六种，通过枚举TestCostmap定义：

这六种场景覆盖了"无障→单障碍→复杂迷宫"的难度梯度，test_planner_costmaps_node.cpp 中的testSimpleCostmaps测试会依次对全部六种场景执行一次defaultPlannerTest并断言全部成功。

Costmap还提供一组静态常量用于映射常见代价语义：no_information（未知，对应代价 255）、lethal_obstacle（致命障碍）、inscribed_inflated_obstacle、medium_cost、free_space。构造参数包括trinary_costmap（是否三值化）、track_unknown_space（是否跟踪未知区域）、lethal_threshold（致死阈值）与unknown_cost_value（未知代价取值）。

PlannerTester构造函数中给出的默认值（planner_tester.cpp）为：

• trinary_costmap_ = true
• track_unknown_space_ = false
• lethal_threshold_ = 100
• unknown_cost_value_ = -1
• 默认测试地图类型TestCostmap::open_space

三、PlannerTester 的核心接口与生命周期管理

PlannerTester对外暴露的主要方法如下（完整声明见 planner_tester.hpp）：

3.1 激活流程（activate）

activate()会按顺序完成以下初始化（planner_tester.cpp）：

1. 启动独立线程旋转 ROS 节点（nav2::NodeThread）；
2. 创建Costmap并默认加载open_space测试地图（10x10 空栅格）；
3. 启动机器人位姿 TF 广播（map → base_link，初始位置为(1.0, 1.0)，每 100ms 发布一次）；
4. 创建NavFnPlannerTester规划服务端，并声明三个关键参数：
   • GridBased.use_astar = true（启用 A* 变体而非纯 Dijkstra）；
   • expected_planner_frequency = -1.0（不约束规划频率，避免测试因频率告警失败）；
   • costmap_update_timeout = 0.0（不等待代价地图更新超时）；
5. 发布map话题（OccupancyGrid，用于可视化/调试）；
6. 创建is_path_valid服务客户端；
7. 依次驱动规划服务端完成onConfigure与onActivate。

注意其中NavFnPlannerTester是真实规划服务端（nav2_planner::PlannerServer的子类），这意味着测试直接走生产代码的配置与规划路径，具备很高的可信度。

3.2 世界加载的两种方式

方式一：真实地图（loadDefaultMap）。从环境变量TEST_MAP读取地图图片路径，使用nav2_map_server的loadMapFromFile加载，加载参数在 planner_tester.cpp 中硬编码为：

• 分辨率resolution = 1.0；
• negate = false；
• 占用阈值occupancy_threshold = 0.65；
• 空闲阈值free_threshold = 0.196；
• 原点偏移origin = {0.0, 0.0, 0.0}；
• 地图模式MapMode::Trinary（三值化）。

若TEST_MAP未设置，会抛出运行时异常并给出明确提示。加载成功后，通过 1 秒周期的定时器持续在map话题上发布地图，并将using_fake_costmap_置为false。

方式二：内置测试代价地图（loadSimpleCostmap）。直接调用costmap_->set_test_costmap(testCostmapType)，速度快、无 I/O 依赖，适合作为 CI 常规测试路径。

四、单次路径测试：defaultPlannerTest 的起终点约定

defaultPlannerTest（planner_tester.cpp）执行一次固定起终点的规划并校验结果，起终点根据世界来源不同而不同：

• 内置测试地图（using_fake_costmap_为 true）：起点(1.0, 1.0)→ 目标(8.0, 8.0)，在 10x10 栅格内留出边界，确保起点与目标均位于空闲区域；
• 真实地图：起点(390.0, 10.0)→ 目标(10.0, 390.0)，以世界坐标系表述（规划器内部会自行转换到地图坐标系）。

随后调用plannerTest(robot_position, goal, path)完成实际执行：

1. updateRobotPosition()把机器人位姿写入 TF，并睡眠 50ms 让变换生效；
2. createPlan(goal, path)先通过planner_tester_->setCostmap(costmap_.get())把测试世界同步进规划服务端，再调用planners_["GridBased"]->createPlan(...)生成路径（planner_tester.hpp）；
3. 若路径点数为 0（即使无异常抛出），同样判定失败；
4. 成功后进入质量校验：isCollisionFree(path) && isWithinTolerance(...)。

五、随机化回归测试：defaultPlannerRandomTests

这是文档强调的"可以顺序传入随机起点与目标点并检查路径是否碰撞"的能力，实现在 planner_tester.cpp：

1. 随机源：使用std::random_device+std::mt19937；
2. 采样范围：uniform_int_distribution<>(1, size_x-1)/(1, size_y-1)，避开地图边界；
3. 自由点筛选：反复采样直到该栅格costmap_->is_free(x, y)为真，保证起点和目标点必然落在可通行区域（这一筛选也解释了图中为何不存在"起点在障碍物内"的情况）；
4. 执行：每轮调用plannerTest并累计失败数，同时记录num_fail与总耗时（high_resolution_clock）；
5. 判据：num_fail / number_tests > acceptable_fail_ratio则整体失败。acceptable_fail_ratio默认 0.1（10%），可在调用时覆盖。

配套的 test_planner_random_node.cpp 中，testWithHundredRandomEndPoints每次执行 100 轮随机测试、可重试 3 次（容忍偶发失败），任一成功即通过，用EXPECT_EQ(true, success)断言。

从测试输出看（README 中给出的 example_result.png）：蓝色球体为起点，绿色球体为目标点，红色线段为规划出的路径，灰色格子为障碍物。图中存在少量只有起点/终点而无红线的"孤儿球体"——文档注明这是 NavFn 算法偶发无法生成路径所致，这也是引入acceptable_fail_ratio容错机制的根源。

六、路径质量校验：碰撞检测与起终点一致性

6.1 碰撞检测 isCollisionFree

逐点遍历路径，对每个位姿用costmap_->is_free(round(x), round(y))检查所在栅格是否空闲，任一栅格不空闲即告失败并打印整条路径（planner_tester.cpp）。

6.2 起终点一致性 isWithinTolerance

当前实现为简化版（源码注释标注"Work in progress"）：仅校验路径首点等于请求起点、末点等于请求目标（planner_tester.cpp）。接口预留了deviation_tolerance与reference_path参数（defaultPlannerTest默认容差 1.0），为将来与参考路径做偏差比较留好了扩展点。

6.3 辅助工具

printPath()以x / y三位精度逐点打印路径，用于失败时定位问题栅格；isPathValid()则封装了对IsPathValid服务的调用（见下节）。

七、IsPathValid 服务封装与参数语义

PlannerTester::isPathValid（planner_tester.cpp）构造nav2_msgs::srv::IsPathValid::Request并异步调用is_path_valid服务，spin_until_future_complete超时 100ms。请求字段与语义如下：

test_planner_is_path_valid.cpp 对这些字段做了系统化覆盖测试，值得关注的断言组合包括：

• 空路径：success=false, is_valid=false；
• 穿过障碍的路径：is_valid=false，且invalid_pose_indices非空、首个非法点索引>0；
• 未知区域语义：同一路径在consider_unknown_as_obstacle=false时有效，切换为true后无效；
• max_cost 阈值：max_cost=0时即使路径几何上合法也判为无效；
• 自定义足迹：合法方形足迹通过；畸形字符串（"invalid_footprint_string"）导致success=false；
• 图层名：layer_name=""使用完整代价地图并通过；不存在的图层名返回失败；
• stop_at_first_collision：true时invalid_pose_indices.size()==1，false时>1；
• max_lookahead_distance：短前瞻距离校验的是路径子段，可能避开远处的障碍。

八、多插件回归测试：一条代码覆盖五种全局规划器

test_planner_plugins.cpp 以GridBased.plugin参数切换规划插件，用同一套测试逻辑覆盖仓库中的五种全局规划器：

• nav2_navfn_planner::NavfnPlanner
• nav2_smac_planner::SmacPlanner2D
• nav2_smac_planner::SmacPlannerHybrid
• nav2_smac_planner::SmacPlannerLattice
• nav2_theta_star_planner::ThetaStarPlanner

测试维度包括：

1. 路径有效性：起点/终点重合（length=0）、极短路径（0.00001）、低于栅格分辨率（0.09）、临界分辨率（0.102）、高于分辨率（1.5）五档距离；
2. 末端朝向语义：GridBased.use_final_approach_orientation=false时路径末点朝向应等于目标朝向；为true时末点朝向应等于接近方向（单点路径则退化为起点朝向）；
3. 取消机制：注入always_cancelled取消回调并设GridBased.terminal_checking_interval=1，断言抛出nav2_core::PlannerCancelled；
4. 插件名合法性：空插件名返回空路径（frame_id 仍为map），不存在的插件名fake抛出nav2_core::InvalidPlanner（消息"Planner id fake is invalid"）。

这组测试说明PlannerTester体系天然支持"插件无关"的回归验证——新增全局规划插件时，只需像上述测试一样指定其plugin名称即可复用全部判据。

九、构建与运行方式

9.1 测试目标与依赖

CMakeLists.txt 定义四个测试目标：

链接依赖包括nav2_planner::planner_server_core、nav2_map_server::map_io、nav2_util::nav2_util_core、nav2_msgs、nav2_ros_common、nav_msgs、rclcpp_lifecycle、tf2_ros等，与上文分析的架构一致。

9.2 环境变量与 Launch 入口

两个集成测试通过 launch 文件驱动（test_planner_random_launch.py、test_planner_costmaps_launch.py），执行时注入两个环境变量：

• TEST_EXECUTABLE：测试二进制路径（由 CMake 以$<TARGET_FILE:...>注入）；
• TEST_MAP：地图图片路径（由 CMake 以${PROJECT_SOURCE_DIR}/maps/map.pgm注入，即 nav2_system_tests/maps 下的地图）。

启动命令为cmd=[testExecutable, '--ros-args -p use_sim_time:=True']，即测试以仿真时钟模式运行。

9.3 手动运行示例

在已构建的 ROS 2 工作空间中，可单独运行对应测试：

# 运行六种内置代价地图的单次路径测试 TEST_MAP=$(pwd)/maps/map.pgm \ ./test_planner_costmaps_node --ros-args -p use_sim_time:=True # 运行 100 次随机起终点回归测试 TEST_MAP=$(pwd)/maps/map.pgm \ ./test_planner_random_node --ros-args -p use_sim_time:=True # 运行多插件（NavFn / Smac2D / SmacHybrid / SmacLattice / Theta*）回归测试 ./test_planner_plugins # 运行 IsPathValid 服务语义测试 ./test_planner_is_path_valid

所有测试退出码为 0 即代表全部断言通过。

十、已知限制与注意事项

结合 README 的两条注释与源码中的 TODO 标记，使用该测试体系时需明确以下前提：

1. 机器人尺寸为 1x1 栅格，代价地图不做障碍膨胀：碰撞检测是"路径点所在栅格是否空闲"级别的判断，未考虑真实机器人足迹。因此该测试验证的是规划算法本身的正确性，不代表带足迹/膨胀的真实导航场景，后者需依赖 nav2_costmap_2d 的膨胀层与足迹碰撞检查（IsPathValid的footprint参数可在此方向上补充覆盖）；
2. NavFn 算法偶发失败：部分起点/终点组合下规划器返回空路径或路径切角进入障碍（源码注释指出"navfn 会偶尔切角"），这正是随机测试引入acceptable_fail_ratio=0.1与 3 次重试的原因；
3. 朝向支持不完整：defaultPlannerTest系列目前只比较位置（x/y），不校验起点/终点朝向，源码以TODO(orduno) #443标记了"支持考虑机器人朝向的规划器"这一扩展方向；
4. 随机测试不适用于内置测试代价地图：defaultPlannerRandomTests对using_fake_costmap_场景直接返回 false 并提示"尚未实现"，随机测试需配合真实地图使用。

结语：把 PlannerTester 用于你的全局规划器验证

PlannerTester的价值在于它把"世界构造—规划请求—质量断言"封装成可复用的测试骨架：世界层面可用六种内置代价地图或TEST_MAP真实地图，规划层面直接驱动生产级PlannerServer，校验层面覆盖碰撞、起终点一致性、IsPathValid全参数语义与多插件回归。对 Navigation2 的贡献者而言，新增或修改全局规划插件后，跑一遍 test_planner_plugins 即可获得跨五类规划器的行为基线；对二次开发者而言，也可以参照 PlannerTester 的接口，为自己的规划器搭建同类组件级回归测试。

• 机器人
• ROS
• 自动驾驶