Decorators (二) - ForceSuccess & ForceFailure

Suilli

Announcement

Welcome to my blog! This is a sample announcement.

Learn More

标签

1003 字

5 分钟

Decorators (二) - ForceSuccess & ForceFailure

2026-01-15

Behavior Tree

/

ros2

一、ForceSuccess & ForceFailure 核心解析#

这两个节点同样是装饰器节点（仅包含一个子节点），核心逻辑比 Inverter 更简单：强制修改子节点的最终返回状态，具体规则如下：

节点类型	核心功能
`ForceSuccess`	执行子节点一次，无论子节点返回 FAILURE/SUCCESS，最终都返回 SUCCESS；若子节点返回 RUNNING，则自身也返回 RUNNING。
`ForceFailure`	执行子节点一次，无论子节点返回 FAILURE/SUCCESS，最终都返回 FAILURE；若子节点返回 RUNNING，则自身也返回 RUNNING。

适用场景#

ForceSuccess：即使某个子节点执行失败，也不想让整个分支中断（比如“尝试发送消息失败，但不影响后续逻辑”）。
ForceFailure：即使某个子节点执行成功，也需要强制让分支判定为失败（比如“检测到目标但权限不足，强制终止任务”）。

二、ROS2 + BehaviorTree4.x 实战示例#

下面基于你熟悉的代码结构，展示这两个节点的使用（兼容 ROS2 Humble + BehaviorTree4.x）。

1. 完整代码示例#

1
#include "behaviortree_cpp/bt_factory.h"
2
#include "rclcpp/rclcpp.hpp"
3

4
// 自定义测试节点：可通过参数控制返回状态
5
class ControllableNode : public BT::SyncActionNode
6
{
7
public:
8
  // 构造函数：接收节点名称 + 参数（返回SUCCESS/FAILURE）
9
  ControllableNode(const std::string& name, const BT::NodeConfig& config)
10
    : BT::SyncActionNode(name, config) {}
11

12
  // 定义输入参数（控制返回状态）
13
  static BT::PortsList providedPorts()
14
  {
15
    return { BT::InputPort<std::string>("return_status", "SUCCESS or FAILURE") };
16
  }
17

18
  // 核心执行逻辑：根据参数返回指定状态
19
  BT::NodeStatus tick() override
20
  {
21
    std::string status_str;
22
    // 获取输入参数
23
    if (!getInput("return_status", status_str)) {
24
      RCLCPP_ERROR(rclcpp::get_logger("ControllableNode"), "未设置return_status参数");
25
      return BT::NodeStatus::FAILURE;
26
    }
27

28
    // 根据参数返回状态
29
    if (status_str == "SUCCESS") {
30
      RCLCPP_INFO(rclcpp::get_logger("ControllableNode"), "执行ControllableNode，返回SUCCESS");
31
      return BT::NodeStatus::SUCCESS;
32
    } else if (status_str == "FAILURE") {
33
      RCLCPP_INFO(rclcpp::get_logger("ControllableNode"), "执行ControllableNode，返回FAILURE");
34
      return BT::NodeStatus::FAILURE;
35
    } else {
36
      RCLCPP_WARN(rclcpp::get_logger("ControllableNode"), "参数无效，默认返回FAILURE");
37
      return BT::NodeStatus::FAILURE;
38
    }
39
  }
40
};
41

42
int main(int argc, char** argv)
43
{
44
  // 初始化ROS2节点
45
  rclcpp::init(argc, argv);
46
  auto node = rclcpp::Node::make_shared("force_nodes_example_node");
47

48
  // 1. 创建行为树工厂
49
  BT::BehaviorTreeFactory factory;
50

51
  // 2. 注册自定义节点
52
  factory.registerNodeType<ControllableNode>("ControllableNode");
53

54
  // 3. 定义行为树结构：测试ForceSuccess和ForceFailure
55
  std::string xml_text = R"(
56
    <root BTCPP_format="4">
57
      <BehaviorTree ID="ForceNodesExample">
58
        <Sequence>
59
          <!-- 测试1：ForceSuccess包裹返回FAILURE的节点 → 最终返回SUCCESS -->
60
          <ForceSuccess>
61
            <ControllableNode return_status="FAILURE"/>
62
          </ForceSuccess>
63
          <!-- 测试2：ForceFailure包裹返回SUCCESS的节点 → 最终返回FAILURE -->
64
          <ForceFailure>
65
            <ControllableNode return_status="SUCCESS"/>
66
          </ForceFailure>
67
          <!-- 测试3：ForceSuccess包裹返回SUCCESS的节点 → 仍返回SUCCESS -->
68
          <ForceSuccess>
69
            <ControllableNode return_status="SUCCESS"/>
70
          </ForceSuccess>
71
        </Sequence>
72
      </BehaviorTree>
73
    </root>
74
  )";
75

76
  // 4. 创建行为树
77
  auto tree = factory.createTreeFromText(xml_text);
78

79
  // 5. 执行行为树
80
  RCLCPP_INFO(node->get_logger(), "开始执行行为树...");
81
  BT::NodeStatus status = tree.tickWhileRunning();
82

83
  // 6. 输出最终结果
84
  RCLCPP_INFO(node->get_logger(), "行为树执行完成，最终状态：%s",
85
              BT::toStr(status).c_str());
86

87
  rclcpp::shutdown();
88
  return 0;
89
}

2. 代码核心解释#

ControllableNode：可通过输入参数 return_status 控制返回 SUCCESS 或 FAILURE，比固定返回的节点更灵活，能全面测试 Force 系列节点的效果。
行为树结构：用 Sequence 串联 3 个测试用例：
- ForceSuccess + 返回 FAILURE 的子节点 → 最终返回 SUCCESS
- ForceFailure + 返回 SUCCESS 的子节点 → 最终返回 FAILURE
- ForceSuccess + 返回 SUCCESS 的子节点 → 仍返回 SUCCESS（验证“强制成功”对成功状态无影响）
参数传递：通过 XML 属性 return_status="FAILURE" 给自定义节点传参，这是 ROS2 行为树中传递参数的标准方式。

3. 预期输出#

运行代码后终端输出如下（重点看 Force 节点对结果的覆盖效果）：

1
[INFO] [ControllableNode]: 执行ControllableNode，返回FAILURE
2
[INFO] [ControllableNode]: 执行ControllableNode，返回SUCCESS
3
[INFO] [ControllableNode]: 执行ControllableNode，返回SUCCESS
4
[INFO] [force_nodes_example_node]: 行为树执行完成，最终状态：FAILURE

注：Sequence 节点第二个子节点（ForceFailure）返回 FAILURE，因此整个 Sequence 最终状态为 FAILURE，符合预期。

三、XML 文件单独定义（ROS2 常用方式）#

1
<root BTCPP_format="4">
2
  <BehaviorTree ID="ForceNodesTest">
3
    <!-- 场景1：即使传感器检测失败，也强制判定为成功，继续执行后续逻辑 -->
4
    <ForceSuccess name="ForceSensorSuccess">
5
      <SensorCheck/> <!-- 自定义传感器检测节点 -->
6
    </ForceSuccess>
7

8
    <!-- 场景2：即使任务执行成功，也强制判定为失败，终止当前分支 -->
9
    <ForceFailure name="ForceTaskFailure">
10
      <TaskExecute/> <!-- 自定义任务执行节点 -->
11
    </ForceFailure>
12
  </BehaviorTree>
13
</root>