ROS2 进阶实战：从 Launch、TF2、ros2_control 到 Nav2 自主导航系统 Lv2

适用版本：ROS 2 Humble / Jazzy（Ubuntu 22.04 / 24.04）
前置要求：已掌握 ROS2 五大核心概念（Node / Topic / Service / Action / Parameter）

目录

1. 进阶全景：从“单个节点”到“整车自主导航”
2. Launch 文件：工程化一键启动体系
   • 2.1 为什么必须用 Python Launch？
   • 2.2 实战：编写多节点协同启动脚本
   • 2.3 进阶技巧：参数传递、命名空间与话题重映射（Remapping）
3. TF2 坐标变换系统：机器人的空间感官
   • 3.1 坐标树原理：map -> odom -> base_link -> laser_frame
   • 3.2 实战：静态坐标广播器（Static Broadcaster）
   • 3.3 实战：动态坐标监听与坐标点解算（Transform Listener）
4. ros2_control：工业级软硬件解耦控制框架
   • 4.1 传统驱动 vs ros2_control 标准化框架
   • 4.2 核心三要素：Hardware Interface、Controller Manager 与 Controller
   • 4.3 实战：差速底盘（Diff Drive Controller）配置实战
5. Nav2 自主导航栈：让机器人真正动起来
   • 5.1 Nav2 内部架构与五大核心概念的结合
   • 5.2 代价地图（Costmap）与行为树（Behavior Tree）
   • 5.3 实战：手写 Python 导航脚本（调用 /navigate_to_pose 动作）
6. 综合实战：一键启动完整自主导航小车
7. 核心排错与最佳实践速查

1、进阶全景：从“单个节点”到“整车自主导航”

在上一篇中，我们学习了单个节点的编写与通信。然而，在真实的机器人产品中，没有一个功能是单个节点能独立搞定的。

以“让小车自主避障开到客厅”为例，整个系统的数据流向如下：

2、Launch 文件：工程化一键启动体系

2.1 为什么必须用 Python Launch？

在 ROS 1 中主要使用 XML 文件编写 Launch，而在 ROS 2 中，推荐使用 Python 编写 Launch 脚本（.launch.py）。

Python Launch 的杀手级优势：

• 逻辑判断能力：可以使用 if/else、for 循环根据环境变量或输入参数动态决定启动哪些节点。
• 事件驱动（Event Handling）：能够实现“等 A 节点启动完成并返回就绪后，再启动 B 节点”。
• 动态重命名与传参：随时加载 YAML 参数，重映射话题名，统一加入命名空间。

---

2.2 实战：编写多节点协同启动脚本

假设我们要把上一期写的 speed_publisher（发布节点）和 speed_subscriber（订阅节点）以及参数配置一次性启动。

在功能包目录中创建 launch/ 文件夹：

mkdir -p ~/ros2_learning_ws/src/learning_pkg/launch

新建 learning_pkg/launch/robot_bringup.launch.py：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    """
    ROS2 Launch 脚本必须包含 generate_launch_description 函数，
    并返回一个 LaunchDescription 对象。
    """

    # 1. 节点 A：速度发布节点
    speed_pub_node = Node(
        package='learning_pkg',            # 功能包名称
        executable='speed_publisher',      # 可执行文件名（在 setup.py 中注册的名）
        name='my_speed_publisher',         # 节点运行时名称（覆盖代码中的默认名）
        output='screen',                   # 将日志输出打印到终端屏幕
        parameters=[{                      # 节点内部参数设置
            'publish_rate': 20.0,
            'use_sim_time': False
        }]
    )

    # 2. 节点 B：速度订阅与里程计监听节点
    speed_sub_node = Node(
        package='learning_pkg',
        executable='speed_subscriber',
        name='my_speed_subscriber',
        output='screen',
        # 话题重映射（Remapping）：把代码中的 /cmd_vel 改为监听 /robot1/cmd_vel
        remappings=[
            ('/cmd_vel', '/robot1/cmd_vel')
        ]
    )

    # 3. 构造并返回启动描述对象
    return LaunchDescription([
        speed_pub_node,
        speed_sub_node,
    ])

---

2.3 进阶技巧：动态传参与加载外部 YAML 参数文件

新建 learning_pkg/launch/advanced_bringup.launch.py：

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 1. 获取功能包的安装共享路径
    pkg_share = get_package_share_directory('learning_pkg')
    default_param_file = os.path.join(pkg_share, 'config', 'robot_params.yaml')

    # 2. 声明命令行可配置的 Launch 参数
    # 外部可以通过: ros2 launch learning_pkg ... use_sim:=true 修改
    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='是否使用仿真时钟 (Gazebo 等仿真时设为 true)'
    )

    params_file_arg = DeclareLaunchArgument(
        'params_file',
        default_value=default_param_file,
        description='参数文件路径'
    )

    # 3. 创建节点并加载外部 YAML 参数文件
    robot_system_node = Node(
        package='learning_pkg',
        executable='robot_system',
        name='robot_system_node',
        output='screen',
        # 从声明的 Launch 参数中动态获取
        parameters=[
            LaunchConfiguration('params_file'),
            {'use_sim_time': LaunchConfiguration('use_sim_time')}
        ]
    )

    return LaunchDescription([
        use_sim_time_arg,
        params_file_arg,
        robot_system_node
    ])

修改 setup.py 确保安装 launch 目录：

打开 learning_pkg/setup.py，在 data_files 中添加 launch 文件的安装规则：

import os
from glob import glob

data_files=[
    ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
    ('share/' + package_name, ['package.xml']),
    # 包含所有 launch 文件
    (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
    # 包含所有 config 参数文件
    (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
]

运行与验证：

cd ~/ros2_learning_ws
colcon build --packages-select learning_pkg
source install/setup.bash

# 一键启动
ros2 launch learning_pkg robot_bringup.launch.py

3、TF2 坐标变换系统：机器人的空间感官

3.1 坐标树原理：为什么机器人需要坐标变换？

机器人的各个部件在物理空间中是有相对位置的。例如：

• 激光雷达装在机器人底盘中心的前方 0.2 米、上方 0.1 米 处（laser_frame）。
• 机器人底盘在房间世界坐标系中的坐标为 (x: 2.0, y: 3.0)（base_link）。

当激光雷达检测到前方 1.0 米 处有一个障碍物时，障碍物在房间中的绝对坐标到底是多少？

这就是 TF2 解决的问题。它建立了一棵有向无环的坐标树（TF Tree）：

3.2 实战：静态坐标广播器（Static Broadcaster）

如果一个传感器固定在小车上永远不会移动（例如雷达装在底盘中心前 0.2m、高 0.15m 处），我们需要向 TF 系统广播一个静态坐标变换（Static Transform）。

新建 learning_pkg/learning_pkg/static_tf_broadcaster.py：

import sys
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
import math


class StaticTFBroadcaster(Node):

    def __init__(self):
        super().__init__('static_tf_broadcaster')

        # 1. 创建静态广播器
        self.tf_broadcaster = StaticTransformBroadcaster(self)

        # 2. 构造静态坐标变换消息
        t = TransformStamped()

        # 时间戳
        t.header.stamp = self.get_clock().now().to_msg()
        # 父坐标系（基准坐标系）
        t.header.frame_id = 'base_link'
        # 子坐标系（挂载的传感器坐标系）
        t.child_frame_id = 'laser_frame'

        # 3. 设置相对平移（单位：米）
        t.transform.translation.x = 0.2    # 前方 20cm
        t.transform.translation.y = 0.0    # 左右居中
        t.transform.translation.z = 0.15   # 上方 15cm

        # 4. 设置相对旋转（四元数：Quaternion，此处假设无旋转）
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0       # w=1 表示旋转角为 0

        # 5. 发布静态变换
        self.tf_broadcaster.sendTransform(t)
        self.get_logger().info('✅ 静态 TF 广播已就绪: base_link -> laser_frame')


def main(args=None):
    rclpy.init(args=args)
    node = StaticTFBroadcaster()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

---

3.3 实战：动态坐标监听与跨坐标系目标点换算（TF Listener）

现在，假设激光雷达在 laser_frame 坐标系下检测到一个障碍物点 (x: 1.0, y: 0.0, z: 0.0)，我们写一个节点实时计算出这个障碍物在底盘 base_link 和里程计 odom 下的真实坐标。

新建 learning_pkg/learning_pkg/tf_listener_demo.py：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PointStamped
import tf2_ros
import tf2_geometry_msgs  # 必须引入，用于 PointStamped 跨坐标系转换


class TFListenerDemo(Node):

    def __init__(self):
        super().__init__('tf_listener_demo')

        # 1. 创建 TF 缓冲器与监听器
        # Buffer 会在后台开辟线程自动维护最近一段时间的所有坐标变换树
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)

        # 2. 定时器：每秒进行一次坐标换算查询
        self.timer = self.create_timer(1.0, self.transform_point)
        self.get_logger().info('TF 监听与点转换演示节点已启动...')

    def transform_point(self):
        try:
            # 构造雷达坐标系下的一个障碍物点 (1.0, 0.0, 0.0)
            point_in_laser = PointStamped()
            point_in_laser.header.frame_id = 'laser_frame'
            point_in_laser.header.stamp = rclpy.time.Time().to_msg() # 表示获取最新变换
            point_in_laser.point.x = 1.0
            point_in_laser.point.y = 0.0
            point_in_laser.point.z = 0.0

            # 查询并执行坐标变换：将点从 laser_frame 转换到底盘 base_link
            # timeout 设置超时等待时间，防止变换尚未广播时抛异常
            point_in_base = self.tf_buffer.transform(
                point_in_laser,
                'base_link',
                timeout=rclpy.duration.Duration(seconds=0.5)
            )

            self.get_logger().info(
                f'📍 [坐标转换成功]\n'
                f'  雷达坐标系 (laser_frame): ({point_in_laser.point.x}, {point_in_laser.point.y})\n'
                f'  底盘坐标系 (base_link)  : ({point_in_base.point.x:.2f}, {point_in_base.point.y:.2f})'
            )

        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
            self.get_logger().warn(f'等待 TF 树建立中... 原因: {str(e)}')


def main(args=None):
    rclpy.init(args=args)
    node = TFListenerDemo()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

TF 命令行调试神技：

# 1. 命令行快速发布静态 TF（开发调试最常用，无需写代码）
# 格式: x y z yaw pitch roll parent_frame child_frame
ros2 run tf2_ros static_transform_publisher 0.2 0 0.15 0 0 0 base_link laser_frame

# 2. 查看两个坐标系之间的实时相对位姿
ros2 run tf2_ros tf2_echo base_link laser_frame

# 3. 一键生成当前系统中所有坐标系树状拓扑图 PDF
ros2 run tf2_tools view_frames
# 会在当前目录生成 frames.pdf，打开即可查看完整的 TF 树

4、ros2_control：工业级软硬件解耦控制框架

4.1 传统驱动 vs ros2_control 标准化框架

传统机器人开发中，很多开发者直接在 Python 节点里打开串口往电机写指令。这种做法有致命缺陷：

1. 换硬件全重写：换个电机品牌或从仿真切到真机，整个驱动节点推倒重来。
2. 实时性极差：Python 垃圾回收机制和系统调度抖动，无法保证 1kHz 的确定性电机闭环。

ros2_control 将底层电机驱动抽象为标准接口：

4.2 差速小车控制器配置文件（controllers.yaml）

在功能包的 config/ 目录下创建 controllers.yaml：

controller_manager:
  ros__parameters:
    update_rate: 100 # 控制周期 100Hz (10ms)

    # 声明挂载的两个标准控制器
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    diff_drive_base_controller:
      type: diff_drive_controller/DiffDriveController

# 差速驱动控制器详细参数
diff_drive_base_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]

    wheel_separation: 0.30  # 左右轮距 30cm
    wheel_radius: 0.05      # 轮子半径 5cm

    # 话题与坐标系配置
    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_link
    enable_odom_tf: true    # 自动广播 odom -> base_link 的 TF 变换！

    # 速度与加速度限制
    linear.x.max_velocity: 1.0
    linear.x.max_acceleration: 1.0
    angular.z.max_velocity: 2.0
    angular.z.max_acceleration: 2.0

核心威力：配置了 diff_drive_controller 后，它会自动监听 /cmd_vel，自动解算轮速，并自动发布 /odom 话题和 odom -> base_link 的 TF 树！开发者无需手动计算里程计积分。

5、Nav2 自主导航栈：让机器人真正动起来

5.1 Nav2 内部架构与五大核心概念的结合

Nav2 是 ROS 2 的自主移动机器人大脑。它是如何完美融合五大基础概念的？

5.2 实战：编写 Python 导航任务脚本（调用 Nav2 Action）

新建 learning_pkg/learning_pkg/nav_to_goal_client.py：

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
import math


class NavToGoalClient(Node):

    def __init__(self):
        super().__init__('nav_to_goal_client')

        # 1. 创建 Nav2 官方导航动作客户端
        self._action_client = ActionClient(
            self,
            NavigateToPose,
            'navigate_to_pose'
        )

    def send_navigation_goal(self, x: float, y: float, theta_degree: float):
        """下发目标点坐标 (x, y, 角度)"""
        self.get_logger().info('正在连接 Nav2 导航服务器...')
        self._action_client.wait_for_server()

        # 2. 构造目标位姿 (Goal)
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'       # 目标点基于 map 全局地图坐标系
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()

        # 设置目标位置
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.position.z = 0.0

        # 将欧拉角转化为四元数 (朝向)
        rad = math.radians(theta_degree)
        goal_msg.pose.pose.orientation.x = 0.0
        goal_msg.pose.pose.orientation.y = 0.0
        goal_msg.pose.pose.orientation.z = math.sin(rad / 2.0)
        goal_msg.pose.pose.orientation.w = math.cos(rad / 2.0)

        self.get_logger().info(f'🚀 目标点已发送: X={x}m, Y={y}m, 角度={theta_degree}°')

        # 3. 异步发送目标并注册反馈回调
        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg,
            feedback_callback=self.feedback_callback
        )
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('❌ 导航目标被拒绝（可能目标点在障碍物内部）！')
            return

        self.get_logger().info('✅ 目标已被接受，Nav2 正在自主规划路径与避障...')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.result_callback)

    def feedback_callback(self, feedback_msg):
        """实时接收 Nav2 返回的进度反馈"""
        feedback = feedback_msg.feedback
        # 获取距离目标点的剩余距离和已导航时间
        distance_remaining = feedback.distance_remaining
        navigation_time = feedback.navigation_time.sec
        self.get_logger().info(
            f'📊 [导航中] 剩余距离: {distance_remaining:.2f} 米 | '
            f'耗时: {navigation_time} 秒',
            throttle_duration_sec=1.0 # 限制每秒最多打印一次
        )

    def result_callback(self, future):
        status = future.result().status
        # status 4 代表 SUCCEEDED
        if status == 4:
            self.get_logger().info('🎉 恭喜！小车已成功避障到达目的地！')
        else:
            self.get_logger().warn(f'⚠️ 导航结束，状态码: {status}')


def main(args=None):
    rclpy.init(args=args)
    node = NavToGoalClient()

    # 目标：导航到地图坐标 (x: 2.5米, y: 1.5米, 角度朝向 90度)
    node.send_navigation_goal(x=2.5, y=1.5, theta_degree=90.0)

    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

6、综合实战：一键启动完整自主导航小车

我们来把所有内容串联起来，进行一次标准的仿真导航演练：

Step 1：安装 Nav2 与 经典小车仿真包

# 安装 Nav2 核心包与 TurtleBot3 仿真包
sudo apt update
sudo apt install -y \
  ros-$ROS_DISTRO-navigation2 \
  ros-$ROS_DISTRO-nav2-bringup \
  ros-$ROS_DISTRO-turtlebot3*

Step 2：启动 Gazebo 仿真环境与 Nav2 导航系统

打开终端 1（设置小车模型并一键启动 Gazebo + Nav2 + RViz2）：

export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models

# 一键启动包含建图、TF、Costmap、Nav2 核心的完整仿真世界
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

打开终端 2（启动 Nav2 导航系统与 RViz 交互界面）：

export TURTLEBOT3_MODEL=waffle
ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True

Step 3：运行我们自己编写的 Python 导航脚本

打开终端 3：

# 启动刚才写的 Python 导航客户端下发坐标任务
ros2 run learning_pkg nav_to_goal_client

在 RViz2 可视化界面中，你将看到：

1. 全局路径规划线（红色线条）避开地图障碍物；
2. 小车根据 ros2_control 输出速度向前移动；
3. 终端实时打印剩余距离反馈；
4. 到达目标点后小车自动平稳刹停并旋转至指定朝向！

7、核心排错与最佳实践速查

常见故障现象	根本原因定位	解决办法
Nav2 报错: Timed out waiting for transform from base_link to map	TF 树断裂，没有定位或缺少静态 TF	运行 ros2 run tf2_tools view_frames，检查 map->odom->base_link 是否连通
下发导航目标后小车完全不动	1. 未下发初始位姿 (2D Pose Estimate) 2. 机器人陷入 Costmap 膨胀层	1. 在 RViz 中先用 "2D Pose Estimate" 点一下小车当前位置 2. 调小 inflation_radius 参数
Launch 报错找不到可执行文件	setup.py 中未在 console_scripts 注册或未 colcon build	检查 setup.py，重新执行 colcon build --symlink-install 并 source install/setup.bash
动态调整参数不生效	节点内部未添加 add_on_set_parameters_callback	在节点 __init__ 中注册参数更新回调函数

---

💡 如果在编译或运行实战代码时遇到问题，欢迎在评论区贴出报错日志共同交流！