写一个行为树插件-navigation2
Kong Liangqian Lv6
 2021-12-07 18:38:29  

创建一个新插件

我们将创建一个简单的 BT 插件节点，使他在另一台服务器中执行操作。 对于本示例，我们将分析 nav2_behavior_tree 包中最简单的行为树操作节点，即wait节点。

除了这个动作 BT 节点示例之外，您还可以创建自定义装饰器、条件和控制节点。 每个节点类型在行为树中都具有独特的作用，可以执行诸如planning、control BT 流、检查condition的状态或修改其他 BT 节点的输出等操作。

下面的这份代码是 nav2_behavior_tree 的包中的wait_action 节点.

我们的示例插件继承自基类 nav2_behavior_tree::BtActionNode。 基类是 BehaviorTree.CPP BT::ActionNodeBase 的包装器，它利用 ROS 2 action 客户端简化了 BT action 节点。 BTActionNode 既是 BT action，又使用 ROS 2 action 网络接口, 可以调用远程服务器来做一些工作。

如果是其他类型的节点，就需要继承对应的Node,decorator, control, condition分别对应 BT::DecoratorNode, BT::ControlNode, or BT::ConditionNode。

对于不使用 ROS 2 action 接口的 BT action 节点，请使用 BT::ActionNodeBase 基类本身。

method Method description Required?
Constructor Constructor to indicate the corresponding XML tag name to that matches the plugin, the name of the action server to call using the plugin, and any BehaviorTree.CPP special configurations required. Yes
providedPorts() A function to define the input and output ports a BT node may have. These are analogous to parameters that are defined in the BT XML by hardcoded values or by the value of other output ports of other nodes. Yes
on_tick() Method is called when this BT node is ticked by the behavior tree while executing. This should be used to get dynamic updates like new blackboard values, input ports, or parameters. May also reset state for the action. No
on_wait_for_result() Method is called when the behavior tree node is waiting for a result from the ROS 2 action server it called. This could be used to check for updates to preempt the current task, check for a timeout, or anything to compute while waiting for the action to complete. No
on_success() Method is called when the ROS 2 action server returns a successful result. Returns the value the BT node will report back to the tree. No
on_aborted() Method is called when the ROS 2 action server returns an aborted result. Returns the value the BT node will report back to the tree. No
on_cancelled() MMethod is called when the ROS 2 action server returns a cancelled result. Returns the value the BT node will report back to the tree. No

在本教程中，我们只使用on_tick方法

在这个例子中，我们设置了需要等待的时间duration

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
WaitAction::WaitAction(
  const std::string & xml_tag_name,
  const std::string & action_name,
  const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
{
  int duration;
  getInput("wait_duration", duration);
  if (duration <= 0) {
    RCLCPP_WARN(
      node_->get_logger(), "Wait duration is negative or zero "
      "(%i). Setting to positive.", duration);
    duration *= -1;
  }

  goal_.time.sec = duration;
}

这里，我们给出xml_tag_name的输入，它告诉BT节点插件与该节点对应的XML中的字符串。action_name是将会被执行的action，最后一系列的配置我们可以暂时性忽略

然后调用了BtActionNode构造函数，模板参数为nav2_msgs::action::Wait，即一个action message类型，BTActionNode中的tick()方法，当从树中调用该节点时，行为树直接调用该方法。然后，随着action客户端目标一起调用On_tick()。

在构造函数中，我们通过输入给duration赋值，并且也给goal_对象中的时间进行赋值。

providedPorts() 可以定义输入和输出，他是行为树节点可以访问的。我们的例子中，只有一个输入， wait_duration，这是对每一个BT xml节点都可以设置的。我们设置其值的类型为int，1，变量名为wait_duration，一个简单的描述 Wait time.

1
2
3
4
5
6
7
static BT::PortsList providedPorts()
{
  return providedBasicPorts(
    {
      BT::InputPort<int>("wait_duration", 1, "Wait time")
    });
}

当行为树标记特定节点时，调用ontick()方法。对于此wait BT节点，我们只需通知blackboard上的计数器，对应于recovery的动作插件被勾选了。这对于保存在特定导航运行期间执行的recovery数量的度量非常有用。你也可以记录或更新goal等待时间，如果那是一个变量输入。

1
2
3
4
void WaitAction::on_tick()
{
  increment_recovery_count();
}

其余的方法不使用，也不强制覆盖它们。只有一些BT节点插件需要覆盖on_wait_for_result()来检查抢占或超时。如果没有方法的重写，success, aborted, and cancelled 方法将分别默认为success、FAILURE、success。

导出插件

我们已经完成了插件的制作，现在需要将其导出。在BehaviorTree.CPP中，加载和导出插件的操作是通过宏BT_REGISTER_NODES来完成的。

1
2
3
4
5
6
7
8
9
10
11
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  BT::NodeBuilder builder =
    [](const std::string & name, const BT::NodeConfiguration & config)
    {
      return std::make_unique<nav2_behavior_tree::WaitAction>(name, "wait", config);
    };

  factory.registerBuilder<nav2_behavior_tree::WaitAction>("Wait", builder);
}

在宏里面，我们必须要创建一个NodeBuilder的对象，这样才可以指定我们自定义的action，此lambda 函数会返回一个指向我们自定义的节点，传入参数name和config，然后定义ROS2的action 的名字，这里是wait action。

最后把这个builder传入工厂去创建，Wait是xml行为树中的名字，也对应的插件的名字，下面是示例，其中Wait BT XML节点指定了一个非变量输入端口wait_duration为5秒。

1
<Wait wait_duration="5"/>

添加插件名字到config文件

1
2
3
4
5
6
7
8
9
10
bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_back_up_action_bt_node # other plugin
    - nav2_wait_action_bt_node    # our new plugin

跑起来~

现在在行为树中可以使用刚刚自定义的节点了。下面是navigate_w_replanning_and_recovery.xml的例子

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <SequenceStar name="RecoveryActions">
          <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
          <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
        </SequenceStar>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>
 Comments