企业官网建设流程全解析

• 机器人
• ROS
• 自动驾驶

本篇文章以 nav2_system_tests/src/waypoint_follower/README.md 中记录的 Waypoint Follower 测试为核心，深入剖析该测试在 Navigation2（ROS 2 导航框架）中的设计思路、启动流程与全部测试用例，并结合nav2_waypoint_follower包源码与消息定义，讲解其底层实现原理。读完本文，你将掌握如何在仿真环境中搭建一套可复现的 Waypoint Follower 端到端测试，理解follow_waypoints动作服务器的完整工作流程，并能根据实际业务场景配置其参数与任务执行器插件。

一、测试目标：一个快速验证 Waypoint Follower 的集成测试

原文档对该测试的定义非常精炼：它通过实例化完整的导航栈，向 Waypoint Follower 发送请求，并在 turtlebot 地图中检查机器人能否依次成功到达 4 个预设航点。该测试属于nav2_system_tests包中的全系统（Full-System）集成测试。按照 nav2_system_tests/README.md 的定位，这类测试的主要目的是在高层面上验证整个导航系统协同工作正常——单元测试与子系统级测试则发生在各自算法所在的包内。

从 CMakeLists.txt 可以看到测试的注册方式：

nav2_add_test(test_waypoint_follower GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_case_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml )

关键点说明：

• 测试命令入口是test_case_launch.py，并通过TEST_DIR环境变量把测试目录传给启动脚本，供其定位tester.py执行器；
• BT_NAVIGATOR_XML指定了本次测试使用的行为树为navigate_to_pose_w_replanning_and_recovery.xml（位于 nav2_bt_navigator/behavior_trees/ 目录），即"带重规划与恢复机制"的导航行为树，确保单个航点导航失败时能够触发恢复行为；
• TIMEOUT 180表示整个测试最长执行 180 秒，超时即判定失败，用于防止机器人卡死时测试无限挂起。

测试由test_case_launch.py（仿真环境与导航栈启动）和tester.py（测试逻辑执行）两个文件组成，下面分别解析。

二、环境搭建：test_case_launch.py 的启动编排

test_case_launch.py 负责拉起一整套完整的仿真 + 导航环境，其启动内容可分为四个部分。

1. 仿真世界与机器人模型

测试使用nav2_minimal_tb3_sim包提供的资源：tb3_sandbox.sdf.xacro仿真世界、gz_waffle.sdf.xacro机器人模型，并通过GZ_SIM_RESOURCE_PATH环境变量将模型资源路径注入 Gazebo：

sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')

随后通过gz sim -r -s启动无界面（headless）仿真，并调用spawn_tb3.launch.py在(-2.0, -0.5)处、航向角为 0 生成机器人。同时启动robot_state_publisher发布机器人 URDF 描述的 TF 树。

2. 地图与参数重写

地图使用nav2_bringup包中的 tb3_sandbox.yaml（分辨率 0.05 m/cell，原点[-10, -10]）。导航参数则取自 nav2_params.yaml，并通过nav2_common的RewrittenYaml工具进行运行时重写：

configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites={}, value_rewrites={ 'KEEPOUT_ZONE_ENABLED': 'False', 'SPEED_ZONE_ENABLED': 'False', }, convert_types=True)

这里将 keepout 禁行区与 speed 限速区功能关闭，保证测试只聚焦于 Waypoint Follower 本身的导航行为，避免其他子系统干扰结论。

3. 导航栈整体启动

通过IncludeLaunchDescription引入 bringup_launch.py，并传入关键参数：

launch_arguments={ 'map': map_yaml_file, 'use_sim_time': 'True', 'use_composition': 'False', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True' }

autostart=True让生命周期管理器自动把各导航节点从unconfigured一路带到active，省去手动调用ManageLifecycleNodes服务的步骤；use_composition=False则让各节点以独立进程方式运行，便于测试过程观察和排错。

4. 挂载测试执行器

启动脚本的主函数通过launch_testing的LaunchTestService把tester.py作为测试动作挂载到启动描述中：

test1_action = ExecuteProcess( cmd=[os.path.join(os.getenv('TEST_DIR', ''), 'tester.py')], name='tester_node', output='screen') lts = LaunchTestService() lts.add_test_action(ld, test1_action)

这样导航栈启动完成后，tester.py随即开始执行测试逻辑，并在结束时把返回码反馈给测试框架。

三、测试执行器：tester.py 的测试逻辑

tester.py 定义了一个WaypointFollowerTest节点（ROS 节点名nav2_waypoint_tester），其职责包括：

• 创建follow_waypoints动作客户端：用于向nav2_waypoint_follower的follow_waypoints动作服务器发送目标；
• 发布初始位姿：通过initialpose话题发布PoseWithCovarianceStamped，并订阅amcl_pose话题等待定位收敛确认；
• 动态调整参数：通过/waypoint_follower/set_parameters服务在运行时切换stop_on_failure参数，以测试"失败即停止"与"失败继续"两种模式；
• 生命周期管理：测试结束时调用lifecycle_manager_nav2/manage_nodes服务请求SHUTDOWN，干净地关闭整套导航栈。

关键交互流程

主函数main()的执行流程如下：

1. 初始化rclpy后先time.sleep(10)，等待整个导航栈完成启动与生命周期激活；
2. 设置初始位姿(-2.0, -0.5)，最多重试 3 次（含首次），直到收到amcl_pose确认定位成功；
3. 依次执行下面的各类测试用例；
4. 最终调用shutdown()关闭动作客户端并请求整个导航栈 shutdown。

测试的核心交互方法run(block, cancel)展示了标准的 ROS 2 动作客户端调用模式：

action_request = FollowWaypoints.Goal() action_request.poses = self.waypoints send_goal_future = self.action_client.send_goal_async(action_request) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() ... get_result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, get_result_future)

其中block=False时发送目标后立即返回（用于抢占测试），cancel=True时在目标被接受后 2 秒主动调用cancel_goal_async（用于取消测试）。最终判定标准有两条：动作状态必须为GoalStatus.STATUS_SUCCEEDED，且返回结果中的missed_waypoints列表为空。

测试用例矩阵

tester.py中的航点坐标均位于 tb3_sandbox 地图（地图原点[-10, -10]，即地图中央为坐标原点附近的可导航区域）内：

值得注意的实现细节：run(True, False)返回成功后，测试断言assert result；而越界、stop_on_failure、取消等用例则用assert not result验证"预期失败"。测试对越界航点的错误码断言还直接引用了ComputePathToPose.Result().GOAL_OUTSIDE_MAP，说明 Waypoint Follower 会把底层ComputePathToPose动作的错误码透传到missed_waypoints中，这正是"从源码结构推断出的实现事实"的典型例证。

四、底层原理：waypoint_follower 如何执行航点任务

1. 动作服务器与消息定义

nav2_waypoint_follower包对外暴露两个动作服务器（见 waypoint_follower.cpp）：

• follow_waypoints：类型为 FollowWaypoints.action，接收PoseStamped[] poses；
• follow_gps_waypoints：类型为 FollowGPSWaypoints.action，接收geographic_msgs/GeoPose[] gps_poses。

FollowWaypoints.action的目标还包含number_of_loops（循环次数）和goal_index（起始索引，默认 0）。结果中除了missed_waypoints（未完成航点列表，元素类型为 WaypointStatus.msg）外，还定义了 4 个错误码：

NONE=0, UNKNOWN=600, TASK_EXECUTOR_FAILED=601, NO_VALID_WAYPOINTS=602, STOP_ON_MISSED_WAYPOINT=603

反馈消息只包含一个字段current_waypoint（当前正在处理的航点索引），客户端可据此感知任务进度。

2. 主循环：逐个航点驱动 navigate_to_pose

在on_configure阶段，节点创建navigate_to_pose动作客户端，将整个航点列表逐个转化为NavigateToPose目标发给nav2_bt_navigator（这也是测试中BT_NAVIGATOR_XML指定navigate_to_pose_w_replanning_and_recovery.xml的原因——每个航点本身就是一个标准的导航到姿态任务）。

主循环followWaypointsHandler的核心状态机逻辑（见 waypoint_follower.cpp）：

1. 取消检测：若收到取消请求，先async_cancel_all_goals取消当前navigate_to_pose目标，再terminate_all结束动作；
2. 抢占检测：若收到新目标（preempt），接受新目标、重置goal_index = 0，以新航点列表重新执行；
3. 发送新目标：new_goal标志为真时，把poses[goal_index]封装为NavigateToPose目标异步发送，并附带result_callback与goal_response_callback两个回调；
4. 发布反馈：每轮循环把goal_index写入feedback->current_waypoint并publish_feedback；
5. 结果分发：根据回调更新的current_goal_status_分支处理——SUCCEEDED时调用任务执行器插件的processAtWaypoint，FAILED/UNKNOWN时把该航点记入missed_waypoints；
6. 失败策略：若stop_on_failure为真，任一航点失败（含任务执行器失败）即以STOP_ON_MISSED_WAYPOINT或TASK_EXECUTOR_FAILED终止整个动作；否则打印日志并跳到下一个航点继续；
7. 循环控制：goal_index递增，当遍历完所有航点且current_loop_no == no_of_loops时succeeded_current返回成功；否则回绕到索引 0 并递增循环计数。

整个循环受params_->loop_rate（默认 20 Hz）节拍控制，并使用独立的MutuallyExclusive回调组 + 专用 executor 处理动作客户端回调，避免阻塞服务器主循环。

3. 任务执行器插件（Waypoint Task Executor）

到达每个航点后，节点会通过 pluginlib 加载waypoint_task_executor_plugin（默认wait_at_waypoint）并调用其processAtWaypoint(pose, index)。这为"到达指定位置后执行特定任务"（拍照、抓取、等待人工指令等）提供了扩展点，对应的插件接口定义在nav2_core::WaypointTaskExecutor。

默认插件 wait_at_waypoint.cpp 的行为是：读取wait_at_waypoint.waypoint_pause_duration参数，若为 0 则自动禁用自身（is_enabled_ = false），否则在到达航点后sleep_for指定的毫秒数。若插件任务执行失败且stop_on_failure为真，整个动作会以TASK_EXECUTOR_FAILED终止。

五、参数配置：从测试环境到生产环境

Waypoint Follower 的参数解析位于 parameter_handler.cpp，可在运行时通过set_parameters动态更新（这正是测试器setStopFailureParam的底层机制）。默认配置见 nav2_params.yaml：

waypoint_follower: ros__parameters: loop_rate: 20 # 主循环执行频率 (Hz)，参数校验要求 > 0 stop_on_failure: false # 单个航点失败时是否终止整个动作 introspection_mode: "disabled" waypoint_task_executor_plugin: "wait_at_waypoint" # 任务执行器插件 id wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" # 插件类名 enabled: true # 插件启用开关 waypoint_pause_duration: 200 # 到达航点后的停留时长 (ms)，为 0 则自动禁用插件

参数要点：

• loop_rate：主循环速率，默认 20 Hz；参数校验回调会拒绝小于等于 0 的整型更新（见 parameter_handler.cpp）；
• stop_on_failure：默认false（测试中用setStopFailureParam(True)临时开启以验证停止逻辑）；false时即使个别航点失败，也会继续处理剩余航点，最终把失败航点汇总到missed_waypoints返回；
• waypoint_task_executor_plugin与wait_at_waypoint.*：任务执行器的选择与参数化。

六、如何运行与扩展这套测试

1. 运行前提与方式

本测试依赖nav2_minimal_tb3_sim（Gazebo 仿真）与nav2_bringup，属于nav2_system_tests包的一部分。在完成依赖安装与仿真环境配置后，可通过 colcon 构建整个工作空间，然后运行：

colcon build --packages-select nav2_system_tests # 进入安装后的测试目录或通过 ctest 运行 ctest -R test_waypoint_follower

由于测试需要完整启动 Gazebo 与整个导航栈，建议在具备图形/GPU 或已配置 headless 仿真（测试启动脚本默认即-sheadless 模式）的 Linux 环境下运行，并保证180秒超时窗口内资源充足。

2. 扩展思路

• 更换地图与航点：修改 test_case_launch.py 中的map_yaml_file，并将tester.py中wps坐标改为目标地图内的可达坐标；
• 更换行为树：通过BT_NAVIGATOR_XML环境变量（见 CMakeLists.txt）切换nav2_bt_navigator/behavior_trees/下的其他行为树，验证不同恢复策略下的航点跟随表现；
• 覆盖更多失败场景：参考stop_on_failure用例，可新增对number_of_loops循环、抢占后清空航点、GPS 航点转换失败（需robot_localization的fromLL服务，见 waypoint_follower.cpp）等场景的断言。

七、总结

Waypoint Follower 系统测试虽然只有一句简短的 README 描述，但它背后是一套完整的全栈验证方案：通过test_case_launch.py编排仿真环境与导航栈，通过tester.py覆盖常规导航、抢占、取消、越界、失败停止等关键路径，并以follow_waypoints动作的结果与错误码作为系统健康度的判据。理解这套测试，既能帮助你在本地快速验证 Waypoint Follower 的可用性，也能为你设计其他导航能力（如FollowWaypoints任务执行器插件、GPS 航点跟随）的集成测试提供直接可复用的模板。

• 机器人
• ROS
• 自动驾驶