Nav2 Integration

Navigation2 (Nav2) 集成技能

用于将 Navigation2 导航堆栈集成到 ROS2 机器人项目中，涵盖配置、调参和定制开发

何时使用

当需要以下帮助时使用此技能：

• 将 Nav2 集成到机器人项目
• 配置行为树（Behavior Tree）
• 调优路径规划和避障参数
• 多机器人导航
• Nav2 与定位系统（SLAM）集成

快速参考

Nav2 架构

Nav2 堆栈
├── planner_server      # 全局路径规划 (NavFn, Smac, Theta*)
├── controller_server   # 局部路径跟踪 (DWB, TEB, RPP)
├── smoother_server     # 路径平滑
├── behaviors_server    # 行为树 (备份、等待、旋回)
├── bt_navigator        # 行为树导航器
├── navigatorLifecycle  # 生命周期管理
└── recovery_server     # 恢复行为

核心话题

ROS2 launch 集成

最小启动配置

# launch/nav2_bringup.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from nav2_common.launch import RewrittenYaml
import os

def generate_launch_description():
    # 导航参数文件
    nav2_params = os.path.join(
        get_package_share_directory('nav2_bringup'),
        'params/nav2_params.yaml'
    )

    # 替换参数
    configured_params = RewrittenYaml(
        source_file=nav2_params,
        root_key='',
        param_rewrites={
            'robot_base_frame': 'base_link',
            'use_sim_time': 'false',
        }
    )

    return LaunchDescription([
        # Nav2 生命周期节点
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            parameters=[configured_params],
            remappings=[
                ('/cmd_vel', '/diff_driver_base_controller/cmd_vel_unstamped'),
            ],
        ),
        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            parameters=[configured_params],
        ),
        Node(
            package='nav2_behaviors',
            executable='behavior_server',
            name='behavior_server',
            parameters=[configured_params],
        ),
        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            parameters=[configured_params],
        ),
        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager',
            parameters=[{'autostart': True}],
        ),
    ])

订阅 SLAM 定位

# nav2_params.yaml
amcl:
  ros__parameters:
    use_sim_time: false
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame: base_link
    global_frame: map
    robot_frame: base_link

bt_navigator:
  ros__parameters:
    default_nav_to_pose_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_ifcovery_bt.xml
    plugin_lib_names:
      - nav2_compute_path_to_pose_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

行为树 (Behavior Tree)

自定义行为树 XML

<?xml version="1.0"?>
<root BTCPP_format="4">
  <BehaviorTree ID="main_bt">
    <PipelineSequence name="navigate_recovering">
      <!-- 主导航 -->
      <RecoveryNode name="navigate_and_recover">
        <Sequence name="nav_w_replanning">
          <RateController hz="1.0">
            <ComputePathToPose goal="{goal}" path="{path}" error_code_id="{error_code}"/>
          </RateController>
          <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_error}"/>
        </Sequence>
        <!-- 恢复策略 -->
        <Sequence name="recovery">
          <ClearEntireCostmapService name="clear_costmap" service="/global_costmap/clear_entirely_costmap"/>
          <WaitTrigger name="wait" duration="3"/>
        </Sequence>
      </RecoveryNode>
    </PipelineSequence>
  </BehaviorTree>
</root>

Python 自定义行为节点

# my_bt_nodes.py
from nav2_behaviors.plugins.backup import BackUp

class CustomBackUp(BackUp):
    def onConfigure(self) -> bool:
        self->state->setInput("speed", 0.1)  # 自定义速度
        self->state->setInput("time_allowed", 10.0)
        return True

注册到插件库：

<library path="my_bt_nodes">
  <node library="my_bt_nodes" name="CustomBackUp" action="nav2_behaviors/plugins/actions"/>
</library>

局部路径跟踪

DWB Controller 配置

dwb_controller:
  ros__parameters:
    min_vel_x: 0.0
    min_vel_theta: -1.5
    max_vel_x: 0.5
    max_vel_y: 0.5
    max_vel_theta: 1.5
    min_speed_xy: 0.0
    max_speed_xy: 0.5
    acc_lim_x: 2.5
    acc_lim_y: 2.5
    acc_lim_theta: 3.2

    # TEB 配置（备选）
teb_local_planner:
  ros__parameters:
    max_vel_x: 0.5
    max_vel_theta: 1.5
    max_accel: 2.5
    wheelbase: 0.5
    cmd_angle_instead_rotvel: true

跟踪前沿（FollowWaypoints）

from nav2_msgs.action import FollowWaypoints
from rclpy.action import ActionClient

class WaypointFollower:
    def __init__(self):
        self._action_client = ActionClient(
            self, FollowWaypoints, '/follow_waypoints'
        )

    def follow_waypoints(self, poses: list):
        goal = FollowWaypoints.Goal()
        goal.poses = poses
        self._action_client.wait_for_server()
        self._action_client.send_goal_async(goal)

代价地图配置

全局代价地图

global_costmap:
  global_costmap:
    ros__parameters:
      footprint_padding: 0.05
      plugin_lib_names:
        - nav2_layered_plugins.CostmapPlugin
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      resolution: 0.05
      track_unknown_space: true
      plugins:
        - plugin: nav2_costmap_2d_plugins.StaticLayer
        - plugin: nav2_costmap_2d_plugins.ObstacleLayer
          observation_sources: scan
          scan:
            topic: /scan
            max_obstacle_height: 2.0
            clearing: true
        - plugin: nav2_costmap_2d_plugins.VoxelLayer
          enabled: true

本地代价地图

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      resolution: 0.05
      rolling_window: true
      width: 3
      height: 3
      plugins:
        - plugin: nav2_costmap_2d_plugins.ObstacleLayer

多机器人导航

命名空间隔离

# launch/multi_robot.launch.py
def generate_multi_robot_launch(robot_names: list):
    nodes = []
    for name in robot_names:
        # 每个机器人有独立的节点组
        ns = f'/{name}'
        nodes.extend([
            Node(
                package='nav2_bringup',
                executable='navigation_launch',
                name='nav2_bringup',
                namespace=ns,
                parameters=[f'{name}_nav2_params.yaml'],
                remappings=[
                    ('/cmd_vel', f'{ns}/cmd_vel'),
                    ('/scan', f'{ns}/scan'),
                ],
            ),
        ])
    return LaunchDescription(nodes)

坐标系管理

# 每个机器人使用独立的 map frame
amcl:
  ros__parameters:
    global_frame: $(var robot_name)/map
    robot_base_frame: $(var robot_name)/base_link

故障排查

调试命令

# 查看 Nav2 状态
ros2 action list -t

# 查看代价地图
ros2 topic echo /local_costmap/costmap

# 发送导航目标
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {header: {frame_id: map}, pose: {position: {x: 2.0, y: 1.0}}}}"

# 运行时修改参数
ros2 param set /bt_navigator plugin_lib_names "['nav2_compute_path_to_pose_action_bt_node', 'nav2_follow_path_action_bt_node']"

# 查看行为树状态
ros2 topic echo /bt_navigator/transition_log

相关技能

• navigation/slam — SLAM 定位系统
• navigation/obstacle-avoidance — 动态避障
• navigation/path-planning — 路径规划算法
• wheeled_vehicle/navigation — 轮式车辆导航
• quadruped/navigation — 四足机器人导航
• humanoid/navigation — 人形机器人导航