ROS2 Navigation 进阶教程学习笔记 第二章

第二章 Nav2行为

Nav2使用行为树来控制导航行为。行为树用于建立导航到目标位置以及机器人无法导航的情况的标准。

本节解释Nav2如何使用行为树。

什么是行为树导航

nav2_bt_navigator是行为树的包

由以下部分组成:

1. bt_navigator节点和配置文件

2. bt_navigator节点的behavior

3. behavior_server节点和其配置文件

bt_navigator和behavior_server的连接方式使用行为树指定。本单元中学习如何创建此类行为并将其提供给导航系统。

bt_navigator节点

必须管理其他导航相关节点在行为树中的定义。

必须给bt_navigator创建一个行为,确定它如何移动机器人。

如何创建一个行为

要创建行为,请使用可用性为的节点类型创建XML文件。此XML文件提供给bt_behavior,以便在需要时执行。

behavior.xml例子

<!--
  This Behavior Tree replans the global path periodically at 1 Hz, and it also has
  recovery actions.
-->
<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 service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <SequenceStar name="RecoveryActions">
        <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
        <Spin spin_dist="1.57"/>
        <Wait wait_duration="5"/>
      </SequenceStar>
    </RecoveryNode>
  </BehaviorTree>
</root>

可以等价为如下图

 分析XML文件

对于每个行为,由以下tag开始

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    ...
  </BehaviorTree>
</root>

root标记要执行的子树

behaviortree标签标记具有更定媒称的行为树 ID就是它的名字

接下来,在behavior标记中,按构建所需行为的顺序包含要使用的行为树节点。例如,对于导航课程的默认行为,你需要以下行为

每秒重新规划全局路径

跟随那个路径

如果机器人卡住,执行一下顺序

1.清除本地costmap

2. 清除全局costmap

3. 旋转以检查新的障碍物,再次构建costmap

4. 等待五秒,然后返回主要行为

看看它是如何在Nav2提供的行为节点实现的。

使用BT节点

RecoveryNode

此节点用于封装其他两个节点,并通过以下方式控制其激活

1.此节点将开始执行第一个子节点

2. 如果第一个子节点SUCCESS,那么节点返回SUCCESS

3. 如果第一个子节点failure,执行第二个子节点

4. 如果第二个子节点success,执行第一个子节点

5. 如果第二个子节点failure,那么返回failure并且结束

在导航中第一个子节点是导航任务,第二节点是recovery动作

在之前的XML文件中的例子

<RecoveryNode number_of_retries="6" name="NavigateRecovery">
    <PipelineSequence name="NavigateWithReplanning">
      ...
    </PipelineSequence>
    <SequenceStar name="RecoveryActions">
      ...
    </SequenceStar>
</RecoveryNode>

节点叫做navigaterecovery

行为开始于启动pipelinesequence节点叫做navigateweithreplanning

如果上个节点fail,会开始sequencestar节点叫做recoveryactions

分别是第一子节点和第二子节点

不过会尝试6次,仍然失败才会退出任务

注意:请记住,行为节点可以有输入端口(相当于传递给节点的参数)和输出端口(等同于节点返回的结果)。例如,RecoveryNode有一个名为number_of_retrys的输入端口(该名称是默认情况下每个节点都存在的另一个输入端口)。检查每个节点的文档,查看其输入输出端口列表。

pipelinesequence

此节点将按以下方式激活子节点:

第一个节点激活第一个子节点,直到它返回SUCCESS。

这将激活第一和第二个子节点(再次),直到第二个返回SUCCESS。

然后它(再次)激活第一、第二和第三个子节点,直到第三个节点返回SUCCESS,依此类推。

如果任何子节点返回FAILURE或最后一个子模块返回SUCCESS,它将停止。

请参见上一个XML文件中的示例:

<PipelineSequence name="NavigateWithReplanning">
    <RateController hz="1.0">
    ...
    </RateController>
    <RecoveryNode number_of_retries="1" name="FollowPath">
    ...
    </RecoveryNode>
</PipelineSequence>

这里,该节点用于每秒计算全局路径,然后使机器人遵循新计算的路径。

这有助于考虑机器人当前位置的修改,以使全局路径适应这些修改。

此节点名为NavigateWithReplanning,有两个子节点

第一个是RateController节点(检查其含义)。该节点包括目标路径的计算。

第二个是RecoveryNode节点。这一个用于使机器人遵循计算出的路径。

Ratecontroller

此节点将以指定为参数的特定频率调用后续节点。 上一个XML文件的示例:

<RateController hz="1.0">
    <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            ...
    </RecoveryNode>
</RateController>

在这种情况下,RateController将以1 Hz的频率调用RecoveryNode。

<RecoveryNode number_of_retries="1" name="ComputePathToPose">
   <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
   <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>

有两个子节点

一个是computepathtopose

一个是clearentirecostmap

ComputePathToPose

调用nav2_planner ROS2节点(planner服务器)提供的ComputePathToPose ROS2动作服务器,以计算目标的路径。通过使用黑板的变量(名为{goal}的变量)将目标引入节点。然后,在另一个名为{path}的黑板变量中引入规划器的结果。

什么是黑板 blackboard

黑板就像一个所有节点都可以访问的变量空间。它用于在节点之间共享信息。一个节点可以在那里放置一个值,另一个节点也可以读取它。在我们的ROS2行为树课程中了解更多信息。

<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>

在这种情况下,有人将{目标}变量的值放在黑板上,节点用计算出的路径填充{路径}黑板变量。目标和planner_id是节点的输入端口,路径是其输出端口。

clearentirecostmap

调用清除Costmap的服务。您必须指明要调用哪个服务器来清除本地或全局Costmap。 上一个XML文件的示例:

<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>

sequence star

工作方式与PipelineSequence节点相同,但不会激活已完成SUCCESS的节点。 上一个XML文件的示例:

<SequenceStar name="RecoveryActions">
    <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
    <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
    <Spin spin_dist="1.57"/>
    <Wait wait_duration="5"/>
</SequenceStar>

followpath

调用控制器中的动作服务器,该服务器将向机器人的轮子发送命令,以遵循计算出的路径。 上一个XML文件的示例:

<FollowPath path="{path}" controller_id="FollowPath"/>

spin

调用nav2_recoveries ROS节点提供的自旋ROS2动作服务器。该服务器将使机器人按spin_dist参数中指示的度数旋转到位。 上一个XML文件的示例:

<Spin spin_dist="1.57"/>

wait

调用nav2_recovers ROS节点提供的等待ROS2动作服务器。它将使行为等待指定的秒数。 上一个XML文件的示例:

<Wait wait_duration="5"/>

结论

还有很多其他节点可以用于您的行为。查看此处查看Nav2节点的官方列表,以了解有关节点的更多信息以及如何配置它们。 此外,如果您需要大量处理行为,您应该了解更多关于行为树的一般信息,以及BehaviorTree.CPP实现库。

如何向bt_behavior提供行为

在启动节点期间,必须为bt_navigation节点指定两项内容:

带有节点配置的btnavigator.yaml文件。

behavior.xml文件。

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_nav_to_pose_bt_xml: "/home/user/ros2_ws/src/path_planner_server/config/behavior.xml"
    default_server_timeout: 20
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node

在参数下,pluging_lib_names指定XML行为文件所需的行为节点列表。

记住在此参数中添加所需的值。

可在此处找到可用节点插件的完整列表 检查您在路径规划单元中创建的代码,它在其中执行配置文件的加载:

bt_navigator_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'bt_navigator.yaml')
...
Node(
    package='nav2_bt_navigator',
    executable='bt_navigator',
    name='bt_navigator',
    output='screen',
    parameters=[bt_navigator_yaml])

练习2.1

创建一个名为abort_withn_low_battery.xml的新行为文件,该文件执行以下行为:

每当提供新的目标目的地时,机器人就会前往该目标。

如果电池在任何时候低于25%,则中止当前目标。

调用电池主题/电池。发布该主题的100%电池状态。

在某一时刻,将该主题的发布值更改为20%。机器人必须改变其行为,并在那时放弃其目标。

注释

使用behavior.xml作为模板并相应地修改它。

有一个Nav2行为节点用于电池状态检测:IsBatteryLow。查看官方文档,了解如何在代码中使用它。

由于IsBatteryLow节点在电池电量正常时返回FALSE(因为它正在检查IsBattery Low),因此您需要使用名为<Inverter>的BT节点来否定该检查。 要在电池主题中发布,请使用以下命令(根据所需测试):

ros2 topic pub /battery sensor_msgs/BatteryState '{voltage: 12.0, percentage: 1.0, power_supply_status: 3}' 
<!--
  This Behavior Tree replans the global path periodically at 1 Hz, and it also has
  recovery actions.
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <Inverter>
            <IsBatteryLow battery_topic="/battery" is_voltage="false" min_battery="0.25" />
        </Inverter>
        <RateController hz="1.0">                
            <RecoveryNode number_of_retries="1" name="ComputePathToPose">
                <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
                <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
            </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="FollowPath"/>
            <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <SequenceStar name="RecoveryActions">
        <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
        <Spin spin_dist="1.57"/>
        <Wait wait_duration="5"/>
      </SequenceStar>
    </RecoveryNode>
  </BehaviorTree>
</root>
rosrun groot Groot

可以将行为树可视化。

为Nav2配置Groot

打开Groot并选择编辑器模式 现在您需要将自定义Nav2节点加载到其中。

为此,选择“加载调色板”图标。

选择/opt/ros/humble/share/nav2_behavior_tree/nav2_tree_nodes.xml文件 新节点将显示在节点列表中

可视化现有行为

将behavior.xml文件加载到编辑器中,为此

点击loadtree图标

然后打开behavior.xml文件

可以看到行为树的可视化图形被打开了

修改现有行为

通过添加检查电池状态的节点来修改加载的行为。为此,请执行以下步骤

在节点调色板中选择逆变器节点。将其拖放到行为桌面上。

在节点调色板中选择IsBatteryLow节点。将其拖放到行为桌面上。

将逆变器节点连接到名为NavigateWithReplanning的PipelineSequence节点。将它移到最左边,这样它将是它将启动的第一个子节点

现在将逆变器节点连接到IsBatteryLow节点。 你应该有这样的东西:

 从上到下填入 /battery false 0.25

将其另存为abort_win_low_battery_2.xml。

修改导航启动文件,使其加载新的行为文件。

在另一个终端上,将100%的值发布到/battery主题中。

ros2 topic pub /battery sensor_msgs/BatteryState '{voltage: 12.0, percentage: 1.0, power_supply_status: 3}' 

定位机器人并向其发送导航目标。 现在将/beactery主题中发布的值更改为20%。机器人应该放弃它的路径。

Recovery Behaviors

当机器人卡住时,Nav2恢复行为会自动激活,以尝试恢复

当controller_server的配置文件中指定的一个检查程序发出机器人未达到目标的信号时,bt_navigator将激活恢复行为:

controller_server检测机器人卡住的情况,并通知bt_navigator。

如其配置文件所示,bt_navigator调用recovery_server以激活恢复插件。

您在recoveries_server配置文件Recovery.yaml中配置了恢复行为。

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

目前,nav2_recovers包提供了三个可用插件:

旋转:当Costmap更新时,它将在原地执行旋转。当机器人看到周围的Costmap充满障碍物(现实中可能存在或不存在)时,这很有用。这种行为将有助于确定目前存在的障碍,从而增加找到通往目标的新道路的机会

备份:执行机器人在一定距离内的线性运动。

等待:将机器人停止到位并等待一定时间。等待时间在操作请求中提供。

配置

一些参数与recovery_server本身相关,其他参数仅与插件相关。 recovery_server参数包括:

    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0

所有插件都将在适用于所有插件的特定条件下运行。这些条件与速度限制和要使用的帧有关。

    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

它们是如何工作的

每个插件都提供一个动作服务器,需要它的行为节点将调用它。

如果尚未启动,请启动导航系统,然后请求可用的操作服务器列表:

ros2 action list
/backup
/compute_path_through_poses
/compute_path_to_pose
/follow_path
/move_robot_as
/navigate_through_poses
/navigate_to_pose
/spin
/wait

如您所见,有/backup、/spin和/wait操作服务器可供调用。

当BT节点请求一个恢复行为时,BT导航器将调用恢复行为的动作服务器。如果controller_server由于目标缺乏进展而请求,BT导航器也会调用它们。

  • 4
    点赞
  • 10
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值