【navigation2全局路径更新频率修正】行为树框架的巧妙利用

前言

最近的项目需要用到navigation2中的全局路径规划器提供的/plan用于自主局部路径规划lqr的实现，但是奈何出现了以下的问题：
如上所示，全局路径规划器提供的/plan更新的频率过低，即便是更改了导航的配置参数也无动于衷：
- 但实际上，expected_planner_frequency 只是用来监控 planner 是否按预期运行，超出会报 warning，但不会改变实际规划频率。

yaml 复制代码

planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    use_sim_time: false
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.2
      use_astar: false
      allow_unknown: true

本文提供一个方案，在不更改底层源码的前提，直接暴力修改nav2运行时候的行为树，来达到提高navigation2全局路径更新频率的效果。

解决方案

分析

通常情况下，nav2导航的启动文件bringup_launch.py会加载nav2.yaml，我们在其中可以看到：
- 关于导航的基础参数可以看之前的文章：【10天速通Navigation2】(六)：Navigation2 导航bringup基础配置和参数详解

yaml 复制代码

bt_navigator:
  ros__parameters:
    use_sim_time: false
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
    ...

可以看到注释里头说道，如果没有特别设置，则使用的是默认的default_nav_through_poses_bt_xml和default_nav_to_pose_bt_xml

修改

我们可以新建一个yaml(可以参考官方的默认实现 )

bash 复制代码

mkdir -p /root/Terra_ws/src/slam/slam_gmapping/bt

cp \
/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml \
/root/Terra_ws/src/slam/slam_gmapping/bt/navigate_to_pose.xml

这里也提供官方的默认实现：

xml 复制代码

<!--
  This Behavior Tree replans the global path periodically at 1 Hz and it also has
  recovery actions specific to planning / control as well as general system issues.
  This will be continuous if a kinematically valid planner is selected.
-->
<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/>
        <RoundRobin name="RecoveryActions">
          <Sequence name="ClearingActions">
            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          </Sequence>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

上述代码的逻辑可以简单理解为：

bash 复制代码

NavigateRecovery (最多6次)
 └── PipelineSequence
      ├── 每1秒：
      │     └── ComputePathToPose
      │           └── fail → clear global costmap
      │
      └── FollowPath
            └── fail → clear local costmap

如果整个失败：
 └── ReactiveFallback
      ├── GoalUpdated → restart
      └── RecoveryActions：
            ├── clear costmaps
            ├── spin
            ├── wait
            └── back up

然后我们可以直接在yaml设置default_nav_to_pose_bt_xml的路径

yaml 复制代码

bt_navigator:
  ros__parameters:
    use_sim_time: false
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    default_nav_to_pose_bt_xml: "/root/Terra_ws/src/slam/slam_gmapping/bt/navigate_to_pose.xml"

注意：修改文件后记得编译!!!
运行bringup_launch.py,可以通过查询确认运行的是我们新增的内容：

bash 复制代码

ros2 param get /bt_navigator default_nav_to_pose_bt_xml

最终实现

如果你和本项目一样，只需要navigation2中的全局路径规划器提供的/plan，那么你直接暴力删除其他内容，留下面内容即可。

xml 复制代码

<!--
  This Behavior Tree replans the global path periodically at 1 Hz and it also has
  recovery actions specific to planning / control as well as general system issues.
  This will be continuous if a kinematically valid planner is selected.
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">

    <Repeat num_cycles="-1">
      <RateController hz="10.0">
        <ComputePathToPose
            goal="{goal}"
            path="{path}"
            planner_id="GridBased"/>
      </RateController>
    </Repeat>

  </BehaviorTree>
</root>

上述代码会约用一个极快的频率不断"重新计算从当前位姿到 goal 的全局路径"，并把结果写入 {path}，但不会执行运动。

报错解决：

如果你尝试关掉bringup_launch.py出现了下述报错
那就强行杀掉吧：

bash 复制代码

pkill -9 -f component_container
pkill -9 -f bt_navigator
pkill -9 -f nav2

总结

本文提供一个方案，在不更改底层源码的前提，直接暴力修改nav2运行时候的行为树，来达到提高navigation2全局路径更新频率的效果。
如有错误，欢迎支持!
感谢观看！