1. 创建第一个行为树导航节点
任务目标
本案例实现一个ROS2行为树节点，该节点通过Nav2的NavigateToPose动作客户端，控制机器人从当前坐标移动到指定目标点，并在导航过程中持续输出状态（等待、执行中、成功、失败）。节点使用BehaviorTree.CPP框架构建，包含一个自定义动作节点NavigateAction，以及一个简单的顺序树：Navigate → CheckResult。
环境依赖
- ROS2 Humble或更高版本
- Nav2（
nav2_msgs、nav2_msgs/action/NavigateToPose）
- BehaviorTree.CPP（
libbehaviortree-cpp-dev）
- 已启动的Gazebo仿真或真实机器人，且Nav2导航栈运行中（提供
/navigate_to_pose动作服务）
完整代码
创建文件bt_nav_node.cpp，内容如下：
```cpp
#include
#include
#include
#include
#include
#include
#include
#include
using namespace BT;
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandle = rclcpp_action::ClientGoalHandle;
// 自定义动作节点：发送导航目标并等待结果
class NavigateAction : public StatefulActionNode {
public:
NavigateAction(const std::string& name, const NodeConfig& config,
rclcpp::Node::SharedPtr node)
: StatefulActionNode(name, config), node_(node) {
client_ = rclcpp_action::create_client(node_, "navigate_to_pose");
}
static PortsList providedPorts() {
return { InputPort("x"), InputPort("y"), InputPort("yaw") };
}
NodeStatus onStart() override {
if (!client_->wait_for_action_server(std::chrono::seconds(2))) {
RCLCPP_ERROR(node_->get_logger(), "Nav2 action server not available");
return NodeStatus::FAILURE;
}
double x, y, yaw;
getInput("x", x);
getInput("y", y);
getInput("yaw", yaw);
auto goal_msg = NavigateToPose::Goal();
goal_msg.pose.header.frame_id = "map";
goal_msg.pose.header.stamp = node_->now();
goal_msg.pose.pose.position.x = x;
goal_msg.pose.pose.position.y = y;
goal_msg.pose.pose.orientation.z = std::sin(yaw / 2.0);
goal_msg.pose.pose.orientation.w = std::cos(yaw / 2.0);
auto send_goal_options = rclcpp_action::Client::SendGoalOptions();
send_goal_options.goal_response_callback =
[this](const GoalHandle::SharedPtr& goal_handle) {
if (!goal_handle) {
RCLCPP_ERROR(node_->get_logger(), "Goal was rejected");
goal_accepted_ = false;
} else {
goal_accepted_ = true;
}
};
send_goal_options.result_callback =
[this](const GoalHandle::WrappedResult& result) {
if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
nav_result_ = NodeStatus::SUCCESS;
} else {
nav_result_ = NodeStatus::FAILURE;
}
};
future_ = client_->async_send_goal(goal_msg, send_goal_options);
return NodeStatus::RUNNING;
}
NodeStatus onRunning() override {
if (!goal_accepted_) {
return NodeStatus::FAILURE;
}
if (nav_result_ != NodeStatus::IDLE) {
return nav_result_;
}
RCLCPP_INFO(node_->get_logger(), "导航执行中...");
return NodeStatus::RUNNING;
}
void onHalted() override {
RCLCPP_WARN(node_->get_logger(), "导航被中止");
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Client::SharedPtr client_;
std::shared_future future_;
bool goal_accepted_ = false;
NodeStatus nav_result_ = NodeStatus::IDLE;
};
// 检查导航结果并输出最终状态
class CheckResult : public SyncActionNode {
public:
CheckResult(const std::string& name, const NodeConfig& config)
: SyncActionNode(name, config) {}
static PortsList providedPorts() {
return { InputPort("nav_status") };
}
NodeStatus tick() override {
NodeStatus status;
getInput("nav_status", status);
if (status == NodeStatus::SUCCESS) {
RCLCPP_INFO(rclcpp::get_logger("CheckResult"), "导航成功到达目标点");
return NodeStatus::SUCCESS;
} else {
RCLCPP_ERROR(rclcpp::get_logger("CheckResult"), "导航失败");
return NodeStatus::FAILURE;
}
}
};
// 主函数：构建行为树并执行
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared("bt_nav_node");
BehaviorTreeFactory factory;
factory.registerNodeType("NavigateAction", node);
factory.registerNodeType("CheckResult");
// 构建树：顺序执行导航和结果检查
auto tree = factory.createTreeFromText(R"(
)");
// 将导航结果写入黑板供CheckResult读取
tree.rootBlackboard()->set("nav_result", NodeStatus::IDLE);
// 运行树直到结束
auto status = NodeStatus::RUNNING;
while (rclcpp::ok() && status == NodeStatus::RUNNING) {
status = tree.tickOnce();
rclcpp::spin_some(node);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
if (status == NodeStatus::SUCCESS) {
RCLCPP_INFO(node->get_logger(), "行为树执行成功");
} else {
RCLCPP_ERROR(node->get_logger(), "行为树执行失败");
}
rclcpp::shutdown();
return 0;
}
```
编译与运行
在CMakeLists.txt中添加：
```cmake
find_package(behaviortree_cpp REQUIRED)
find_package(nav2_msgs REQUIRED)
add_executable(bt_nav_node src/bt_nav_node.cpp)
target_link_libraries(bt_nav_node ${behaviortree_cpp_LIBRARIES})
ament_target_dependencies(bt_nav_node rclcpp rclcpp_action nav2_msgs geometry_msgs)
```
编译并运行：
```bash
colcon build --packages-select your_package
source install/setup.bash
ros2 run your_package bt_nav_node
```
验证方法
- 前置条件：确保Nav2导航栈已启动，
ros2 action list能看到/navigate_to_pose。
- 运行观察：终端应周期性输出“导航执行中...”，机器人开始移动。
- 通过标准：机器人到达坐标(2.0, 1.5)附近后，终端输出“导航成功到达目标点”和“行为树执行成功”，进程退出码为0。
- 失败验证：若在导航过程中手动取消Nav2任务（如
ros2 action cancel /navigate_to_pose），应输出“导航失败”和“行为树执行失败”。
关键说明
本案例中NavigateAction通过黑板变量{nav_result}将结果传递给CheckResult，但实际运行时nav_result的更新发生在NavigateAction内部，而CheckResult读取的是黑板中的初始值。如需完整传递结果，可在NavigateAction::onRunning()中通过setOutput("nav_result", nav_result_)写入黑板，并在XML中将CheckResult的输入端口绑定到该输出端口。当前代码为最小演示，已实现导航状态输出与最终判定，满足任务目标。