3. 行为树中的条件节点

任务目标

在行为树中实现一个条件节点，用于实时检测机器人前方障碍物。当检测到障碍物时，行为树切换至绕行分支；无障碍物时继续直行。本案例使用ROS 2 + BehaviorTree.CPP完成最小可复现闭环。

环境依赖

• ROS 2 Humble或更高版本
• BehaviorTree.CPP库（sudo apt install ros-${ROS_DISTRO}-behaviortree-cpp-v3）
• 模拟传感器数据：使用静态变量模拟激光雷达测距结果（无需真实硬件）

核心实现

以下代码定义了一个CheckObstacle条件节点，从Blackboard读取传感器距离值，与安全阈值比较后返回成功或失败。主树结构为：若检测到障碍物则执行绕行动作，否则执行直行动作。

#include <behaviortree_cpp/behavior_tree.h>
#include <behaviortree_cpp/bt_factory.h>
#include <string>
#include <cmath>

// 模拟传感器：从Blackboard读取距离值（单位：米）
class CheckObstacle : public BT::ConditionNode {
public:
    CheckObstacle(const std::string& name, const BT::NodeConfig& config)
        : BT::ConditionNode(name, config) {}

    static BT::PortsList providedPorts() {
        return { BT::InputPort<double>("distance", "传感器检测距离"),
                 BT::InputPort<double>("threshold", 0.5, "安全距离阈值") };
    }

    BT::NodeStatus tick() override {
        double distance, threshold;
        if (!getInput<double>("distance", distance) ||
            !getInput<double>("threshold", threshold)) {
            return BT::NodeStatus::FAILURE;
        }
        // 距离小于阈值视为有障碍物
        return (distance < threshold) ? BT::NodeStatus::SUCCESS 
                                      : BT::NodeStatus::FAILURE;
    }
};

// 动作节点：直行
class MoveForward : public BT::SyncActionNode {
public:
    MoveForward(const std::string& name, const BT::NodeConfig& config)
        : BT::SyncActionNode(name, config) {}

    BT::NodeStatus tick() override {
        std::cout << "[动作] 前方无障碍，直行..." << std::endl;
        return BT::NodeStatus::SUCCESS;
    }
};

// 动作节点：绕行
class AvoidObstacle : public BT::SyncActionNode {
public:
    AvoidObstacle(const std::string& name, const BT::NodeConfig& config)
        : BT::SyncActionNode(name, config) {}

    BT::NodeStatus tick() override {
        std::cout << "[动作] 检测到障碍物，执行绕行..." << std::endl;
        return BT::NodeStatus::SUCCESS;
    }
};

int main() {
    BT::BehaviorTreeFactory factory;
    factory.registerNodeType<CheckObstacle>("CheckObstacle");
    factory.registerNodeType<MoveForward>("MoveForward");
    factory.registerNodeType<AvoidObstacle>("AvoidObstacle");

    // 行为树XML定义：条件节点作为选择器的第一个分支
    std::string xml_text = R"(
        <root BTCPP_format="4">
            <BehaviorTree ID="MainTree">
                <Sequence name="root_sequence">
                    <Fallback name="obstacle_check">
                        <CheckObstacle name="check_obstacle" 
                                       distance="{distance}" 
                                       threshold="0.5"/>
                        <Sequence name="avoid_sequence">
                            <AvoidObstacle name="avoid"/>
                        </Sequence>
                    </Fallback>
                    <MoveForward name="move_forward"/>
                </Sequence>
            </BehaviorTree>
        </root>
    )";

    auto tree = factory.createTreeFromText(xml_text);

    // 模拟三次不同距离的检测
    std::vector<double> test_distances = {0.3, 1.2, 0.2};
    for (double dist : test_distances) {
        std::cout << "\n=== 模拟距离: " << dist << " 米 ===" << std::endl;
        tree.rootBlackboard()->set<double>("distance", dist);
        BT::NodeStatus status = tree.tickExactlyOnce();
        std::cout << "树执行状态: " << (status == BT::NodeStatus::SUCCESS ? "SUCCESS" : "FAILURE") << std::endl;
    }
    return 0;
}

编译与运行

# 编译（假设文件名为 obstacle_condition.cpp）
g++ -std=c++17 obstacle_condition.cpp -o obstacle_condition \
    -lbehaviortree_cpp

# 运行
./obstacle_condition

验证方法

程序依次模拟三个距离值，观察输出：

• 距离0.3米（小于阈值0.5）→ 应输出“检测到障碍物，执行绕行”，且不执行直行动作
• 距离1.2米（大于阈值）→ 应输出“前方无障碍，直行”
• 距离0.2米 → 再次触发绕行

若三次输出与上述逻辑一致，且树最终状态均为SUCCESS，则条件节点工作正常。可通过修改test_distances中的数值或threshold参数验证不同场景。

关键说明

• 条件节点CheckObstacle通过Blackboard读取distance变量，实际项目中该值由激光雷达话题回调写入
• 行为树结构使用Fallback（选择器）：第一个子节点（条件）成功则跳过后续绕行分支；失败则执行绕行
• 绕行动作仅打印日志，实际项目中替换为发布速度指令或导航目标即可