2. 使用Nav2行为树框架加载XML

任务目标

本案例要求编写一个ROS2节点，通过Nav2的BehaviorTreeEngine加载一个包含导航动作的XML行为树文件，并执行一次从当前坐标到目标坐标的导航任务。核心目标是验证行为树XML能够被正确解析并驱动Nav2的导航动作。

环境与依赖

• ROS2 Humble或更高版本
• 已安装nav2_behavior_tree和nav2_msgs包
• 已启动Nav2仿真环境（如turtlebot3_gazebo）及导航相关节点

实现方案

创建一个ROS2节点，在on_configure阶段加载XML行为树文件，在on_activate阶段执行一次导航。行为树XML中定义NavigateToPose动作节点，目标点通过端口从节点代码传入。

关键完整代码

// bt_nav_executor.cpp
#include <rclcpp/rclcpp.hpp>
#include <nav2_behavior_tree/behavior_tree_engine.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <behaviortree_cpp/bt_factory.h>
#include <memory>

class BtNavExecutor : public rclcpp::Node
{
public:
  BtNavExecutor() : Node("bt_nav_executor")
  {
    // 声明目标点参数
    this->declare_parameter("goal_x", 2.0);
    this->declare_parameter("goal_y", 2.0);
    this->declare_parameter("goal_yaw", 0.0);
    this->declare_parameter("bt_xml_file", "");
  }

  void execute()
  {
    // 1. 获取参数
    double goal_x = this->get_parameter("goal_x").as_double();
    double goal_y = this->get_parameter("goal_y").as_double();
    double goal_yaw = this->get_parameter("goal_yaw").as_double();
    std::string bt_xml_file = this->get_parameter("bt_xml_file").as_string();

    if (bt_xml_file.empty()) {
      RCLCPP_ERROR(this->get_logger(), "bt_xml_file parameter is empty");
      return;
    }

    // 2. 创建行为树引擎
    nav2_behavior_tree::BehaviorTreeEngine bt_engine;

    // 3. 创建黑板并设置目标点
    BT::Blackboard::Ptr blackboard = BT::Blackboard::create();
    blackboard->set<double>("goal_x", goal_x);
    blackboard->set<double>("goal_y", goal_y);
    blackboard->set<double>("goal_yaw", goal_yaw);

    // 4. 加载XML并创建行为树
    BT::Tree tree = bt_engine.createTreeFromFile(bt_xml_file, blackboard);

    // 5. 执行行为树（阻塞直到完成或失败）
    auto result = bt_engine.run(tree, [this]() { return !rclcpp::ok(); });

    if (result == BT::NodeStatus::SUCCESS) {
      RCLCPP_INFO(this->get_logger(), "Navigation BT completed successfully");
    } else {
      RCLCPP_ERROR(this->get_logger(), "Navigation BT failed with status: %d", static_cast<int>(result));
    }
  }
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<BtNavExecutor>();
  node->execute();
  rclcpp::shutdown();
  return 0;
}

行为树XML文件

<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="navigate_sequence">
      <NavigateToPose goal_x="{goal_x}" goal_y="{goal_y}" goal_yaw="{goal_yaw}" />
    </Sequence>
  </BehaviorTree>
</root>

编译与运行

# 在CMakeLists.txt中添加依赖
find_package(nav2_behavior_tree REQUIRED)
find_package(nav2_msgs REQUIRED)

# 编译
colcon build --packages-select your_package_name

# 运行（需先启动Nav2仿真环境）
ros2 run your_package_name bt_nav_executor --ros-args \
  -p bt_xml_file:=/path/to/navigate_bt.xml \
  -p goal_x:=2.0 -p goal_y:=2.0 -p goal_yaw:=0.0

验证方法

1. 启动Nav2仿真环境，确保/navigate_to_pose动作服务器可用：ros2 action list应显示/navigate_to_pose
2. 运行上述节点，观察终端输出
3. 在RViz中观察机器人是否开始移动并最终到达目标点(2.0, 2.0)
4. 通过条件：终端输出Navigation BT completed successfully，且机器人到达目标位置附近（误差小于0.1m）

常见问题

• XML加载失败：检查文件路径是否正确，XML格式是否合法，行为树ID是否与XML中main_tree_to_execute一致
• 端口未设置：确保黑板中设置的goal_x等变量名与XML中{goal_x}完全匹配
• 导航失败：确认Nav2的map_server、amcl、controller_server等节点已正确启动，且目标点在可行区域内