Navigation2自定义Behavior插件开发：从机制到实战

跑过Navigation2的同学迟早会遇到一个问题默认行为树里的节点不够用。比如我想让机器人在导航任务开始前检查一个“允许出站”的信号到了目标点后拍一张照片这些逻辑放在哪放Nav2的动作服务端里会侵入核心逻辑放在控制器插件里又不合适。最干净的做法是把这些业务逻辑写成Navigation2的Behavior插件在行为树XML里像CheckDoorOpen/一样直接被调用。这篇文章就是围绕“Navigation2如何创建自己的Behavior插件”这个主题把整个链路讲透插件机制的原理、基类怎么选、代码怎么写、plugin描述文件怎么配、导航栈怎么加载以及我实际开发中踩过的几个坑。1. Behavior插件到底是怎么跑起来的1.1 一条行为树节点的“加载链路”在动笔写代码之前我先花点时间把插件的运行机制讲清楚。Navigation2的BT Navigator在启动时会读取一个行为树XML文件XML文件里每个标签比如NavigateToPose、PipelineSequence最终都会对应到BehaviorTree.CPP库里面的一个C类实例。负责干这件事的是BehaviorTree.CPP的NodeFactory它会根据标签名去查找并实例化对应的节点类。那这些类是怎么“按名字找到”的呢这里就得说到ROS2的pluginlib机制。Navigation2把每个行为树节点都打包成了pluginlib插件插件注册信息会写进ament index。当BT Navigator启动时它会通过pluginlib把所有注册过的插件库尝试加载一遍把节点名和类的对应关系登记到NodeFactory里。XML解析器遇到一个标签就去这个登记表里查查到就实例化查不到就报错。理解这条链路很重要。很多人有一个误区以为只要代码能编译通过导航栈就能自动识别自定义节点。实际上从“代码编译通过”到“XML标签可用”中间还隔着插件描述文件、CMake导出宏、plugin_lib_names配置三座大山。后面我会逐一展开。1.2 为什么Navigation2要把行为节点做成插件插件化的好处在工程上非常明显。首先导航栈的核心开发者不需要关心你的业务逻辑你也不需要因为想加一个节点就去改Nav2的源码。团队协作时有人负责导航算法有人负责业务行为大家各自维护自己的功能包互不干扰。其次是复用的便利性。一个Navigation2的Behavior插件写好了编译生成的库可以直接被BT Navigator加载也能被behavior_server加载。你可以在不同项目、不同机器人上复用同一个节点库只需要在行为树XML里调整节点组合和参数就行。第三个好处是编排灵活。任务流程被写在XML里改流程不需要重新编译C代码只要把新XML文件路径配给导航栈重启即可。对现场调试、快速迭代来说这种“逻辑与流程分离”的架构非常舒服。1.3 内置节点已有哪些你缺的是哪一类Navigation2的nav2_behavior_tree包里已经内置了几十个节点按功能大致可以分这么几类。节点类型代表节点职责动作节点NavigateToPose、FollowPath、BackUp、Spin、Wait执行具体的导航动作规划节点ComputePathToPose、ComputePathThroughPoses计算路径但不执行条件节点IsStuck、GoalReached、IsGoalUpdated、InitialPoseReceived判断某个条件是否满足控制节点PipelineSequence、RecoveryNode、FallbackNode控制子节点的执行逻辑装饰节点RateController、DistanceController、SpeedController控制子节点的执行频率或条件内置节点再全也不可能覆盖每个项目的业务需求。比如我想让机器人到点之后自动呼叫电梯、自动开启充电桩这些都得自己写。写的时候先明确一个事你缺的是“动作型”节点还是“条件型”节点。判断标准很简单这个节点是“做一件事”还是“判断一个状态”。做事的用动作节点判断的用条件节点。2. 动手前先做两个决定2.1 选基类同步节点还是Action型节点Navigation2的Behavior插件可以继承的基类主要有这么几个每个的适用场景差别很大。基类特点适用场景BT::SyncActionNode同步执行tick里做完事直接返回SUCCESS或FAILURE几毫秒到几十毫秒能完成的逻辑如检查信号、调用轻量服务BT::ConditionNode条件判断节点不产生副作用树分支判断如“是否到达目标点”BT::ActionNodeBase需要自己维护RUNNING状态手动管理onStart等回调复杂状态机但开发成本高nav2_behavior_tree::BtActionNodeT模板类封装了ROS2 Action客户端tick时异步发送Action goal长时间异步动作如机械臂动作、回充、自定义导航需求我的建议是第一次开发自定义Behavior插件优先用BT::SyncActionNode。原因很简单同步节点没有复杂的RUNNING状态管理也不涉及Action server的启停逻辑能把“插件注册链路”单独抽出来验证。等你跑通了一个最小同步插件再去碰BtActionNode心里的底气会完全不一样。2.2 用端口和黑板设计节点的输入输出BehaviorTree.CPP里面有两个概念必须搞清楚端口Port和黑板Blackboard。端口是节点的输入输出接口。在providedPorts()静态函数里声明比如声明一个service_timeout_ms输入端口XML里就能这么写CheckService service_timeout_ms500/端口的取值可以是一个字面量也可以是一个黑板键。黑板键的写法是在值外面加花括号比如{timeout}表示运行时会从黑板上读取键名为timeout的值。这种机制让节点之间可以共享数据。比如规划节点把目标点写到黑板的goal键上后续的动作节点从goal读取。一个非常常见的坑是端口声明了但忘记在XML里传值节点内部又是用getInput取的这时候如果没做默认值兜底行为树会直接报错。所以我建议每个输入端口都在providedPorts()里给出默认值并且tick里对端口读取结果做判空处理。3. 完整实操从0到1创建自定义Behavior插件3.1 准备环境与创建功能包先说明一下版本环境。我这边用的ROS2 HumbleNavigation2对应的是Humble分支BehaviorTree.CPP版本是4.x。其他版本比如Jazzy也适用原理一样。创建工作空间和功能包mkdir -p ~/nav2_ws/src cd ~/nav2_ws ros2 pkg create nav2_custom_behavior --build-type ament_cmake --dependencies rclcpp nav2_behavior_tree pluginlib std_srvs behaviortree_cpp这个包名nav2_custom_behavior后面会频繁出现它既是库名的一部分也是插件描述文件注册时的包名。依赖项里nav2_behavior_tree是必须的因为自定义节点要继承它提供的类和模板pluginlib是插件机制的基础behaviortree_cpp是节点基类和工厂所在。3.2 自定义节点头文件怎么写我准备实现一个很有代表性的同步行为节点CheckService功能是调用一个std_srvs/srv/Trigger类型的服务服务返回success则节点返回SUCCESS否则返回FAILURE超时也返回FAILURE。先看头文件路径是include/nav2_custom_behavior/behaviors/check_service_node.hpp#ifndef NAV2_CUSTOM_BEHAVIOR__BEHAVIORS__CHECK_SERVICE_NODE_HPP_ #define NAV2_CUSTOM_BEHAVIOR__BEHAVIORS__CHECK_SERVICE_NODE_HPP_ #include string #include chrono #include rclcpp/rclcpp.hpp #include std_srvs/srv/trigger.hpp #include behaviortree_cpp/behavior_tree.h #include nav2_behavior_tree/bt_action_node.hpp namespace nav2_custom_behavior { class CheckServiceNode : public BT::SyncActionNode { public: CheckServiceNode( const std::string xml_tag_name, const BT::NodeConfig conf); // 声明节点的输入输出端口 static BT::PortsList providedPorts() { return { BT::InputPortstd::string(service_name, check_signal, service name to call), BT::InputPortint(service_timeout_ms, 1000, timeout for service call in ms) }; } BT::NodeStatus tick() override; private: rclcpp::Node::SharedPtr node_; rclcpp::Clientstd_srvs::srv::Trigger::SharedPtr client_; }; } // namespace nav2_custom_behavior #endif这里有几个关键点。第一构造函数参数是固定的xml_tag_name是XML里的标签名conf是节点配置里面装着黑板和端口信息BT.CPP框架会按这个签名调用构造函数不要改成自定义参数。第二providedPorts()声明了两个输入端口把服务名和超时时间做成可配置项而不是硬编码。这是行为节点设计的一个好习惯节点要做到参数化具体的服务名、阈值、话题名尽量通过端口传进来这样同一个插件能在不同任务场景复用。第三类里面存了一个node_指针这个指针要从黑板上取具体怎么取下一节配合实现文件一起说。3.3 实现文件核心的tick逻辑实现文件在src/behaviors/check_service_node.cpp完整代码如下#include nav2_custom_behavior/behaviors/check_service_node.hpp #include pluginlib/class_list_macros.hpp namespace nav2_custom_behavior { CheckServiceNode::CheckServiceNode( const std::string xml_tag_name, const BT::NodeConfig conf) : BT::SyncActionNode(xml_tag_name, conf) { // 从黑板里取ROS2节点指针这是Nav2插件节点的标准做法 node_ config().blackboard-template getrclcpp::Node::SharedPtr(node); } BT::NodeStatus CheckServiceNode::tick() { std::string service_name; int timeout_ms 0; getInput(service_name, service_name); getInput(service_timeout_ms, timeout_ms); if (!client_) { client_ node_-create_clientstd_srvs::srv::Trigger(service_name); } if (!client_-wait_for_service(std::chrono::seconds(1))) { RCLCPP_WARN(node_-get_logger(), Service %s not available, service_name.c_str()); return BT::NodeStatus::FAILURE; } auto request std::make_sharedstd_srvs::srv::Trigger::Request(); auto future client_-async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future, std::chrono::milliseconds(timeout_ms)) ! rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_WARN(node_-get_logger(), Service %s call timeout, service_name.c_str()); return BT::NodeStatus::FAILURE; } if (future.get()-success) { RCLCPP_INFO(node_-get_logger(), Service %s call success, service_name.c_str()); return BT::NodeStatus::SUCCESS; } return BT::NodeStatus::FAILURE; } } // namespace nav2_custom_behavior PLUGINLIB_EXPORT_CLASS(nav2_custom_behavior::CheckServiceNode, BT::Node)这段代码信息量比较大我拆开讲。先说构造函数。很多第一次写插件的人会在这里尝试this-node()或者rclcpp::Node::init()但Navigation2插件节点的正确姿势是从黑板取“node”这个键。config().blackboard-template getrclcpp::Node::SharedPtr(node)这行代码是Nav2约定俗成的做法BT Navigator启动时会把当前节点的SharedPtr放到黑板的node键上供所有插件节点共享。这样每个插件节点不需要单独创建节点直接复用主节点日志、参数、时间源都统一了。再说tick()。这里演示了端口读取、服务调用、状态返回三个环节。端口读取用getInput第一个参数是端口名第二个是接收变量。注意我用了两次getInput虽然返回值没检查但端口在providedPorts()里有默认值所以读到的一定有效。更严谨的做法是检查布尔返回值这里为了简洁省略了实际项目建议加上。服务调用用了async_send_request配合spin_until_future_complete。这里用spin_until_future_complete是因为当前线程执行的是行为树tick需要在这个线程临时驱动一下ROS2的 executor让service响应能被处理直到future完成或者超时。这种方式在Nav2的BT执行环境里是安全的因为行为树执行线程和主executor是分离的。最后看文件末尾的宏PLUGINLIB_EXPORT_CLASS(nav2_custom_behavior::CheckServiceNode, BT::Node)这个宏的作用是把类导出为pluginlib插件第一个参数是完整C类名第二个参数是基类类型。注意基类类型必须写BT::Node不是BT::SyncActionNode因为这个宏要注册的是继承体系下的节点类而BT::Node才是BT.CPP工厂认识的统一接口。3.4 插件描述XML怎么写代码写完了还要给插件写一个“身份证”文件让pluginlib知道这个类叫什么名字、在哪个库里、基类是什么。我在包根目录下创建custom_behavior_plugin.xmllibrary pathnav2_custom_behavior class namenav2_custom_behavior/CheckService typenav2_custom_behavior::CheckServiceNode base_class_typeBT::Node description Call a std_srvs/srv/Trigger service and report SUCCESS or FAILURE based on the response. /description /class /library这里面的几个字段值得好好说。library path填的是库名也就是CMake构建出来的.so文件名去掉lib前缀、去掉.so后缀。如果CMake里add_library出来的目标是nav2_custom_behavior构建产物就是libnav2_custom_behavior.so这里就填nav2_custom_behavior。name是插件在行为树XML里的标签名。这个名字要有全局唯一性因为BT Navigator会把所有加载的库里的插件名都注册到同一个NodeFactory如果不同库里出现重名后面加载的会覆盖前面的这种问题很难排查。所以我建议标签名带上包名前缀像nav2_custom_behavior/CheckService。type是C的完整类名必须和代码里PLUGINLIB_EXPORT_CLASS的第一个参数一致也必须在class命名空间里能被找到。base_class_type固定为BT::Node没有例外。3.5 CMakeLists与package.xml配置插件描述XML文件写好后必须告诉构建系统这个XML要安装、要注册到ament index。这一步由CMake宏完成。CMakeLists.txt的关键部分是这样find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(pluginlib REQUIRED) find_package(std_srvs REQUIRED) find_package(behaviortree_cpp REQUIRED) add_library(nav2_custom_behavior SHARED src/behaviors/check_service_node.cpp ) target_include_directories(nav2_custom_behavior PUBLIC include ) ament_target_dependencies(nav2_custom_behavior rclcpp nav2_behavior_tree pluginlib behaviortree_cpp ) # 关键导出插件描述文件注册到ament index pluginlib_export_plugin_description_file(nav2_custom_behavior custom_behavior_plugin.xml) ament_export_dependencies(rclcpp nav2_behavior_tree pluginlib behaviortree_cpp) ament_export_include_directories(include) ament_package()pluginlib_export_plugin_description_file这个宏是整套插件机制里最容易漏的一环。它做了两件事把custom_behavior_plugin.xml安装到share/nav2_custom_behavior/目录下同时在ament index里写入插件路径信息。如果没有这一行即使你的代码编译过了、so文件也生成了pluginlib依然找不到你的插件。package.xml里的依赖声明和普通ROS2包一样在depend标签里加上rclcpp、nav2_behavior_tree、pluginlib、behaviortree_cpp、std_srvs就行。不需要在package.xml里额外写插件相关的内容CMake宏已经帮你搞定注册了。4. 编译、验证、把节点用起来4.1 编译与检查产物编译这一步比较常规cd ~/nav2_ws colcon build --packages-select nav2_custom_behavior source install/setup.bash编译完成后确认一下关键产物是否存在ls -l install/nav2_custom_behavior/lib/libnav2_custom_behavior.so ls -l install/nav2_custom_behavior/share/nav2_custom_behavior/custom_behavior_plugin.xml第一个是插件库本体第二个是注册到ament index的插件描述文件。如果第二个文件不存在说明CMake里的pluginlib_export_plugin_description_file没配对赶紧回头检查宏的参数和XML路径。4.2 验证插件是否被“登记”成功有一种快速验证插件注册是否成功的方法是直接查ament index里注册的插件描述文件ros2 pkg prefix nav2_custom_behavior这个命令会输出包的前缀路径。然后检查share目录下的custom_behavior_plugin.xml内容是否正常。但说实话最直接有效的验证方式是让Navigation2自己加载一次看启动日志里有没有“Registered”相关的输出。Navigation2的BT Navigator加载插件库时会逐个尝试加载plugin_lib_names列表里的库。如果库加载成功且插件注册成功日志里会出现类似“BtActionServer: Registered ... by pluginlib这样的信息。如果没有通常就是没有在配置里把自定义库加进加载列表。4.3 修改plugin_lib_names是绕不过去的配置这一步是整个自定义插件能否被Nav2加载的关键也是我见别人踩坑最多的地方。BT Navigator默认只会加载nav2_bt_navigator和nav2_behavior_tree两个库你自己的库即使编译得再完美如果不在配置里点名它连理都不会理你。找到你的nav2_params.yaml在bt_navigator节点参数段下面加上自定义库bt_navigator: ros__parameters: plugin_lib_names: - nav2_bt_navigator - nav2_behavior_tree - nav2_custom_behavior同样如果你用的是behavior_server来处理行为树服务也要在behavior_server的参数段里加behavior_server: ros__parameters: plugin_lib_names: - nav2_behavior_tree - nav2_custom_behavior改完参数重启导航栈。如果配置正确启动日志里会出现你的插件名的注册信息。如果没出现检查库名拼写、XML描述文件里的library path是否和.so文件名一致。4.4 写一棵包含自定义节点的行为树插件注册成功后接下来就是写XML行为树。我自定义了一个任务流程先检查信号再导航再检查一次信号确认到达后的状态。XML如下root main_tree_to_executeMainTree BehaviorTree IDMainTree PipelineSequence nameworkflow nav2_custom_behavior/CheckService service_namedoor_control/check_open service_timeout_ms500/ NavigateToPose goal{goal}/ nav2_custom_behavior/CheckService service_namedock_control/check_latched service_timeout_ms1000/ /PipelineSequence /BehaviorTree /root注意两个细节。第一自定义节点的标签名是nav2_custom_behavior/CheckService和插件描述XML里的name字段严格一致。第二同一个节点在XML里出现了两次这没问题BT.CPP会实例化两个独立的节点对象端口互不干扰。把这份XML文件放到Nav2能访问的路径比如~/nav2_ws/src/nav2_custom_behavior/behavior_trees/custom_workflow.xml然后在nav2_params.yaml里把default_bt_xml_filename指向这个文件bt_navigator: ros__parameters: default_bt_xml_filename: /home/user/nav2_ws/src/nav2_custom_behavior/behavior_trees/custom_workflow.xml重启导航栈发布一个导航目标观察终端。如果一切顺利树执行时会看到Service door_control/check_open call success这样的日志然后才进入导航流程。5. 排查与避坑5.1 高频错误速查表我自己在开发过程中折腾了不少个性化问题也帮同事排查过类似问题这里整理成一张速查表。错误现象可能原因解决办法启动日志出现Plugin not found或Could not load class插件库没加入plugin_lib_names或库名拼错检查nav2_params.yaml里的plugin_lib_names确认库名与library path一致编译通过但导航栈不加载没有调用pluginlib_export_plugin_description_file或XML描述文件路径不对检查CMakeLists里宏的位置和XML路径行为树直接报Undefined portXML里写了没声明的端口或端口名拼错核对providedPorts()里的端口名和XML里的属性名节点没有执行树直接跳过插件标签名和XML描述文件里的name不一致检查XML标签名注意大小写和包名前缀tick()里拿不到node构造函数没用config().blackboard-get从黑板取node按本文的构造函数写法获取rclcpp::Node::SharedPtr服务调用超时节点一直FAILURE服务端没启动或wait_for_service在句首就返回false先启动服务端用ros2 service list确认服务名节点内部RCLCPP_INFO不打印日志级别被过滤或者节点对象是空的确认node_是否为空检查logger名称5.2 用日志方式快速定位问题排查插件问题时日志是首选工具。我在tick里习惯加这么几行方便在树执行过程中定位节点身份和端口值RCLCPP_INFO(node_-get_logger(), CheckServiceNode tick, tag%s, name().c_str()); RCLCPP_INFO(node_-get_logger(), service_name%s, timeout%d, service_name.c_str(), timeout_ms);name()是BT::TreeNode提供的方法返回X里这个节点标签名。多个CheckService节点同时出现时没有这两个日志你根本看不出来是哪个实例在跑。还有一个特别好用的功能是Groot2。Nav2支持把行为树的执行过程记录成tlog文件之后在Groot2里可视化回放能直观看到每个节点的执行顺序、状态切换、端口变化。自定义节点接入Groot2唯一的条件就是节点状态语义要规范同步节点在tick里必须明确返回SUCCESS或FAILUREAction型节点要正确返回RUNNING、SUCCESS、FAILURE。节点本身不用做任何额外适配。5.3 两个阶段性的开发心得第一插件注册链路要倒过来背。我踩过最深的坑就是代码没问题、XML没问题、编译没问题但导航栈就是加载不到。最后定位到是plugin_lib_names没加库。建议你在项目里新增自定义插件时把下面三件事当成一个整体做CMake里导出的宏、插件描述XML、plugin_lib_names列表。三个地方必须同时正确插件才能被导航栈识别。第二控制第一个节点的复杂度。第一个自定义节点我建议控制在20行核心逻辑以内比如一个带超时的Trigger服务调用、一个话题读取、一个信号判断。跑通整个链路后再去挑战Action型节点和复杂状态管理。不要一上来就试图写一个复杂的机器手臂控制节点一旦出问题你根本分不清是插件机制的问题还是业务逻辑的问题。6. 进阶把自定义节点升级为Action型节点6.1 BtActionNode基本框架同步节点虽然开发简单但它有一个天生的弱点如果tick里执行的操作耗时很长比如等待一个机械臂完成动作整个行为树会阻塞在那里。这时候就需要引入nav2_behavior_tree::BtActionNodeT它封装了ROS2 Action的完整生命周期让行为树节点在任务执行期间返回RUNNING同时异步等待结果。一个Action型自定义节点的骨架如下#include nav2_behavior_tree/bt_action_node.hpp class DoSomethingNode : public nav2_behavior_tree::BtActionNodemy_msgs::action::DoSomething { public: using ActionType my_msgs::action::DoSomething; using BaseActionT nav2_behavior_tree::BtActionNodeActionType; DoSomethingNode( const std::string xml_tag_name, const BT::NodeConfig conf) : BaseActionT(xml_tag_name, conf) { // 黑板取node初始化配置 } void on_tick() { // 在这里填充goal goal_.target 42; } BT::NodeStatus on_success() { // Action结束且结果为success时被调用 return BT::NodeStatus::SUCCESS; } }; PLUGINLIB_EXPORT_CLASS(DoSomethingNode, BT::Node)on_tick()是每次tick进入节点时调用用来把参数填进goal_on_success()、on_aborted()、on_cancelled()对应Action的不同终止状态。整个运行期间BT.CPP框架负责把节点置为RUNNING并等待Action server的反馈。6.2 什么时候该用Action型节点我的判断标准是如果这个动作预计超过1秒并且需要支持取消、进度反馈、与新目标抢占就值得用Action型。比如机器人的回充流程、机械臂抓取流程、多楼层电梯呼叫流程这些都属于“长时间异步任务”用BtActionNode最合适。同步节点不是不能用但要注意一个隐含风险长耗时操作会阻塞行为树执行线程。如果树里还有别的节点需要并行工作比如避障检测、电量监控就会受到影响。所以设计时要想清楚这个节点到底是“瞬间判断”还是“持续任务”两者的基类选择完全不同。6.3 把业务逻辑沉淀为可配置节点库写自定义Node的时间越久越能体会“参数化”的价值。同样是检查一个信号不同的项目信号源不同、服务名不同、超时时间不同如果把服务名硬编码在代码里每换一个项目就要改代码重新编译如果做成端口XML里切一个参数就完事。所以我的建议是一套完整的业务逻辑沉淀成2到5个通用节点每个节点的输入输出都用端口暴露节点之间通过黑板共享数据。这样在导航任务里你只需要改XML就能组合出不同的行为策略。自定义节点库积累到十几个之后你会发现大部分新需求根本不用写新代码用XML组合一下端口和节点就能搞定。我个人在实际操作中的体会是Navigation2的Behavior插件开发真正难的从来不是写C类而是把“代码、插件描述XML、CMake导出宏、导航栈加载配置”这一整条链路打通。链路上任何一环没对上编译时都不会报错但运行时就是找不到节点。第一次做的时候先跑通一个最小同步节点再逐步往上加复杂度是最稳妥的路线。再分享一个小技巧插件库命名尽量和包名保持一致库内节点统一带包名前缀这样不管在plugin描述XML里、CMake配置里还是YAML的加载列表里都不容易产生歧义。把这件事做好后面维护和团队协作会省掉非常多时间。