Nav2 Behaviors 行为服务器深度解析：TimedBehavior 模板、内置行为插件与实战配置-平芜编程栈

机器人
ROS
自动驾驶

【免费下载链接】navigation2

ROS 2 Navigation Framework and System

项目地址：https://gitcode.com/gh_mirrors/na/navigation2

本文以 ROS 2 Navigation2 框架中的nav2_behaviors包为对象，系统讲解集中式行为服务器的设计动机、TimedBehavior模板的执行模型、Spin/BackUp/DriveOnHeading/Wait/AssistedTeleop五类内置行为的工作原理，并结合仓库源码与nav2_bringup示例参数，给出可直接复制使用的配置与自定义行为插件的开发路径。读完本文，你将掌握行为服务器从参数配置、插件加载、动作接口到源码实现的全链路知识。

一、nav2_behaviors 是什么：行为即服务

在 Navigation2 的生态中，nav2_behaviors是一个专门用于"执行行为（Behaviors）"的任务服务器包。所谓行为，是指机器人在导航过程中需要临时执行的、短期的运动或状态原语，例如原地旋转（Spin）、后退脱困（BackUp）、沿当前朝向直线前进（DriveOnHeading）、原地等待（Wait）以及人工辅助遥控（AssistedTeleop）。这些行为通常由行为树（Behavior Tree）中的 Recovery 节点在导航异常时触发，用于让机器人从卡死、碰撞风险等状态中恢复。

从仓库结构看，该包的核心构成如下：

include/nav2_behaviors/timed_behavior.hpp：TimedBehavior模板，行为开发的最常用基类；
include/nav2_behaviors/behavior_server.hpp 与 src/behavior_server.cpp：行为服务器的生命周期节点实现，负责通过 pluginlib 动态加载行为插件；
plugins/：五个内置行为插件的具体实现（spin.cpp、back_up.cpp、drive_on_heading.cpp、wait.cpp、assisted_teleop.cpp）；
behavior_plugin.xml：插件的 pluginlib 注册清单；
test/test_behaviors.cpp：针对TimedBehavior基类的单元测试。

二、集中式行为服务器：为什么要共享资源

导航系统中，TF（坐标变换）订阅、代价地图（costmap）订阅等都是相当"昂贵"的资源：每个独立节点都会建立自己的订阅连接，消耗可观的计算与通信开销。如果每个行为都做成独立节点，机器人的资源开销会显著增加。

nav2_behaviors的集中式行为服务器正是为了解决这一问题而设计：把所有行为合并到一个服务器进程中，让它们共享 TF 缓冲、代价地图订阅等公共资源，同时每个行为在执行逻辑与对外接口上保持完全独立。这也是官方 README 中强调的核心价值——"share resources whilst retaining complete independence in execution and interface"。

这一设计在 src/behavior_server.cpp 的setupResourcesForBehaviorPlugins()中体现得淋漓尽致：服务器根据各行为插件通过getResourceInfo()声明的代价地图需求（NONE/LOCAL/GLOBAL/BOTH，定义见 nav2_core/include/nav2_core/behavior.hpp），按需创建唯一的本地/全局代价地图订阅器、足迹订阅器和CostmapTopicCollisionChecker碰撞检查器，再统一传给每个行为插件使用。例如Spin、DriveOnHeading、Wait、AssistedTeleop都声明只需要LOCAL代价地图（见各自的头文件getResourceInfo()实现），因此服务器只需创建一个本地代价地图订阅即可服务全部行为。

三、插件接口：一切行为的基石 nav2_core::Behavior

在 Nav2 中，插件体系遵循"核心接口 + pluginlib 动态加载"的经典模式。一个行为插件唯一必须继承的基类是 nav2_core/include/nav2_core/behavior.hpp 中定义的nav2_core::Behavior抽象类，它定义了行为服务器与插件之间的契约：

virtual void configure( const nav2::LifecycleNode::WeakPtr & parent, const std::string & name, nav2::TransformBuffer::SharedPtr tf, std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> local_collision_checker, std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> global_collision_checker) = 0; virtual void cleanup() = 0; virtual void activate() = 0; virtual void deactivate() = 0; virtual CostmapInfoType getResourceInfo() = 0;

接口只要求实现生命周期四个阶段（配置、清理、激活、去激活）以及资源需求声明。值得注意的是，行为并不强制要求是 Action 形式——如果你愿意，完全可以把行为建模成 Service 或其他接口。官方文档明确指出TimedBehavior只是"方便但不是必须"的辅助类。不过，绝大多数运动原语都是长时间运行的任务，天然适合建模为 ROS 2 Action（可取消、可反馈、可超时），因此仓库默认提供了TimedBehavior模板来简化这类行为的开发。

四、TimedBehavior 模板：定时行为引擎的源码解剖

include/nav2_behaviors/timed_behavior.hpp 是行为开发的核心模板类，它继承自nav2_core::Behavior，并内置了一个泛型 Action 服务器（nav2::SimpleActionServer<ActionT>）。其设计思想是：把"行为"抽象成两个用户回调，框架负责循环调度。

4.1 核心抽象：三个必须/可选的虚函数

virtual ResultStatus onRun(const std::shared_ptr<const typename ActionT::Goal> command) = 0; virtual ResultStatus onCycleUpdate() = 0; virtual void onConfigure() {} virtual void onCleanup() {} virtual void onActionCompletion(std::shared_ptr<typename ActionT::Result> result) {}

onRun(command)：在动作开始时只调用一次，用于解析 Goal、做前置校验（如 TF 是否可用、输入是否合法），返回SUCCEEDED才会进入主循环，否则整个行为直接以失败终止；
onCycleUpdate()：主循环的核心，以cycle_frequency的频率被循环调用，每次执行一个单位的"工作"（例如计算一次速度指令），返回RUNNING则继续，返回SUCCEEDED则行为成功结束，返回FAILED则行为失败并携带错误码与错误信息；
其余三个虚函数是可选的挂钩点，分别用于配置阶段、清理阶段和动作结束时的善后处理。

4.2 状态与结果类型

模板定义了执行状态枚举和结果结构体：

enum class Status : int8_t { SUCCEEDED = 1, FAILED = 2, RUNNING = 3, }; struct ResultStatus { Status status; uint16_t error_code{0}; std::string error_msg; };

error_code与各动作消息中定义的错误码常量对应（详见后文"动作接口与错误码"小节），error_msg提供人类可读的错误描述，二者都会在行为失败时写入 Action Result 中返回给调用方。

4.3 主执行循环的完整流程

模板的execute()方法（timed_behavior.hpp）实现了完整的动作执行状态机，其流程如下：

活性检查：若服务器处于未激活状态（enabled_为 false），直接警告并忽略请求；
调用onRun：失败则立即以terminate_current结束动作并返回错误；
计时开始：记录start_time，创建nav2::Rate loop_rate(node, cycle_frequency_)控制循环频率；
循环处理（while (rclcpp::ok())）：
- 更新elapsed_time_；
- 检查抢占请求：当前版本尚未实现抢占（源码中留有TODO(orduno) #868注释），收到抢占请求会记录错误、stopRobot()停止机器人并终止动作；
- 检查取消请求：收到取消则停止机器人、记录已耗时、终止所有动作；
- 调用onCycleUpdate()并按其返回状态分发：SUCCEEDED→ 记录total_elapsed_time、调用onActionCompletion、succeeded_current；FAILED→ 写入错误码与错误信息、终止动作；RUNNING→loop_rate.sleep()后进入下一轮。

4.4 生命周期管理与公共资源

configure()阶段会从参数服务器读取cycle_frequency、local_frame、global_frame、robot_base_frame、transform_staleness_threshold（默认 0.0），创建以行为名命名的 Action Server，并创建nav2_util::TwistPublisher（发布到cmd_vel话题）。activate()/deactivate()会同步激活/去激活速度发布器与动作服务器。模板还提供了两个开箱即用的受保护工具：

getCurrentPoseChecked()：在本地坐标系下获取"新鲜"的机器人位姿，若 TF 缺失或超时（超过transform_staleness_threshold）则返回 false 且不改写位姿——所有依赖位姿的行为都用它做前置检查；
stopRobot()：发布一个全零速度指令（linear.x/y = 0, angular.z = 0，frame 为robot_base_frame），用于超时、取消、碰撞等场景下的紧急制动。

4.5 测试印证

test/test_behaviors.cpp 中定义了一个继承TimedBehavior<DummyBehaviorAction>的DummyBehavior测试类：onRun根据命令字符串模拟成功/失败初始化，onCycleUpdate模拟 1 秒的"运动时长"，正好验证了"onRun 单次初始化 + 主循环周期性更新 + 三态返回"这套执行模型。官方 README 也提到TimedBehavior内部使用了 nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities 中的nav2_util::TwistPublisher。

五、行为服务器：插件加载与生命周期管理

5.1 节点与默认插件

src/behavior_server.cpp 中的behavior_server::BehaviorServer是一个继承自nav2::LifecycleNode的组件节点（通过RCLCPP_COMPONENTS_REGISTER_NODE注册为 ROS 组件，可被 Composition 容器加载）。构造函数中声明的默认插件列表为：

default_ids_ {"spin", "backup", "drive_on_heading", "wait"} default_types_{"nav2_behaviors::Spin", "nav2_behaviors::BackUp", "nav2_behaviors::DriveOnHeading", "nav2_behaviors::Wait"}

也就是说，默认启用四个行为；AssistedTeleop需要显式加入behavior_plugins列表才会被加载（这正是官方 README 专门说明其teleop_command_timeout参数的原因——它默认不在插件列表中）。

5.2 加载与配置流程

生命周期on_configure阶段依次执行：

创建 TF 缓冲与监听器（create_transform_buffer/create_transform_listener）；
loadBehaviorPlugins()：遍历behavior_plugins中声明的每个 ID，读取{id}.plugin参数得到具体类名（如nav2_behaviors::Spin），通过pluginlib::ClassLoader<nav2_core::Behavior>创建插件实例，任一插件创建失败都会导致配置失败；
setupResourcesForBehaviorPlugins()：按需创建本地/全局代价地图订阅器、足迹订阅器与碰撞检查器（见第二节）；
configureBehaviorPlugins()：逐个调用插件的configure(node, id, tf_, local_collision_checker_, global_collision_checker_)。

on_activate/on_deactivate阶段则遍历所有插件调用其activate()/deactivate()，同时创建/销毁 bond 连接（用于节点间活性监控）。

5.3 插件的 XML 注册

每个行为插件都通过PLUGINLIB_EXPORT_CLASS宏导出，并在 behavior_plugin.xml 中登记，例如：

<library path="nav2_spin_behavior"> <class type="nav2_behaviors::Spin" base_class_type="nav2_core::Behavior"/> </library>

新建自定义行为插件时，需要同样编写 XML 描述并在 CMake 中安装（pluginlib_export_plugin_description_file），服务器即可通过参数动态加载，无需改一行服务器代码。

六、内置行为插件逐个拆解

6.1 Spin：原地旋转

plugins/spin.cpp 实现了Spin行为，目标是通过target_yaw（弧度）旋转指定角度。它的关键点在onCycleUpdate()：

角度累加：通过current_yaw - prev_yaw_计算每帧增量，并做 ±π 的环绕处理（abs(delta_yaw) > M_PI时取补角方向），累加到relative_yaw_作为已转角度，同时发布angular_distance_traveled反馈；
三角速度剖面：剩余角度remaining_yaw接近 0（< 1e-6）即成功停车；否则按vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw)计算速度，并钳制在min_rotational_vel_与max_rotational_vel_之间，实现"减速逼近目标角"的平滑控制；
碰撞前瞻：isCollisionFree()按simulate_ahead_time_（默认 2.0s）和cycle_frequency_的步长，用本地代价地图碰撞检查器逐帧模拟旋转轨迹上的位姿，若前方有碰撞则停车并返回COLLISION_AHEAD错误；
超时保护：超过 Goal 中的time_allowance仍未完成则停车并返回TIMEOUT。

其参数在onConfigure()中读取，默认值分别为：simulate_ahead_time=2.0、max_rotational_vel=1.0、min_rotational_vel=0.4、rotational_acc_lim=3.2（单位为 rad/s 与 rad/s²）。

6.2 DriveOnHeading：沿朝向直线行驶

include/nav2_behaviors/plugins/drive_on_heading.hpp 实现了一个带加减速约束的直线行驶行为，目标target是一个geometry_msgs/Point（只允许 X 方向分量，Y/Z 非零会返回INVALID_INPUT）。实现要点：

速度规划：每周期根据上一周期速度last_vel_，按acceleration_limit_/deceleration_limit_（默认 2.5 / -2.5）计算可行速度区间并std::clamp，保证加减速平滑且可控；接近目标时按max_vel_to_stop = sqrt(-2 * deceleration_limit_ * remaining_distance)提前减速防超调；低于minimum_speed_（默认 0.10）时以最小速度兜底，避免低速死区；
方向一致性校验：onRun中要求target.x与speed同号，否则返回INVALID_INPUT；
碰撞前瞻：同样以simulate_ahead_time_步长沿行驶方向逐帧检查本地代价地图，碰撞则停车返回COLLISION_AHEAD；
成功判据：已行驶距离（hypot(diff_x, diff_y)，随distance_traveled反馈发布）达到|target.x|即停车成功。

6.3 BackUp：倒车脱困

include/nav2_behaviors/plugins/back_up.hpp 中BackUp类直接继承自DriveOnHeading<BackUpAction>，仅重写了onRun：它静默地把速度和目标方向强制转为负值（command_x_ = -fabs(target.x)、command_speed_ = -fabs(speed)），从而复用 DriveOnHeading 的整套加减速与碰撞检测逻辑，只允许沿 X 轴负方向后退。Y/Z 方向输入同样被拒绝（INVALID_INPUT）。这是模板与继承复用带来的典型收益：一个新行为往往只需要几十行代码。

6.4 Wait：定时等待

plugins/wait.cpp 是最简单的行为：onRun记录wait_end_ = now + goal.time；onCycleUpdate每周期发布剩余时间time_left反馈，剩余时间大于 0 返回RUNNING，否则返回SUCCEEDED。它不需要运动控制，主要用于导航流程中的停顿场景（如等待传感器数据、等待任务切换）。

6.5 AssistedTeleop：带碰撞保护的人工遥控

plugins/assisted_teleop.cpp 实现了"辅助遥控"行为，是五个插件中机制最丰富的一个，官方 README 也专门为其补充了说明：

遥操作输入：通过nav2_util::TwistSubscriber订阅cmd_vel_teleop话题（默认话题名cmd_vel_teleop），同时支持Twist与TwistStamped两种消息类型；当使用TwistStamped时，要求上游必须填充消息的 header 时间戳，否则新鲜度检查无法生效；
命令超时保护：一旦操作者开始驾驶（收到第一条指令），若在teleop_command_timeout（默认0.25s）内没有新的遥操作指令到达，行为判定"操作者松手或链路中断"，立即停车并以TELEOP_INPUT_TIMEOUT失败退出；将该参数设为0.0可关闭此检查，适用于指令稀疏的遥操作源（如低频手柄、网络遥控）；
碰撞投影：onCycleUpdate以simulation_time_step_（默认 0.1s）为步长、projection_time_（默认 1.0s）为窗口，用projectPose()对当前遥操作速度做前向运动学投影（含线性/角速度的姿态更新），逐帧在本地代价地图上检查碰撞；若第一个时间步就碰撞，则速度指令直接清零；若后续时间步碰撞，则按time / projection_time_比例缩放速度，实现"前方有障碍就自动减速"；
主动退出：订阅preempt_teleop话题（std_msgs/Empty），收到消息即判定遥控成功、停车并返回SUCCEEDED；
超时兜底：超过time_allowance未结束则停车返回TIMEOUT，并通过current_teleop_duration反馈实时上报已遥控时长。

七、动作接口与错误码：客户端视角

五个内置行为对应五个动作定义，全部位于 nav2_msgs/action/ 目录。它们的 Goal、Result 与 Feedback 结构如下：

行为	Goal 关键字段	Feedback	动作文件
Spin	`target_yaw`（float32，弧度）、`time_allowance`、`disable_collision_checks`	`angular_distance_traveled`	Spin.action
BackUp	`target`（Point，仅 X 有效）、`speed`、`time_allowance`、`disable_collision_checks`	`distance_traveled`	BackUp.action
DriveOnHeading	`target`（Point，仅 X 有效）、`speed`、`time_allowance`、`disable_collision_checks`	`distance_traveled`	DriveOnHeading.action
Wait	`time`（Duration）	`time_left`	Wait.action
AssistedTeleop	`time_allowance`	`current_teleop_duration`	AssistedTeleop.action

所有行为的 Result 都统一包含total_elapsed_time（实际耗时）、error_code与error_msg。disable_collision_checks为 true 时可跳过行为内部的代价地图碰撞检测（危险操作，仅用于特殊调试场景）。错误码按包分别编号（Spin 以 700 起、BackUp 710、DriveOnHeading 720、AssistedTeleop 730、Wait 740），常见语义包括NONE=0、GOAL_REJECTED=1、SEND_GOAL_FAILURE=2、TIMEOUT、TF_ERROR、COLLISION_AHEAD、INVALID_INPUT、TELEOP_INPUT_TIMEOUT等，客户端可直接依据错误码做恢复策略分支。

八、实战配置：nav2_params.yaml 全量示例

nav2_bringup自带的 params/nav2_params.yaml 给出了行为服务器的完整配置模板，五个行为全部启用：

behavior_server: ros__parameters: local_costmap_topic: local_costmap/costmap_raw global_costmap_topic: global_costmap/costmap_raw local_footprint_topic: local_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: plugin: "nav2_behaviors::Spin" simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 backup: plugin: "nav2_behaviors::BackUp" simulate_ahead_time: 2.0 acceleration_limit: 2.5 deceleration_limit: -2.5 minimum_speed: 0.10 drive_on_heading: plugin: "nav2_behaviors::DriveOnHeading" simulate_ahead_time: 2.0 acceleration_limit: 2.5 deceleration_limit: -2.5 minimum_speed: 0.10 wait: plugin: "nav2_behaviors::Wait" assisted_teleop: plugin: "nav2_behaviors::AssistedTeleop" projection_time: 1.0 simulation_time_step: 0.1 teleop_command_timeout: 0.25 local_frame: odom global_frame: map robot_base_frame: base_link transform_tolerance: 0.1

各参数的作用与默认值汇总如下（默认值取自 behavior_server.cpp 与各插件的onConfigure()）：

参数	默认值	说明
`cycle_frequency`	10.0	行为主循环更新频率（Hz），`TimedBehavior`的执行节奏
`behavior_plugins`	`[spin, backup, drive_on_heading, wait]`	启用的插件 ID 列表；`assisted_teleop`需显式加入
`{id}.plugin`	见默认类型	每个插件 ID 对应的具体 C++ 类名
`local_costmap_topic`/`global_costmap_topic`	`local_costmap/costmap_raw`/`global_costmap/costmap_raw`	代价地图订阅话题
`local_footprint_topic`/`global_footprint_topic`	`local_costmap/published_footprint`/`global_costmap/published_footprint`	足迹订阅话题
`local_frame`/`global_frame`	`odom`/`map`	TF 坐标系参考
`robot_base_frame`	`base_link`	机器人本体坐标系
`transform_tolerance`	0.1	足迹变换允许的容差（秒）
`transform_staleness_threshold`	0.0	位姿新鲜度阈值，0.0 表示不启用过期检查（`TimedBehavior::configure`读取）
`spin.simulate_ahead_time`	2.0	旋转碰撞前瞻模拟时长（秒）
`spin.max_rotational_vel`/`min_rotational_vel`	1.0 / 0.4	最大/最小旋转速度（rad/s）
`spin.rotational_acc_lim`	3.2	旋转加速度限制（rad/s²）
`backup`/`drive_on_heading`.acceleration_limit	2.5	直线加速度限制（m/s²）
`backup`/`drive_on_heading`.deceleration_limit	-2.5	直线减速度限制（m/s²，必须为负，源码会自动取绝对值修正符号）
`backup`/`drive_on_heading`.minimum_speed	0.10	最小行驶速度（m/s）
`assisted_teleop.projection_time`	1.0	碰撞投影前瞻时长（秒）
`assisted_teleop.simulation_time_step`	0.1	投影模拟步长（秒）
`assisted_teleop.teleop_command_timeout`	0.25	遥操作指令超时（秒），0.0 关闭检查
`assisted_teleop.cmd_vel_teleop`	`cmd_vel_teleop`	遥操作速度输入话题名

配置提醒：acceleration_limit与deceleration_limit必须分别为正、负值，源码在onConfigure中会检测并自动修正（std::abs处理）；cycle_frequency同时决定了碰撞前瞻模拟的分辨率（模拟步长为1 / cycle_frequency_）；teleop_command_timeout=0.0仅用于关闭稀疏遥操作源的超时检查，正常情况下建议保留默认值以保障安全。

九、编写自定义行为：最小实现模板

结合第三节与第四节的机制，编写一个新行为插件（例如"自定义的前后蠕动"）只需三步：

继承TimedBehavior<YourAction>，实现onRun（解析 Goal、前置校验、初始化位姿与终点）与onCycleUpdate（每周期计算速度、更新反馈、按状态机返回RUNNING/SUCCEEDED/FAILED），并按需覆写onConfigure（读取behavior_name_ + ".xxx"前缀参数）、getResourceInfo（声明代价地图需求）；
导出插件：文件末尾调用PLUGINLIB_EXPORT_CLASS(your_namespace::YourBehavior, nav2_core::Behavior)；
注册与配置：在包内新建your_behavior.xml（仿照 behavior_plugin.xml 格式），在 CMake 中安装并调用pluginlib_export_plugin_description_file，然后在behavior_server的behavior_plugins列表中追加 ID 并补上{id}.plugin参数即可，服务器与行为树调用方无需任何改动。

官方 README 进一步指出：行为甚至可以不依赖TimedBehavior——只要实现nav2_core::Behavior接口，建模为 Service 等其他交互形式也完全可行；TimedBehavior的价值在于帮你管理长时运行动作的复杂度（循环、超时、取消、停车、碰撞检查），这也是绝大多数运动原语的选择。

十、小结

nav2_behaviors通过"集中式行为服务器 + pluginlib 插件体系 +TimedBehavior定时执行引擎"三层设计，把 TF、代价地图等昂贵资源聚合共享，把 Spin、BackUp、DriveOnHeading、Wait、AssistedTeleop 等恢复与辅助行为统一纳入 Action 接口管理，并为开发者提供了极低门槛的自定义行为扩展路径。理解它的执行模型（onRun 单次初始化 + onCycleUpdate 周期推进 + 三态返回）、参数体系（服务器级与{id}.前缀的插件级两级参数）以及动作错误码约定，是进行导航恢复策略调优与二次开发的关键基础。