BehaviorTree.ROS2 开发者指南

// tree_execution_server.cpp 构造函数中
p_->single_shot_timer = node_->create_wall_timer(
    std::chrono::milliseconds(1), [this]() {
      if(!p_->factory_initialized_) {
        executeRegistration();
      }
      p_->single_shot_timer->cancel();  // 只执行一次
    });

void TreeExecutionServer::executeRegistration()
{
  p_->factory.clearRegisteredBehaviorTrees();
  // 1. 用户自定义注册（虚函数）
  registerNodesIntoFactory(p_->factory);
  // 2. 从目录加载 .so 插件
  RegisterPlugins(p_->params, p_->factory, node_);
  // 3. 从目录加载 .xml 行为树
  RegisterBehaviorTrees(p_->params, p_->factory, node_);
  p_->factory_initialized_ = true;
}

void LoadPlugin(BT::BehaviorTreeFactory& factory,
                const std::filesystem::path& file_path,
                BT::RosNodeParams params)
{
  BT::SharedLibrary loader(file_path.string());
  if(loader.hasSymbol(BT::PLUGIN_SYMBOL)) {
    // 标准 BT.CPP 插件
    auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL);
    func(factory);
  }
  else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL)) {
    // ROS 插件，需要传入 RosNodeParams
    auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL);
    func(factory, params);
  }
}

void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
                       const std::string& directory_path)
{
  for(const auto& entry : directory_iterator(directory_path)) {
    if(entry.path().extension() == ".xml") {
      factory.registerBehaviorTreeFromFile(entry.path().string());
      RCLCPP_INFO(kLogger, "Loaded BehaviorTree: %s",
                  entry.path().filename().c_str());
    }
  }
}

void TreeExecutionServer::execute(
    const std::shared_ptr<GoalHandleExecuteTree> goal_handle)
{
  const auto goal = goal_handle->get_goal();

  // 1. 如果参数有变，重新注册
  if(p_->param_listener->is_old(p_->params)) {
    executeRegistration();
  }

  // 2. 创建黑板（父级为全局黑板）
  auto root_blackboard = BT::Blackboard::create(p_->global_blackboard);

  // 3. 根据 target_tree 名称创建树实例
  p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard);

  // 4. 调用用户钩子
  onTreeCreated(p_->tree);

  // 5. 启动 Groot2 调试发布器
  p_->groot_publisher = std::make_shared<BT::Groot2Publisher>(p_->tree, ...);

  // 6. 循环 tick
  while(rclcpp::ok() && status == BT::NodeStatus::RUNNING) {
    if(goal_handle->is_canceling()) {
      // 处理取消
      p_->tree.haltTree();
      goal_handle->canceled(action_result);
      return;
    }

    status = p_->tree.tickExactlyOnce();

    // 调用用户钩子
    if(auto res = onLoopAfterTick(status); res.has_value()) { ... }
    if(auto res = onLoopFeedback(); res.has_value()) { ... }

    // 按频率休眠
    p_->tree.sleep(...);
  }

  // 7. 返回结果
  goal_handle->succeed(action_result);  // 或 abort
}

static PortsList providedBasicPorts(PortsList addition) {
  PortsList basic = { InputPort<std::string>("action_name", "", "Action server name") };
  basic.insert(addition.begin(), addition.end());
  return basic;
}

class SleepAction : public RosActionNode<btcpp_ros2_interfaces::action::Sleep>
{
public:
  SleepAction(const std::string& name, const NodeConfig& conf,
              const RosNodeParams& params)
    : RosActionNode<btcpp_ros2_interfaces::action::Sleep>(name, conf, params)
  {}

  // 声明自定义端口
  static BT::PortsList providedPorts() {
    return providedBasicPorts({ InputPort<unsigned>("msec") });
  }

  bool setGoal(Goal& goal) override;
  void onHalt() override;
  BT::NodeStatus onResultReceived(const WrappedResult& wr) override;
  BT::NodeStatus onFailure(ActionNodeErrorCode error) override;
};

bool SleepAction::setGoal(RosActionNode::Goal& goal) {
  auto timeout = getInput<unsigned>("msec");
  goal.msec_timeout = timeout.value();
  return true;
}

NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult& wr) {
  RCLCPP_INFO(logger(), "%s: onResultReceived. Done = %s",
              name().c_str(), wr.result->done ? "true" : "false");
  return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
}

NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) {
  RCLCPP_ERROR(logger(), "%s: onFailure with error: %s",
               name().c_str(), toStr(error));
  return NodeStatus::FAILURE;
}

void SleepAction::onHalt() {
  RCLCPP_INFO(logger(), "%s: onHalt", name().c_str());
}

// 关键：注册为插件
CreateRosNodePlugin(SleepAction, "SleepAction");

class SetBoolService : public BT::RosServiceNode<SetBool>
{
public:
  SetBoolService(const std::string& name, const BT::NodeConfig& conf,
                 const BT::RosNodeParams& params)
    : RosServiceNode<SetBool>(name, conf, params) {}

  static BT::PortsList providedPorts() {
    return providedBasicPorts({ BT::InputPort<bool>("value") });
  }

  bool setRequest(Request::SharedPtr& request) override;
  BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override;
  BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override;
};

// src/navigate_to_goal.hpp
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

using NavigateToPose = nav2_msgs::action::NavigateToPose;

class NavigateToGoal : public BT::RosActionNode<NavigateToPose>
{
public:
  NavigateToGoal(const std::string& name, const BT::NodeConfig& conf,
                 const BT::RosNodeParams& params)
    : RosActionNode<NavigateToPose>(name, conf, params)
  {}

  // 声明端口：继承 action_name 端口，再添加 goal 端口
  static BT::PortsList providedPorts()
  {
    return providedBasicPorts({
      BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Target pose")
    });
  }

  // [必须] 设置 Goal
  bool setGoal(Goal& goal) override
  {
    if(!getInput("goal", goal.pose))
    {
      RCLCPP_ERROR(logger(), "Missing required input [goal]");
      return false;
    }
    return true;
  }

  // [必须] 处理结果
  BT::NodeStatus onResultReceived(const WrappedResult& result) override
  {
    if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
      return BT::NodeStatus::SUCCESS;
    return BT::NodeStatus::FAILURE;
  }

  // [可选] 处理反馈
  BT::NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback) override
  {
    RCLCPP_INFO(logger(), "Distance remaining: %.2f",
                feedback->distance_remaining);
    return BT::NodeStatus::RUNNING;
  }

  // [可选] 错误处理
  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
  {
    RCLCPP_ERROR(logger(), "Navigation failed: %s", BT::toStr(error));
    return BT::NodeStatus::FAILURE;
  }
};

// src/perform_inspection.hpp
#pragma once
#include "behaviortree_ros2/bt_service_node.hpp"
#include "std_srvs/srv/trigger.hpp"

using Trigger = std_srvs::srv::Trigger;

class PerformInspection : public BT::RosServiceNode<Trigger>
{
public:
  PerformInspection(const std::string& name, const BT::NodeConfig& conf,
                    const BT::RosNodeParams& params)
    : RosServiceNode<Trigger>(name, conf, params)
  {}

  static BT::PortsList providedPorts()
  {
    return providedBasicPorts({});  // 无额外端口
  }

  // [必须] 设置 Request
  bool setRequest(Request::SharedPtr& request) override
  {
    // Trigger 类型无额外字段
    return true;
  }

  // [必须] 处理 Response
  BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override
  {
    if(response->success)
      return BT::NodeStatus::SUCCESS;

    RCLCPP_WARN(logger(), "Inspection failed: %s", response->message.c_str());
    return BT::NodeStatus::FAILURE;
  }
};

// src/check_battery.hpp
#pragma once
#include "behaviortree_cpp/action_node.h"

class CheckBattery : public BT::SyncActionNode
{
public:
  CheckBattery(const std::string& name, const BT::NodeConfig& conf)
    : BT::SyncActionNode(name, conf)
  {}

  static BT::PortsList providedPorts()
  {
    return { BT::InputPort<double>("min_level", 20.0, "Minimum battery level") };
  }

  BT::NodeStatus tick() override
  {
    double min_level;
    getInput("min_level", min_level);

    // 从黑板获取电池电量（由 TreeExecutionServer 中的订阅回调更新）
    auto battery = config().blackboard->get<float>("battery_level");
    if(battery >= min_level)
      return BT::NodeStatus::SUCCESS;

    return BT::NodeStatus::FAILURE;
  }
};

// src/navigate_to_goal.cpp
#include "navigate_to_goal.hpp"
#include "behaviortree_ros2/plugins.hpp"

// 第一个参数：类名；第二个参数：XML 中使用的节点名
CreateRosNodePlugin(NavigateToGoal, "NavigateToGoal");

// src/my_nodes_plugin.cpp
#include "navigate_to_goal.hpp"
#include "perform_inspection.hpp"
#include "check_battery.hpp"
#include "behaviortree_ros2/plugins.hpp"

BT_REGISTER_ROS_NODES(factory, params)
{
  // ROS 节点（需要 params）
  factory.registerNodeType<NavigateToGoal>("NavigateToGoal", params);
  factory.registerNodeType<PerformInspection>("PerformInspection", params);

  // 纯逻辑节点（不需要 params）
  factory.registerNodeType<CheckBattery>("CheckBattery");
}

// src/my_executor.cpp
#include <behaviortree_ros2/tree_execution_server.hpp>
#include <behaviortree_cpp/loggers/bt_cout_logger.h>
#include <std_msgs/msg/float32.hpp>

class MyExecutor : public BT::TreeExecutionServer
{
public:
  MyExecutor(const rclcpp::NodeOptions& options)
    : TreeExecutionServer(options)
  {
    // 订阅电池电量，更新全局黑板
    battery_sub_ = node()->create_subscription<std_msgs::msg::Float32>(
        "battery_level", 10,
        [this](const std_msgs::msg::Float32::SharedPtr msg) {
          globalBlackboard()->set("battery_level", msg->data);
        });
  }

  // 树创建后的回调：添加日志
  void onTreeCreated(BT::Tree& tree) override
  {
    logger_ = std::make_shared<BT::StdCoutLogger>(tree);
  }

  // 树执行完成后的回调
  std::optional<std::string> onTreeExecutionCompleted(
      BT::NodeStatus status, bool was_cancelled) override
  {
    logger_.reset();  // 清理日志
    if(was_cancelled) {
      return "Mission was cancelled";
    }
    return std::nullopt;  // 使用默认消息
  }

  // 每次 tick 后的反馈
  std::optional<std::string> onLoopFeedback() override
  {
    return "Mission in progress...";
  }

  // 手动注册非插件化的节点
  void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override
  {
    // 可以在这里注册不想做成插件的节点
    // factory.registerNodeType<SomeNode>("SomeNode");
  }

private:
  std::shared_ptr<BT::StdCoutLogger> logger_;
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr battery_sub_;
};

int main(int argc, char* argv[])
{
  rclcpp::init(argc, argv);

  rclcpp::NodeOptions options;
  auto server = std::make_shared<MyExecutor>(options);

  rclcpp::executors::MultiThreadedExecutor exec(
      rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
  exec.add_node(server->node());
  exec.spin();
  exec.remove_node(server->node());

  rclcpp::shutdown();
}

// 每个 RosActionNode/RosServiceNode 类内部
using ClientsRegistry = std::unordered_map<std::string, std::weak_ptr<ClientInstance>>;
static ClientsRegistry& getRegistry();

RosActionNode::RosActionNode(..., const RosNodeParams& params) {
  auto portIt = config().input_ports.find("action_name");
  if(portIt != ...) {
    const auto& bb_name = portIt->second;
    if(isBlackboardPointer(bb_name)) {
      action_name_should_be_checked_ = true;  // 延迟到 tick 时解析
    } else if(!bb_name.empty()) {
      createClient(bb_name);                   // 硬编码值，立即创建
    }
  }
  if(!client_instance_ && !params.default_port_value.empty()) {
    createClient(params.default_port_value);    // 使用默认值
  }
}

// 全局黑板可在构造函数中通过订阅者更新
globalBlackboard()->set("battery_level", msg->data);

// 执行时，根黑板继承全局黑板
auto root_blackboard = BT::Blackboard::create(p_->global_blackboard);
p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard);

p_->groot_publisher = std::make_shared<BT::Groot2Publisher>(
    p_->tree, p_->params.groot2_port);

RosNodeParams params;
params.server_timeout = std::chrono::milliseconds(5000);
params.wait_for_server_timeout = std::chrono::milliseconds(5000);

// tree_execution_server.cpp - execute()
if(p_->param_listener->is_old(p_->params)) {
  executeRegistration();  // 重新加载插件和树
}