Nav2自定义Behavior插件从零实现与避坑指南

最近在搞导航任务时总需要在行为树里塞一些自定义逻辑比如到达目标点后要查询外部服务、绕障完成后要上报状态、或者根据业务侧下发的一个字符串去切换不同的导航模式。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里加上这几个关键依赖dependrclcpp/depend dependbehaviortree_cpp_v3/depend dependnav2_behavior_tree/depend dependpluginlib/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::InputPortstd::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.registerNodeTypemy_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_namesNav2 的 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_nodepluginlib 就会因为找不到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_executeMainTree BehaviorTree IDMainTree Sequence nameroot_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_executeMainTree BehaviorTree IDMainTree Sequence nameroot_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.registerNodeTypemy_behavior_nodes::ReportArrival(ReportArrival); auto tree factory.createTreeFromText(R( root main_tree_to_executeMainTree BehaviorTree IDMainTree 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 RUNNINGhalt()却是空的结果一旦更高优先级的分支出现节点虽然被中断了底层的传感器回调还在跑造成数据错乱。正确的halt()至少要做两件事标记停止标志位、取消正在等待的异步操作。5.5 在自定义节点中使用 ROS2 通信的正确姿势自定义 BT 节点并不是独立 ROS2 节点它运行在 bt_navigator 进程里。想要在节点里发布话题、调用服务常规做法是从黑板里取出 bt_navigator 的节点句柄。在tick()或者构造函数里这样拿node_ config().blackboard-getrclcpp::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 注册这两件事拆开理解再配合一套正确的验证顺序整个流程其实半小时就能走通。希望这篇文章能帮你绕开我当初踩过的那些坑。