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

最新推荐文章于 2024-05-18 10:00:13 发布

PC2721

最新推荐文章于 2024-05-18 10:00:13 发布

阅读量1.5k

点赞数 4

分类专栏： The Consruct网站ROS2学习笔记文章标签：学习自动驾驶机器人

本文链接：https://blog.csdn.net/PC2721/article/details/128755610

版权

The Consruct网站ROS2学习笔记专栏收录该内容

6 篇文章 17 订阅

订阅专栏

第二章 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导航器也会调用它们。

PC2721

关注

4
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
ROS2 Navigation 进阶教程学习笔记第二章

ROS2 Navigation进阶教程第二章学习笔记
复制链接

扫一扫

专栏目录

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

第二章 Nav2行为

什么是行为树导航

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