使用行為樹控制機器人(一) —— 節點
使用行為樹控制機器人(二) —— 黑板
使用行為樹控制機器人(三) —— 通用端口

近期在從ros1導航跨到ros2導航，ros2導航的任務調度邏輯采用的是行為樹，所以在魔改ros2導航前先知己知彼學習一下行為樹到底是個啥？本文也是在大佬的基礎上加入自己的學習過程記錄。(如整理有誤，還請指點)
學習時參考鏈接：ROS機器人行為樹教程

一、背景需求

關于行為樹各節點的定義及其作用的博客整理，可以先參考鏈接中的教程，后續看自己是否有時間整理，本文直接根據需求進行代碼實操。通過行為樹控制機器人的一簡單場景需求如下：

二、創建ActionNodes

1. 功能實現

對于每個動作作為簡單案例多以打印輸出后，直接返回成功，后續根據對行為樹的掌握程度結合實際需求進行自定義復雜需求。

1.1 頭文件定義

根據場景需求創建如下 SimpleActionNode：

• CheckBattery()
• GripperInterface::open()
• GripperInterface::close()
• CameraInterface::open()
• CameraInterface::close()

#ifndef BEHAVIOR_TREE_NODES_H
#define BEHAVIOR_TREE_NODES_H#include "behaviortree_cpp/bt_factory.h"
#include <iostream>// 同步動作節點 (無端口)
class ApproachObject : public BT::SyncActionNode
{
public:ApproachObject(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config){}// 端口是節點與黑板（Blackboard）之間進行數據交換的接口// 必須實現靜態端口聲明方法static BT::PortsList providedPorts() {return {}; // 無端口}BT::NodeStatus tick() override;
};// 夾爪控制接口
class GripperInterface
{
public:GripperInterface() : _open(true) {}BT::NodeStatus open();BT::NodeStatus close();private:bool _open; // 共享狀態
};// 相機控制接口
class CameraInterface
{
public:CameraInterface() : _open(true) {}BT::NodeStatus open();BT::NodeStatus close();private:bool _open; // 共享狀態
};// 電池檢查函數聲明
BT::NodeStatus CheckBattery();#endif // BEHAVIOR_TREE_NODES_H

1.2 源文件實現

#ifndef BEHAVIOR_TREE_NODES_H
#define BEHAVIOR_TREE_NODES_H#include "behaviortree_cpp/bt_factory.h"
#include <iostream>// 同步動作節點 (無端口)
class ApproachObject : public BT::SyncActionNode
{
public:ApproachObject(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config){}// 端口是節點與黑板（Blackboard）之間進行數據交換的接口// 必須實現靜態端口聲明方法static BT::PortsList providedPorts() {return {}; // 無端口}BT::NodeStatus tick() override;
};// 夾爪控制接口
class GripperInterface
{
public:GripperInterface() : _open(true) {}BT::NodeStatus open();BT::NodeStatus close();private:bool _open; // 共享狀態
};// 相機打開
BT::NodeStatus CameraInterface::open() 
{_open = true;std::cout << "CameraInterface::open" << std::endl;return BT::NodeStatus::SUCCESS;
}// 相機關閉
BT::NodeStatus CameraInterface::close() 
{std::cout << "CameraInterface::close" << std::endl;_open = false;return BT::NodeStatus::SUCCESS;
}// 電池檢查函數聲明
BT::NodeStatus CheckBattery();#endif // BEHAVIOR_TREE_NODES_H

1.3 main文件實現

#include "behavior_tree_nodes.h"
#include "behaviortree_cpp/bt_factory.h"int main()
{// 創建行為樹工廠BT::BehaviorTreeFactory factory;// 注冊自定義節點factory.registerNodeType<ApproachObject>("ApproachObject");// 注冊簡單條件節點 (使用正確的lambda簽名)factory.registerSimpleCondition("CheckBattery", [](BT::TreeNode&) { return CheckBattery(); });// 創建相機、夾爪實例并注冊動作CameraInterface camera;GripperInterface gripper;factory.registerSimpleAction("OpenCamera", [&camera](BT::TreeNode&) { return camera.open(); });factory.registerSimpleAction("OpenGripper", [&gripper](BT::TreeNode&) { return gripper.open(); });factory.registerSimpleAction("CloseGripper", [&gripper](BT::TreeNode&) { return gripper.close(); });factory.registerSimpleAction("CloseCamera", [&camera](BT::TreeNode&) { return camera.close(); });try {// 從XML文件加載行為樹auto tree = factory.createTreeFromFile("../trees/my_tree.xml");// 打印樹結構 (調試用)std::cout << "------ Behavior Tree Structure ------" << std::endl;BT::printTreeRecursively(tree.rootNode());std::cout << "------------------------------------" << std::endl;// 執行行為樹tree.tickWhileRunning();} catch (const std::exception& e) {std::cerr << "Error: " << e.what() << std::endl;return 1;}return 0;
}

上述采用的是三種注冊節點方式：

• registerNodeType
• registerSimpleCondition
• registerSimpleAction
  ?

特性維度	registerNodeType	registerSimpleAction	registerSimpleCondition
節點基類	需繼承 BT::ActionNode 等	自動封裝為 SimpleActionNode	自動封裝為 SimpleConditionNode
返回值	自行實現 tick() 返回狀態 SUCCESS / FAILURE / RUNNING	Lambda 返回 NodeStatus SUCCESS / FAILURE / RUNNING	Lambda 返回 bool SUCCESS / FAILURE
數據交互	通過黑板端口(強類型)	可選端口(需手動檢查)	無端口(純條件檢查)
狀態維護	支持(成員變量)	不支持(無狀態)	不支持
典型用途	復雜動作/控制節點	簡單一次性動作	條件判斷(瞬時完成)
生命周期控制	可重寫 halt() 等方法	無	無
線程安全	更易實現	需外部保證	需外部保證

1.4 my_tree.xml 實現

行為樹實現邏輯如上，xml文件定義如下：

<root BTCPP_format="4" main_tree_to_execute="MainTree"><BehaviorTree ID="MainTree"><Sequence name="root_sequence"><CheckBattery name="battery_check"/><OpenCamera name="open_camera"/><OpenGripper name="open_gripper"/><ApproachObject name="approach_object"/><CloseGripper name="close_gripper"/><CloseCamera name="close_camera"/></Sequence></BehaviorTree>
</root>

注意
XML中使用的標識符必須與用于注冊TreeNodes的標識符相一致，比如：xml文件定義的CheckBattery 要和 main文件中 factory.registerSimpleCondition("CheckBattery", [](BT::TreeNode&) { return CheckBattery(); });的CheckBattery 作對應。

2. 執行結果

------ Behavior Tree Structure ------
----------------
root_sequencebattery_checkopen_cameragripper_controlRetryUntilSuccessfulopen_gripperrecovery_sequenceclose_camera_on_failureForceFailurerecovery_successapproach_objectclose_gripperclose_camera
----------------
------------------------------------
[ Battery: OK ]
CameraInterface::open
GripperInterface::open successfully!
ApproachObject: approach_object
GripperInterface::close
CameraInterface::close

三、 執行失敗處理

1. 添加嘗試次數

1.1 功能實現

觀察整個功能實現邏輯，如出現執行異常(如，夾爪初始化中打開夾爪失敗)，無再次嘗試的可能。故調整代碼，在對打開夾爪進行多次打開嘗試，如嘗試3次，3次嘗試都失敗再返回錯誤。行為樹邏輯及xml文件如下：

<root BTCPP_format="4" main_tree_to_execute="MainTree"><BehaviorTree ID="MainTree"><Sequence name="root_sequence"><CheckBattery name="battery_check"/><OpenCamera name="open_camera"/><RetryUntilSuccessful num_attempts="3"><OpenGripper name="open_gripper"/></RetryUntilSuccessful><ApproachObject name="approach_object"/><CloseGripper name="close_gripper"/><CloseCamera name="close_camera"/></Sequence></BehaviorTree>
</root>

為了實現打開次數的嘗試，為打開夾爪添加變量計數：

// 夾爪打開
BT::NodeStatus GripperInterface::open() 
{if(_open_cnt < 2){_open_cnt ++;std::cout << "\033[1;31m"<< "GripperInterface::open failed!"<< "\033[0m" << std::endl;return BT::NodeStatus::FAILURE;}_open = true;_open_cnt = 0;std::cout << "\033[1;32m"<< "GripperInterface::open successfully!"<<"\033[0m" << std::endl;return BT::NodeStatus::SUCCESS;
}

1.2 實驗結果

下圖第一次測試，修改xml嘗試 2次 打開夾爪失敗后，直接返回失敗；第二次測試，嘗試 3次 打開夾爪并成功運行整個流程。

2. 完善異常處理

2.1 多節點組合兜底

觀察嘗試多次打開夾爪實現，發現若嘗試打開夾爪多次依舊打開失敗，直接躺平，沒有進行關閉相機操作，實際場景這是不允許的！！此時就用到了其他節點的組合(此處，想一想該如何組合節點)：

上述行為樹表示：
當3次嘗試打開夾爪均返回失敗，此時通過 Fallback節點，進行關閉相機操作。>

注意：
ForceFailure裝飾器 無論相機是否關閉成功，即：
CloseCamera (SUCCESS) → ForceFailure(AlwaysSuccess) → 最終返回FAILURE CloseCamera (FAILURE) → ForceFailure(AlwaysSuccess) → 最終返回FAILURE
?
為什么要添加 ForceFailure節點，想象一下，若沒有該節點，通過Fallback節點關閉相機成功，此時返回成功，會繼續執行ApproachObject、CloseGripper等操作，這是不允許的。

<root BTCPP_format="4" main_tree_to_execute="MainTree"><BehaviorTree ID="MainTree"><Sequence name="root_sequence"><CheckBattery name="battery_check"/><OpenCamera name="open_camera"/><!-- 主控制流程 --><Fallback name="gripper_control"><!-- 嘗試3次開夾爪 --><RetryUntilSuccessful num_attempts="3"><OpenGripper name="open_gripper"/></RetryUntilSuccessful><!-- 失敗恢復流程 --><Sequence name="recovery_sequence"><CloseCamera name="close_camera_on_failure"/><!-- ForceFailure  裝飾器, 強制覆蓋子節點結果，始終返回FAILURE--><ForceFailure><AlwaysSuccess name="recovery_success"/></ForceFailure></Sequence></Fallback><ApproachObject name="approach_object"/><CloseGripper name="close_gripper"/><CloseCamera name="close_camera"/></Sequence></BehaviorTree>
</root>

2.2 實驗結果

通過下圖可以看出：嘗試 2次 打開夾爪失敗后，先關閉相機后 再直接返回失敗；第二次測試，嘗試 3次 打開夾爪并成功運行整個流程。