最近在搞导航任务时，总需要在行为树里塞一些自定义逻辑，比如到达目标点后要查询外部服务、绕障完成后要上报状态、或者根据业务侧下发的一个字符串去切换不同的导航模式。Navigation2 本身就提供了大量 Behavior 节点，但业务逻辑千奇百怪，官方节点根本不可能全覆盖。于是“自己写 Behavior 插件”就成了绕不开的一个坎。
我第一次想给 Navigation2 加自定义节点时，最大的困惑是：网上教程要么只讲 BehaviorTree.CPP 的节点写法，要么只讲 pluginlib 的插件机制，很少有人把“如何让 Navigation2 在运行时加载你自己的行为节点”这条链路完整讲清楚。这篇文章就把我自己在 Humble 版本上从零写 Behavior 插件、编译、配置、再跑进行为树里的完整过程和踩坑记录整理出来，适合已经跑通 Nav2 基础导航、想深度定制行为树的 ROS2 开发者。
1. 先把“Behavior 插件”的运行机制搞清楚
开始写代码之前，得先理解一件事：Navigation2 里的“行为树节点”和你平时写的普通 ROS2 节点不是一回事。这个理解如果不提前建立，代码写起来很容易走偏。
1.1 Behavior 节点在 Navigation2 里的角色
Navigation2 的整个任务调度核心是行为树（Behavior Tree），而行为树里的每一个叶子节点就叫 Behavior 节点。它们不是独立进程，也不是单独的 ROS2 Node，而是被 bt_navigator 这个节点加载进同一进程里的算法单元。每 tick 一次，节点执行一次tick()，返回 SUCCESS、FAILURE 或 RUNNING，父节点根据返回值决定下一步。
这个设计最大的好处是逻辑可编排。比如“导航到目标点”这个任务，在 Nav2 里被拆成了“计算路径”“控制速度”“检查目标是否到达”等多个节点，再用 Sequence、Fallback 这些控制节点把它们串起来。如果你想在某个环节插入自己的逻辑，唯一的正规做法就是写一个自己的 Behavior 节点，然后注册进行为树。
1.2 两套机制协同：pluginlib 负责加载，BT.CPP 负责注册
这里有个特别容易混淆的点。Navigation2 的节点插件并不是单纯用 pluginlib 那套“继承某个接口、到处导出宏”的机制，它是两套机制叠加：
- pluginlib 负责把动态库文件加载进当前进程，让 Nav2 能找到你的库；
- BehaviorTree.CPP（也就是 behaviortree_cpp_v3 包）负责把库里的节点类注册到它内部的工厂（BehaviorTreeFactory）里，注册之后行为树 XML 里才能用你起的节点名。
所以完整的链路是：编译出.so动态库 → Nav2 通过plugin_lib_names参数让 pluginlib 把这个库加载进来 → 库加载时执行BT_REGISTER_NODES(factory)里的注册逻辑 → 行为树 XML 解析时按节点名去找工厂里注册过的类 → 实例化并执行。
我见过有人只写了 pluginlib 的导出，却忘了在.cpp里加BT_REGISTER_NODES，结果插件库明明加载成功了，行为树解析时却报 “Node Not Registered”。这两个动作缺一不可，后面写代码时你会看到它们各自出现在什么位置。
2. 从零写一个自定义 Behavior 节点
我建议你先建一个独立的功能包，不要直接去改 nav2_behavior_tree 的源码。独立包编译快、隔离好，以后换版本也方便。包名我用my_behavior_nodes做示例，你改成自己的包名就行。
2.1 工程目录和依赖准备
先创建包，这里选 ament_cmake 类型：
ros2 pkg create --build-type ament_cmake my_behavior_nodes然后在package.xml里加上这几个关键依赖：
<depend>rclcpp</depend> <depend>behaviortree_cpp_v3</depend> <depend>nav2_behavior_tree</depend> <depend>pluginlib</depend>这里简单解释一下为什么需要nav2_behavior_tree。虽然你写的逻辑不一定要用到 Nav2 的东西，但 Nav2 的行为树节点插件都是从nav2_behavior_tree包统一加载的，依赖它主要是为了拿到行为树插件相关的头文件和编译期配置，省去很多自己折腾 include 路径的时间。
2.2 头文件：节点类的声明
我习惯在include/my_behavior_nodes/下放头文件。以一个“到达目标后调用外部服务上报”的节点为例，头文件长这样：
#ifndef MY_BEHAVIOR_NODES__REPORT_ARRIVAL_NODE_HPP_ #define MY_BEHAVIOR_NODES__REPORT_ARRIVAL_NODE_HPP_ #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" #include <string> namespace my_behavior_nodes { class ReportArrival : public BT::ActionNodeBase { public: explicit ReportArrival( const std::string & name, const BT::NodeConfiguration & conf); static BT::PortsList providedPorts(); BT::NodeStatus tick() override; void halt() override; private: std::string report_topic_; }; } // namespace my_behavior_nodes #endif几个要点说一下：
- 基类选
BT::ActionNodeBase，这是 Nav2 官方节点普遍采用的基类。如果你的节点是纯同步逻辑，执行完马上返回结果，也可以继承BT::SyncActionNode，那个更轻量，不需要实现halt()。但我这里要演示的节点涉及 ROS2 通信，选ActionNodeBase更典型。 providedPorts()是静态方法，用来声明这个节点有哪些输入输出端口。它会被 BT.CPP 在工厂注册时调用，行为树 XML 里写了什么端口、类型对不对，解析阶段就会校验。tick()是每次执行的核心。halt()是节点被更高优先级打断时调用的停止函数。
2.3 源文件：核心逻辑与注册宏
源文件放在src/report_arrival_node.cpp，完整实现如下：
#include "my_behavior_nodes/report_arrival_node.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "rclcpp/rclcpp.hpp" namespace my_behavior_nodes { ReportArrival::ReportArrival( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf) { getInput("report_topic", report_topic_); } BT::PortsList ReportArrival::providedPorts() { return { BT::InputPort<std::string>("report_topic", "topic for arrival report") }; } BT::NodeStatus ReportArrival::tick() { if (report_topic_.empty()) { RCLCPP_ERROR(rclcpp::get_logger("ReportArrival"), "report_topic is empty"); return BT::NodeStatus::FAILURE; } RCLCPP_INFO( rclcpp::get_logger("ReportArrival"), "Arrival reported to topic: %s", report_topic_.c_str()); // 这里可以放发布消息、调用服务等业务逻辑 return BT::NodeStatus::SUCCESS; } void ReportArrival::halt() { RCLCPP_INFO(rclcpp::get_logger("ReportArrival"), "ReportArrival halted"); } } // namespace my_behavior_nodes BT_REGISTER_NODES(factory) { factory.registerNodeType<my_behavior_nodes::ReportArrival>("ReportArrival"); }这里面最关键的三行就是文件末尾的BT_REGISTER_NODES(factory)块。这个宏是 BehaviorTree.CPP v3 提供的静态注册机制，它会在动态库被加载进进程时自动执行，把ReportArrival这个类型注册成行为树里可用的节点名。如果你希望节点在 XML 里叫其它名字，比如MyArrivalReporter，只需要改registerNodeType里的字符串。注意这个名字是全局唯一的，多个插件库里如果注册了相同名字，会在加载时报冲突。
2.4 端口（Ports）的声明与读取细节
端口是行为树节点和其他节点传数据的主要方式。我上面只声明了一个输入端口report_topic，类型是std::string。在 XML 里你可以直接给它写死一个值：
<ReportArrival report_topic="/arrival_report"/>也可以从黑板上订阅别的节点输出的变量：
<ReportArrival report_topic="{report_target_topic}"/>花括号表示从黑板（Blackboard）里读取。但是要注意，用户直接写死字符串时，BT.CPP 也会把它存到节点的输入端口里，所以getInput的返回值仍然为 true。
读取端口时最好检查返回值：
if (!getInput("report_topic", report_topic_)) { RCLCPP_ERROR(rclcpp::get_logger("ReportArrival"), "Failed to get input"); return BT::NodeStatus::FAILURE; }我给report_topic_设了默认空字符串的兜底，其实更稳妥的是把端口读取放在tick()里而不是构造函数里。因为构造函数只执行一次，如果运行过程中黑板上这个值被改了，构造时读到的还是旧值。我自己的习惯是：固定配置放构造函数读，动态变化的数据放tick()里读。
3. 构建配置：把节点编译成可加载的插件库
写完了代码，接下来要保证编译出来的不是普通可执行文件，而是动态库，同时还要让 pluginlib 能通过包名和库名找到这个库。
3.1 CMakeLists.txt 的完整写法
下面是一个能直接用的CMakeLists.txt，我删掉了大部分模板注释，保留了核心内容：
cmake_minimum_required(VERSION 3.8) project(my_behavior_nodes) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(pluginlib REQUIRED) add_library(report_arrival_action_bt_node SHARED src/report_arrival_node.cpp ) target_include_directories(report_arrival_action_bt_node PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include>) ament_target_dependencies(report_arrival_action_bt_node rclcpp behaviortree_cpp_v3 nav2_behavior_tree pluginlib ) install(TARGETS report_arrival_action_bt_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) install(DIRECTORY include/ DESTINATION include) ament_export_include_directories(include) ament_export_libraries(report_arrival_action_bt_node) ament_package()这里有个命名习惯值得注意：我用了report_arrival_action_bt_node这种库名，而不是随便叫my_lib。Nav2 官方的行为树插件库都是这种xxx_action_bt_node/xxx_condition_bt_node的命名风格，好处是一眼能看出这个库是行为树插件，而且后面配置plugin_lib_names时，库名就是这里add_library起的名字。
编译时如果提示找不到behaviortree_cpp_v3，通常说明你的环境里没有安装这个包。Ubuntu 上一般已经随 Nav2 装了，如果没有，可以手动安装：
sudo apt install ros-${ROS_DISTRO}-behaviortree-cpp-v33.2 编译并验证插件库是否生成
构建命令和其他 ROS2 包完全一样：
cd ~/your_ws colcon build --packages-select my_behavior_nodes source install/setup.bash构建完成后，先确认.so文件确实被放到了install/my_behavior_nodes/lib/目录下：
ls -la install/my_behavior_nodes/lib/如果你只想快速验证插件能否被 pluginlib 正常找到，可以用ros2 pkg prefix和库文件路径结合着查，但我个人更推荐直接用一个极简的加载测试，在 4.3 节里会讲。
3.3 关于nav2_behavior_tree的版本兼容性问题
nav2_behavior_tree这个包在不同版本里 API 有一点点差异。Humble 和 Foxy 版本大体一致，我用的是 Humble，没遇到什么大坑。但如果你用的是 Galactic，或者后来迁移到 Iron 这种更新版本，编译报错时优先怀疑头文件路径和依赖命名变化，不要一上来就怀疑自己的代码逻辑。
之前有个朋友用 Iron 版编译同一个包，报错说找不到behaviortree_cpp_v3/behavior_tree.h。后来发现是 Iron 的 Navigation2 已经切到了 BehaviorTree.CPP 4.x 版本，头文件变成behaviortree_cpp/behavior_tree.h了，宏和基类命名也更了。碰到这种情况，别硬改代码，先确认你机器上apt list --installed | grep behaviortree装的是什么版本，再决定适配方向。
4. 让 Navigation2 真正加载你的插件
这一步才是让很多人卡住的地方：代码写好了，库也编译出来了，但 Nav2 就是不加载，或者加载了却找不到节点。
4.1 修改 nav2_params.yaml 中的 plugin_lib_names
Nav2 的 bt_navigator 节点启动时会读一个参数叫plugin_lib_names，它是一个字符串数组。你需要在你的导航参数文件（通常是nav2_params.yaml）里，找到bt_navigator的ros__parameters段，把自己的插件库加进去：
bt_navigator: ros__parameters: plugin_lib_names: - nav2_behavior_tree::nav2_back_up_action_bt_node - nav2_behavior_tree::nav2_drive_on_heading_bt_node # 其他官方节点... - my_behavior_nodes::report_arrival_action_bt_node注意这个格式是包名::库名。包名是package.xml里的包名，库名是CMakeLists.txt里add_library指定的名字。如果你写成my_behavior_nodes::my_behavior_nodes，而实际库名是report_arrival_action_bt_node，pluginlib 就会因为找不到libmy_behavior_nodes.so报错。我在这里踩过一次坑，后来学聪明了：每次写完插件先到install/<包名>/lib/看一眼实际生成的库名，再回填到 yaml 里。
4.2 在行为树 XML 中使用你自己的节点
插件库加载成功后，还要让你的行为树文件里真的用到这个节点。Nav2 默认的default_bt_xml_filename指向官方那个复杂的navigate_w_replanning_and_recovery.xml，你可以先复制一份出来，在需要的位置插入自己的节点。
我举个例子，把“到达目标后上报”插到“导航到目标”和“检查目标到达”之间：
<root main_tree_to_execute="MainTree"> <BehaviorTree ID="MainTree"> <Sequence name="root_sequence"> <ComputePathToPose goal="{goal}" path="{path}"/> <FollowPath path="{path}"/> <IsGoalReached/> <ReportArrival report_topic="/arrival_report"/> </Sequence> </BehaviorTree> </root>这里关键点是你自定义节点在 XML 里的名字，必须和BT_REGISTER_NODES里registerNodeType的字符串完全一致。不少人在这一步发现插件库加载成功，但 Nav2 日志刷一堆 XML 解析错误，检查下来就是把ReportArrival写成了ReportArrivalNode这种名字不一致的问题。
如果你修改的default_bt_xml_filename不是官方那个完整树，建议先拿一个最小树跑通。比如：
<root main_tree_to_execute="MainTree"> <BehaviorTree ID="MainTree"> <Sequence name="root_sequence"> <ReportArrival report_topic="/arrival_report"/> </Sequence> </BehaviorTree> </root>确认这个最小树里节点能正常返回 SUCCESS，再往完整树里加其它节点，排查难度会小很多。
4.3 不启动 Nav2 也能快速验证插件
有时候为了一个节点去完整启动 Nav2 和仿真环境太痛苦了。我常用一个更简单的验证方法：直接用 BT.CPP 的命令行工具跑一棵极简的行为树。BehaviorTree.CPP 包安装后会带一个bt示例程序，但不是所有发行版都有。更通用的做法是写一个极简测试脚本或程序，只做三件事：创建工厂、加载动态库、解析 XML。
如果你不想写程序，还有一个偷懒的土办法：打开一个终端，用ros2 run nav2_bt_navigator bt_navigator这样直接起 bt_navigator 是没用的，因为它依赖很多参数。我建议你还是写个 20 行的测试程序：
#include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/behavior_tree.h" #include <iostream> int main() { BT::BehaviorTreeFactory factory; // 省略 pluginlib 加载插件库的细节，直接验证节点逻辑时可以 // factory.registerNodeType<my_behavior_nodes::ReportArrival>("ReportArrival"); auto tree = factory.createTreeFromText(R"( <root main_tree_to_execute="MainTree"> <BehaviorTree ID="MainTree"> <Sequence> <ReportArrival report_topic="/test"/> </Sequence> </BehaviorTree> </root> )"); auto status = tree.tickRoot(); std::cout << "Status: " << BT::toStr(status) << std::endl; return 0; }这个方式可以在一两秒内跑完核心逻辑，适合日常开发调试，等节点本身稳了再丢回 Nav2 里联调。
5. 常见问题与排查技巧实录
写插件这事本身不难，难的是出了问题不知道从哪里排查。下面这些坑都是我实际碰到过、或者帮别人排查时反复遇到的，整理成速查表，建议收藏。
5.1 插件库加载失败
现象：bt_navigator 启动后日志出现类似：
[bt_navigator]: Failed to load library: libreport_arrival_action_bt_node.so原因和解决办法：
- 最常见的是
plugin_lib_names里包名或库名写错。去install/<包名>/lib/下确认实际库文件名，然后按包名::库名格式回填。 - 可能忘了 source 当前工作空间。这个错误很蠢但很常见，尤其是你有多个终端时，其中一个终端一直用的是老的
install/setup.bash。 - 依赖库找不到。用
ldd install/my_behavior_nodes/lib/libreport_arrival_action_bt_node.so | grep "not found"查一下，如果有 not found，说明某些依赖没有被正确加载到环境里，重新source一下 Nav2 的安装环境，或者检查是否存在多个 ROS2 发行版的环境混用。
5.2 库加载了但节点未注册
现象：日志里插件库加载成功，没有任何报错，但解析行为树 XML 时报：
Behavior Tree Node 'ReportArrival' not registered or is a custom node原因：.cpp文件里漏写了BT_REGISTER_NODES(factory)块。这个宏是节点能被行为树识别的关键，只编译成动态库、只写 pluginlib 导出都解决不了问题。检查一下注册块里节点名和 XML 里的名字是否完全一致，包括大小写。
5.3 tick 异常：一直返回 FAILURE 或直接不执行
现象：节点被调用了，但结果不对。
排查顺序：
- 先看
getInput是否成功。输入端口没取到值会直接导致你逻辑走不下去，建议在getInput失败时打 ERROR 日志并返回 FAILURE。 - 检查
tick()里是否在某个条件分支下忘记 return。C++ 里函数结尾没有 return 是未定义行为，有时候会随机返回一个错误状态。 - 确认你返回的
BT::NodeStatus和父节点类型匹配。在 Sequence 下，子节点返回 FAILURE 会导致整个序列中断，这不是 bug，是行为树逻辑本身就该这样。 - 如果节点阻塞了、卡在某个循环里不返回，往下检查是不是用了同步等待回包的 ROS2 调用，而超时时间设置过长。
5.4 RUNNING 和 halt 没有正确配合
自定义异步节点最隐蔽的问题就是 RUNNING 状态处理。tick()返回RUNNING表示任务没有结束，行为树会继续 tick 这个节点，直到它返回 SUCCESS 或 FAILURE。当我打断一个 RUNNING 的节点时，框架会调用halt()，你需要在这里把正在进行的资源、回调、状态机全部停下来。
我见过有人写了一个无限循环检测传感器状态的节点，tick()里一直 return RUNNING，halt()却是空的，结果一旦更高优先级的分支出现，节点虽然被中断了，底层的传感器回调还在跑，造成数据错乱。正确的halt()至少要做两件事：标记停止标志位、取消正在等待的异步操作。
5.5 在自定义节点中使用 ROS2 通信的正确姿势
自定义 BT 节点并不是独立 ROS2 节点，它运行在 bt_navigator 进程里。想要在节点里发布话题、调用服务，常规做法是从黑板里取出 bt_navigator 的节点句柄。
在tick()或者构造函数里这样拿：
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");拿到node_之后就能正常创建 publisher、client、subscriber 了。注意这个"node"是 Nav2 在启动行为树前就放进黑板里的键值，不同版本可能键名略有差异，如果拿不到，可以打印一下黑板上已有的键。这个方法我强烈建议掌握，因为以后你写各种定制节点几乎都离不开它。
5.6 修改代码后不生效
这个问题的原理很简单：.so库文件没被重新拷贝到 install 空间，或者进程加载的还是老进程。
我建议：
colcon build --packages-select my_behavior_nodes source install/setup.bash然后一定重启 bt_navigator 进程。如果你在跑完整的 Nav2 bringup，重启导航相关节点即可。有一种情况容易忽略：你用了多个终端，ros2 launch那个终端的环境没重新 source，导致它启动时仍然用旧的.so。检查一下进程打开的库路径：
lsof -p <bt_navigator_pid> | grep my_behavior_nodes这个命令能直接看到进程到底加载了哪个目录下的库文件，非常有效。
6. 我建议的开发工作流和进阶思路
如果你准备在 Navigation2 上长期做定制开发，我强烈建议套用下面这套工作流，能省下不少反复折腾的时间：
- 所有自定义行为节点放独立插件包，不要动 nav2_behavior_tree 源码。
- 用最小行为树做日常验证，跑通逻辑后再集成到完整导航树里。
- 每一步都确认一件事：是节点逻辑错了，还是插件加载错了。把“库被成功加载”和“节点被成功注册”当成两个独立的检查点。
- 在自定义节点里打足日志，但注意不要用
RCLCPP_INFO高频刷屏。行为树的 tick 频率可能非常高，有些节点几百毫秒就 tick 一次，如果每次 tick 都打大量日志，系统性能会被拖垮。我一般把高频日志打在调试级别，或者用条件判断控制输出频率。
如果你以后想把节点做得更优雅，可以考虑学习 BehaviorTree.CPP 的端口自动补全、装饰器自定义、Groot 可视化调试等进阶功能。Nav2 较新版本的 bt_navigator 还支持enable_groot_monitoring参数，可以在 Groot 里实时看到整棵树的执行状态。我当时把自定义节点接进 Nav2 后，第一件事就是用 Groot 看一眼节点在整棵树里的跑动情况，那种能看到自己写的节点真实参与导航决策的感觉，还是挺有成就感的。
在实际开发中，我最深的体会是：Navigation2 的自定义 Behavior 插件并没有多高的技术门槛，真正容易让人放弃的，是卡在“库能编译出来但 Nav2 不认”这类集成问题上。只要把 pluginlib 加载和 BT.CPP 注册这两件事拆开理解，再配合一套正确的验证顺序，整个流程其实半小时就能走通。希望这篇文章能帮你绕开我当初踩过的那些坑。
复杂度剖析)
