3. 行为树动作节点与条件节点：自定义动作节点（C++/Python），条件节点的编写与数据端口

好，咱们进入行为树最核心的部分——节点编写。

上一章我们聊了行为树的基本结构，说白了就是一棵由节点组成的树。但节点到底怎么写？尤其是动作节点和条件节点，它们有什么区别？数据怎么在节点间传递？今天我就把这些讲透。

3.1 动作节点 vs 条件节点：本质区别

先搞清楚一件事：动作节点和条件节点，虽然都是叶子节点，但职责完全不同。

特性 动作节点 (Action) 条件节点 (Condition)
核心目的 执行一个行为（移动、抓取、等待） 检查一个状态（是否到位、是否检测到物体）
返回值 SUCCESS / FAILURE / RUNNING SUCCESS / FAILURE（不支持 RUNNING）
执行时间 可能持续多帧（比如导航到目标点） 瞬间完成（检查一下传感器值）
典型场景 发送速度指令、播放语音、执行路径规划 判断电池电量、检测障碍物距离、检查开关状态

我个人习惯把条件节点叫做「看门狗」——它只看不干，只返回 true 或 false。动作节点才是真正干活的。

重要原则：条件节点不要修改任何系统状态。它只读，不写。如果你在条件节点里发了电机指令，那设计就有问题了。

3.2 自定义动作节点（C++ 实现）

咱们先看 C++ 怎么写。我用的是 BehaviorTree.CPP 库，这是 ROS 生态里最主流的方案。

一个最基本的动作节点，继承自 BT::ActionNodeBase，然后重写 tick() 方法。

// 文件: MoveToGoalNode.cpp
#include "behaviortree_cpp/action_node.h"
#include "ros/ros.h"

class MoveToGoalNode : public BT::ActionNodeBase
{
public:
  MoveToGoalNode(const std::string& name, const BT::NodeConfig& config)
    : BT::ActionNodeBase(name, config)
  {
    // 构造函数里初始化 ROS 句柄
    nh_ = ros::NodeHandle("~");
    cmd_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
  }

  // 必须重写 tick()
  BT::NodeStatus tick() override
  {
    // 1. 从黑板上读取目标位置
    double target_x, target_y;
    if (!getInput("target_x", target_x) || !getInput("target_y", target_y))
    {
      ROS_ERROR("MoveToGoalNode: 缺少目标坐标输入");
      return BT::NodeStatus::FAILURE;
    }

    // 2. 获取当前机器人位置（假设从 /odom 获取）
    // 这里简化处理，实际项目中需要订阅里程计
    double current_x = 1.0, current_y = 2.0; // 伪代码

    // 3. 计算距离
    double distance = sqrt(pow(target_x - current_x, 2) + pow(target_y - current_y, 2));

    // 4. 判断是否到达
    if (distance < 0.1)
    {
      ROS_INFO("MoveToGoalNode: 到达目标点");
      return BT::NodeStatus::SUCCESS;
    }

    // 5. 还没到，继续移动
    geometry_msgs::Twist cmd;
    cmd.linear.x = 0.2;  // 前进速度
    cmd.angular.z = 0.0; // 直行
    cmd_pub_.publish(cmd);

    // 返回 RUNNING 表示还在执行中
    return BT::NodeStatus::RUNNING;
  }

  // 必须重写 halt()，用于中断时停止机器人
  void halt() override
  {
    ROS_WARN("MoveToGoalNode: 被中断，停止机器人");
    geometry_msgs::Twist stop_cmd;
    stop_cmd.linear.x = 0.0;
    stop_cmd.angular.z = 0.0;
    cmd_pub_.publish(stop_cmd);
  }

private:
  ros::NodeHandle nh_;
  ros::Publisher cmd_pub_;
};
我的经验：halt() 方法很多人会忘记实现。我曾经在一个项目里没写 halt()，结果行为树被中断时机器人还在往前冲，差点撞到人。从那以后，我每个动作节点都会先写好 halt()。

3.3 自定义条件节点（C++ 实现）

条件节点更简单。它继承 BT::ConditionNode，同样重写 tick()。

// 文件: BatteryCheckNode.cpp
#include "behaviortree_cpp/condition_node.h"

class BatteryCheckNode : public BT::ConditionNode
{
public:
  BatteryCheckNode(const std::string& name, const BT::NodeConfig& config)
    : BT::ConditionNode(name, config)
  {
    // 初始化电池订阅
    battery_sub_ = nh_.subscribe("/battery_level", 1, 
                                 &BatteryCheckNode::batteryCallback, this);
    battery_level_ = 100.0; // 默认满电
  }

  BT::NodeStatus tick() override
  {
    // 从黑板读取阈值（可选）
    double threshold = 20.0;
    getInput("threshold", threshold);

    if (battery_level_ > threshold)
    {
      ROS_INFO("BatteryCheckNode: 电量充足 (%.1f%%)", battery_level_);
      return BT::NodeStatus::SUCCESS;
    }
    else
    {
      ROS_WARN("BatteryCheckNode: 电量不足 (%.1f%%)", battery_level_);
      return BT::NodeStatus::FAILURE;
    }
  }

private:
  void batteryCallback(const std_msgs::Float32::ConstPtr& msg)
  {
    battery_level_ = msg->data;
  }

  ros::NodeHandle nh_;
  ros::Subscriber battery_sub_;
  double battery_level_;
};

看到了吗？条件节点里没有发布任何指令，只是读取数据、做判断、返回结果。这就是「只读」的含义。

避坑指南：我曾经见过有人把条件节点写成阻塞的——在 tick() 里用 ros::spinOnce() 等待数据。这会导致行为树卡死。条件节点必须瞬间返回，如果需要等待，请用动作节点的 RUNNING 状态。

3.4 Python 实现：更灵活的选择

如果你喜欢 Python，BehaviorTree.CPP 也支持 py_trees 接口。我个人觉得 Python 版本更适合快速原型验证。

# 文件: move_to_goal_node.py
import py_trees
import rospy
from geometry_msgs.msg import Twist

class MoveToGoalNode(py_trees.behaviour.Behaviour):
    def __init__(self, name="MoveToGoal"):
        super(MoveToGoalNode, self).__init__(name)
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.blackboard = self.attach_blackboard_client(name=self.name)
        # 声明需要从黑板读取的变量
        self.blackboard.register_key(key="target_x", access=py_trees.common.Access.READ)
        self.blackboard.register_key(key="target_y", access=py_trees.common.Access.READ)

    def setup(self, timeout):
        rospy.loginfo(f"{self.name}: 设置完成")
        return True

    def initialise(self):
        rospy.loginfo(f"{self.name}: 开始执行")
        # 每次 tick 开始时重置状态
        self.distance = float('inf')

    def update(self):
        # 从黑板读取目标
        target_x = self.blackboard.target_x
        target_y = self.blackboard.target_y

        # 模拟获取当前位置（实际项目中替换为里程计数据）
        current_x = 1.0
        current_y = 2.0

        self.distance = ((target_x - current_x)**2 + (target_y - current_y)**2)**0.5

        if self.distance < 0.1:
            rospy.loginfo(f"{self.name}: 到达目标点")
            # 停止机器人
            self.cmd_pub.publish(Twist())
            return py_trees.common.Status.SUCCESS
        else:
            cmd = Twist()
            cmd.linear.x = 0.2
            self.cmd_pub.publish(cmd)
            rospy.loginfo(f"{self.name}: 正在移动，距离 {self.distance:.2f}m")
            return py_trees.common.Status.RUNNING

    def terminate(self, new_status):
        # 类似 C++ 的 halt()
        rospy.logwarn(f"{self.name}: 被终止，停止机器人")
        self.cmd_pub.publish(Twist())
我的建议：Python 版本调试起来更方便。我经常先用 Python 写节点逻辑，确认没问题后再用 C++ 重写一遍用于生产环境。这样既保证了开发效率，又保证了运行性能。

3.5 数据端口：节点间的通信桥梁

节点之间怎么传数据？靠的就是端口（Port）。

BehaviorTree.CPP 支持三种端口：

  • Input Port：只读，从黑板或父节点获取数据
  • Output Port：只写，把数据写入黑板
  • Bidirectional Port：可读可写，但用得少

在 XML 配置文件中，端口是这样定义的：

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <Sequence>
            <BatteryCheckNode threshold="{battery_threshold}"/>
            <MoveToGoalNode target_x="{goal_x}" target_y="{goal_y}"/>
        </Sequence>
    </BehaviorTree>
</root>

这里的 {battery_threshold} 和 {goal_x} 就是端口变量。它们从黑板上读取值。

在 C++ 节点里注册端口：

// 在节点类中声明端口
static const BT::PortsList providedPorts()
{
    return {
        BT::InputPort<double>("target_x", "目标点 X 坐标"),
        BT::InputPort<double>("target_y", "目标点 Y 坐标"),
        BT::InputPort<double>("threshold", 20.0, "电量阈值")
    };
}
关键点：端口名必须和 XML 中的变量名一致。我见过太多人因为拼写错误导致数据传不过去。建议在代码里用常量定义端口名，避免硬编码字符串。

3.6 实战中的常见坑

最后分享几个我踩过的坑：

  1. 忘记注册端口：C++ 节点必须实现 providedPorts() 静态方法，否则端口不可用。编译器不会报错，但运行时 getInput 会一直失败。
  2. 条件节点里做耗时操作：比如在条件节点里调用 ROS 的 service 等待响应。这会导致行为树卡住。正确的做法是把耗时操作放到动作节点里，用 RUNNING 状态表示正在等待。
  3. 黑板变量命名冲突：多个节点使用同一个黑板变量名，但含义不同。我建议给变量加前缀，比如 nav_goal_x、arm_target_angle，避免混淆。
  4. 忽略 halt() 的幂等性：halt() 可能被多次调用，要确保重复调用不会出问题。比如发布停止指令时，先检查发布器是否有效。

嗯，今天就先聊到这里。下一章我们会讲控制节点——Sequence、Fallback、Parallel 这些组合节点的设计思路。到时候你会发现，动作节点和条件节点只是砖块，控制节点才是搭建行为树的骨架。

← 上一章 📖 返回目录 下一章 →