nav2里行为树的哪个文件去编写create_subscription() 函数来订阅某个节点的主题,并将其与行为节点连接起来
时间: 2024-02-16 22:05:25 浏览: 68
ROS2-Python-Node:基本ROS2节点,以便发布和订阅点云数据
在ROS 2中的Navigation2中,行为树的创建是通过XML文件实现的。因此,你需要打开`nav2_bt_navigator`包的`behavior_trees`目录中的`navigate_w_replanning_and_recovery.xml`文件,然后在其中找到需要订阅主题的行为节点,并在该节点的`params`标签中添加一个`topic`参数,用于指定订阅的主题名称,例如:
```xml
<BehaviorTree>
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<!-- 其他节点 -->
<Sequence name="nav_to_pose_sequence">
<!-- 其他节点 -->
<Action
name="ComputePathToPose"
plugin="nav2_behavior_tree::ComputePathToPose"
parameters="{current_pose: current_pose, goal_pose: goal_pose, planner_id: planner_id}"
>
<params>
<param name="topic" value="/my/topic" Type="string" />
</params>
</Action>
<!-- 其他节点 -->
</Sequence>
<!-- 其他节点 -->
</BehaviorTree>
</root>
</BehaviorTree>
```
在上述示例中,我们为`ComputePathToPose`行为节点添加了一个`topic`参数,并将其设置为`/my/topic`。接下来,你需要在`nav2_bt_navigator`包中的`src`目录下找到`compute_path_to_pose_action.cpp`文件,该文件定义了`ComputePathToPose`行为节点的实现,在该文件中添加`create_subscription()`函数来订阅该主题,并将其与行为节点连接起来。具体实现方式可以参考如下示例代码:
```cpp
#include <string>
#include <memory>
#include "nav2_bt_navigator/bt_navigator.hpp"
#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
namespace nav2_behavior_tree
{
ComputePathToPose::ComputePathToPose(
const std::string & name,
const BT::NodeConfiguration & config)
: SyncActionNode(name, config)
{
// 获取 topic 参数的值
config.blackboard->get<std::string>("topic", topic_);
// 创建订阅者,并绑定回调函数
sub_ = nh_.create_subscription<geometry_msgs::PoseStamped>(
topic_, 10, std::bind(&ComputePathToPose::topic_callback, this, std::placeholders::_1));
}
void ComputePathToPose::topic_callback(const geometry_msgs::PoseStamped::ConstPtr & msg)
{
// 处理订阅到的消息
// ...
}
} // namespace nav2_behavior_tree
```
在上述示例代码中,我们首先通过`NodeConfiguration`的`blackboard`成员获取了`topic`参数的值,并保存在了`topic_`成员变量中。接着,我们在`ComputePathToPose`的构造函数中创建了一个订阅者,通过`create_subscription()`函数订阅了该主题,并指定了回调函数`topic_callback()`。最后,在`topic_callback()`函数中,你可以处理订阅到的消息,完成行为节点的逻辑实现。
阅读全文