ROS 2 Lyrical 第五章 导航前置：TF坐标变换、传感器、里程计与SLAM建图

前言

本章完整迁移ROS1导航前置理论，全量适配ROS 2 Lyrical + Nav2 + Gazebo Garden，替换ROS1专属tf/roslaunch/catkin/rospack，统一使用tf2_ros、Python Launch、colcon、ament、DDS话题通信。
本章是Nav2导航栈必备底层基础，所有硬件/仿真机器人通用，严格遵循ROS 2官方标准，覆盖导航机器人硬件要求、TF2坐标变换体系、激光雷达数据发布、里程计生成、底盘速度控制器、gmapping建图、map_server地图存取全流程，代码、启动文件、命令均可在Ubuntu26.0+Lyrical环境直接复现。

本章学习目标

1. 掌握Nav2导航栈运行前置四大硬性硬件/软件条件
2. 理解TF2坐标变换树：广播器、监听器、坐标换算实操
3. 掌握sensor_msgs/LaserScan消息结构，手写仿真激光发布节点
4. 理解里程计nav_msgs/Odometry消息，Gazebo差速驱动生成里程、手写里程计发布代码
5. 理解/cmd_vel速度指令规范，仿真/实体底盘控制器开发逻辑
6. 使用gmapping完成室内栅格地图构建，地图保存与map_server加载
7. RViz2可视化激光、里程轨迹、占用栅格地图

5.1 Nav2导航栈整体架构与机器人前置要求

5.1.1 Nav2整体数据流图

Nav2（ROS2原生导航功能包集）替代ROS1 move_base，核心数据流逻辑不变，适配DDS去中心化通信。

核心组件说明

1. 灰色平台专属模块：TF2变换源、激光雷达驱动、里程计发布、底盘cmd_vel控制器（由开发者根据硬件实现）
2. 白色通用节点：amcl粒子定位、map_server、全局/局部代价地图、全局/局部规划器、恢复行为、move_base主调度
3. 核心话题链路
   • 传感器：/scan（LaserScan）→ TF2变换 → 代价地图
   • 定位：/map地图 + /scan激光 + /odom里程 → AMCL输出机器人位姿
   • 规划：2D目标/goal → 全局路径 → 局部轨迹 → /cmd_vel下发底盘

Nav2机器人强制前置条件（缺一不可）

1. 运动底盘：支持差动轮/全向轮非完整运动，接收geometry_msgs/msg/Twist格式/cmd_vel速度指令；双足机器人仅支持定位，无法自主导航运动。
2. TF2完整坐标树：必须存在base_link/base_footprint、激光雷达laser_link、odom、map四套坐标系变换。
3. 平面测距传感器：发布sensor_msgs/msg/LaserScan或PointCloud2，2D激光雷达为最优方案。
4. 里程计源：持续发布nav_msgs/msg/Odometry话题，提供机器人相对odom坐标系的位姿与线/角速度。

5.2 TF2坐标变换系统（ROS2标准tf2_ros）

TF2用于管理多坐标系平移/旋转关系，自动完成任意两点位姿换算，导航中激光、底盘、地图的空间关系全部依赖TF2，原生替代ROS1 tf库。

5.2.1 核心概念

• TransformBroadcaster：广播父子坐标系偏移（机器人URDF由robot_state_publisher自动广播连杆TF）
• TransformListener：查询任意两个坐标系间实时变换
• 导航标准坐标链：map(全局地图) → odom(里程原点) → base_footprint/base_link(机器人底盘) → laser_link(雷达)

5.2.2 TF2广播器实操（ROS2 C++适配）

文件chapter5_tutorials/src/tf2_broadcaster.cpp

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <cmath>

class TFBroadcaster : public rclcpp::Node
{
public:
  TFBroadcaster() : Node("robot_tf_publisher")
  {
    broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(10ms, std::bind(&TFBroadcaster::timer_cb, this));
  }
private:
  void timer_cb()
  {
    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "base_link";
    t.child_frame_id = "base_laser";
    // 雷达相对底盘：x+0.1m，z+0.2m，无旋转
    t.transform.translation.x = 0.1;
    t.transform.translation.y = 0.0;
    t.transform.translation.z = 0.2;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;
    broadcaster_->sendTransform(t);
  }
  std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TFBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

CMakeLists.txt依赖配置：

find_package(rclcpp REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
add_executable(tf2_broadcaster src/tf2_broadcaster.cpp)
ament_target_dependencies(tf2_broadcaster rclcpp tf2_ros geometry_msgs)
install(TARGETS tf2_broadcaster DESTINATION lib/${PROJECT_NAME})

5.2.3 TF2监听器坐标换算实操

文件chapter5_tutorials/src/tf2_listener.cpp

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/point_stamped.hpp>

class TFListener : public rclcpp::Node
{
public:
  TFListener() : Node("robot_tf_listener"), buffer_(this->get_clock())
  {
    listener_ = std::make_shared<tf2_ros::TransformListener>(buffer_);
    timer_ = this->create_wall_timer(1000ms, std::bind(&TFListener::timer_cb, this));
  }
private:
  void timer_cb()
  {
    geometry_msgs::msg::PointStamped laser_pt;
    laser_pt.header.frame_id = "base_laser";
    laser_pt.header.stamp = this->get_clock()->now();
    laser_pt.point.x = 1.0;
    laser_pt.point.y = 2.0;
    laser_pt.point.z = 0.0;
    geometry_msgs::msg::PointStamped base_pt;
    try {
      buffer_.transform(laser_pt, base_pt, "base_link", tf2::durationFromSec(0.1));
      RCLCPP_INFO(this->get_logger(),
        "laser(1,2,0) -> base_link(%.2f, %.2f, %.2f)",
        base_pt.point.x, base_pt.point.y, base_pt.point.z);
    } catch (tf2::TransformException &ex) {
      RCLCPP_WARN(this->get_logger(), "变换失败: %s", ex.what());
    }
  }
  tf2_ros::Buffer buffer_;
  std::shared_ptr<tf2_ros::TransformListener> listener_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TFListener>());
  rclcpp::shutdown();
  return 0;
}

5.2.4 TF可视化工具

1. 生成坐标系图：

ros2 run tf2_tools view_frames
evince frames.pdf

2. 实时查看坐标树：

ros2 run rqt_tf_tree rqt_tf_tree

注：完整机器人无需手写TF广播，URDF/Xacro配合robot_state_publisher会自动广播所有连杆TF。

5.3 激光雷达数据发布与LaserScan消息

Nav2必须接收标准2D激光扫描消息sensor_msgs/msg/LaserScan，包含角度范围、测距数组、噪声参数。

5.3.1 消息结构说明

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id  # 雷达坐标系laser_link
float32 angle_min  起始角度(rad)
float32 angle_max  终止角度(rad)
float32 angle_increment 单步角度
float32 time_increment 单测距耗时
float32 scan_time 完整扫描周期
float32 range_min 最小测距
float32 range_max 最大测距
float32[] ranges 测距数组
float32[] intensities 回波强度

5.3.2 仿真激光雷达发布节点

chapter5_tutorials/src/laser_pub.cpp

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

class LaserPub : public rclcpp::Node
{
public:
  LaserPub() : Node("laser_scan_pub")
  {
    pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 50);
    timer_ = this->create_wall_timer(1000ms, std::bind(&LaserPub::timer_cb, this));
    count_ = 0;
  }
private:
  void timer_cb()
  {
    sensor_msgs::msg::LaserScan scan;
    scan.header.stamp = this->get_clock()->now();
    scan.header.frame_id = "laser_link";
    scan.angle_min = -1.5708;
    scan.angle_max = 1.5708;
    scan.angle_increment = 3.1416 / 100.0;
    scan.range_min = 0.1;
    scan.range_max = 30.0;
    scan.ranges.resize(100);
    scan.intensities.resize(100);
    for(size_t i=0; i<100; i++){
      scan.ranges[i] = count_;
      scan.intensities[i] = 100 + count_;
    }
    pub_->publish(scan);
    count_++;
  }
  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  int count_;
};

5.3.3 Gazebo仿真雷达

第四章Xacro中配置libgazebo_ros2_laser.so插件，自动发布/robot/laser/scan，RViz2添加LaserScan插件即可可视化红色测距点云。

5.4 里程计Odometry生成与发布

导航依赖nav_msgs/msg/Odometry提供机器人相对odom坐标系的位姿、线/角速度。

5.4.1 消息核心字段

• header.frame_id = "odom"：里程原点坐标系
• child_frame_id = "base_footprint"：机器人底盘
• pose：二维平面位姿(x,y,偏航角) + 协方差
• twist：底盘线速度linear.x、角速度angular.z

5.4.2 Gazebo差速驱动自动生成里程

Gazebo Garden的libgazebo_ros2_skid_steer_drive.so插件订阅/cmd_vel，运动时自动计算并发布/odom，同时广播odom→base_footprintTF变换。
查看里程数据命令：

ros2 topic echo /odom

5.4.3 手写里程计发布节点（实体机器人通用）

通过速度积分计算位移，同时广播TF变换：

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <cmath>

class OdomPub : public rclcpp::Node
{
public:
  OdomPub() : Node("odometry_pub")
  {
    odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
    tf_broad_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(33ms, std::bind(&OdomPub::timer_cb, this));
    last_time_ = this->get_clock()->now();
    x_=0; y_=0; th_=0; vx_=0.1; vth_=0.02;
  }
private:
  void timer_cb()
  {
    rclcpp::Time curr = this->get_clock()->now();
    double dt = (curr - last_time_).seconds();
    last_time_ = curr;
    // 积分计算位移
    double dx = vx_ * cos(th_) * dt;
    double dy = vx_ * sin(th_) * dt;
    double dth = vth_ * dt;
    x_ += dx; y_ += dy; th_ += dth;
    // 发布TF odom->base_footprint
    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = curr;
    t.header.frame_id = "odom";
    t.child_frame_id = "base_footprint";
    t.transform.translation.x = x_;
    t.transform.translation.y = y_;
    t.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,th_),0));
    tf_broad_->sendTransform(t);
    // 发布Odometry消息
    nav_msgs::msg::odom_msg;
    odom_msg.header.stamp = curr;
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "base_footprint";
    odom_msg.pose.pose.position.x = x_;
    odom_msg.pose.pose.position.y = y_;
    odom_msg.pose.pose.orientation = t.transform.rotation;
    odom_msg.twist.twist.linear.x = vx_;
    odom_msg.twist.angular.z = vth_;
    odom_pub_->publish(odom_msg);
  }
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broad_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Time last_time_;
  double x_, y_, th_, vx_, vth_;
};

RViz2添加Odometry插件，红色箭头为机器人运动轨迹。

5.5 底盘速度控制器 /cmd_vel

Nav2规划器输出geometry_msgs/msg/Twist速度指令至/cmd_vel，底盘控制器订阅并驱动电机。

5.5.1 Twist消息规范

geometry_msgs/msg/Vector3 linear  # 仅linear.x前进后退
geometry_msgs/msg/Vector3 angular # 仅angular.z左右转向

5.5.2 控制器核心逻辑

1. 订阅/cmd_vel获取目标线速度、角速度
2. 根据轮距、轮径换算左右轮转速（差速运动学公式）
3. 下发电机驱动指令（仿真由Gazebo插件实现，实体机器人对接串口/Can总线）

5.5.3 键盘遥控测试

sudo apt install ros-lyrical-teleop-twist-keyboard
ros2 run teleop_twist_keyboard teleop_twist_keyboard

5.5.4 rqt_graph通信拓扑

teleop_twist_keyboard → /cmd_vel → gazebo差速驱动插件，同时robot_state_publisher发布关节TF。

5.6 SLAM栅格地图构建（gmapping）

5.6.1 仿真建图启动Python Launch

chapter5_tutorials/launch/gazebo_mapping.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
    model_arg = DeclareLaunchArgument("model")
    # 加载Gazebo室内世界
    gazebo_wg = IncludeLaunchDescription(PythonLaunchDescriptionSource(
        os.path.join(get_package_share_directory("gazebo_ros2"),"launch/willowgarage_world.launch.py")
    ))
    # 机器人描述、spawn
    robot_state_pub = Node(
        package="robot_state_publisher", executable="robot_state_publisher",
        parameters=[{"robot_description": Command(["xacro ", LaunchConfiguration("model")])}]
    )
    spawn_robot = Node(package="gazebo_ros2", executable="spawn_entity.py",
        arguments=["-topic", "robot_description","-entity","robot1","-z","0.1"])
    # gmapping SLAM
    gmapping = Node(package="gmapping", executable="slam_gmapping",
        remappings=[("scan","/robot/laser/scan")],
        parameters=[{"base_frame":"base_footprint"}])
    # RViz地图可视化
    rviz = Node(package="rviz2", executable="rviz2",
        arguments=["-d", get_package_share_directory("chapter5_tutorials")+"/launch/mapping.rviz"])
    return LaunchDescription([model_arg, gazebo_wg, robot_state_pub, spawn_robot, gmapping, rviz])

启动命令：

ros2 launch chapter5_tutorials gazebo_mapping.launch.py model:=$(ament_index_get_resource robot1_description urdf/robot1_base_04.xacro)

操作：键盘遥控机器人遍历室内全部区域，RViz2 OccupancyGrid插件实时生成灰色占用栅格地图。

5.6.2 地图保存map_saver

ros2 run map_server map_saver -f my_map

生成两个文件：

1. my_map.pgm：灰度栅格图像（黑色障碍物、白色可通行、灰色未知）
2. my_map.yaml：地图分辨率、原点、阈值配置文件

5.6.3 地图加载map_server

ros2 run map_server map_server my_map.yaml

或写入launch文件长期加载，为AMCL定位提供全局地图。

本章小结

1 Nav2导航运行必须具备：差动底盘、2D激光、完整TF2树、标准里程计四大基础模块，缺一不可；
2 TF2统一管理多传感器空间坐标，URDF自动广播连杆变换，无需重复手写广播代码；
3 激光雷达统一输出LaserScan，里程计输出Odometry，底盘接收Twist型/cmd_vel，三者为导航数据输入源；
4 Gazebo仿真内置差速驱动、激光插件，快速验证整套导航前置链路；
5 gmapping基于激光+里程实现2D栅格SLAM，map_server完成地图持久化加载，为下一章自主导航提供全局地图与AM定位基础。

下一章将讲解Nav2完整配置：AMCL自适应蒙特卡洛定位、全局/局部代价地图、全局/局部路径规划器、自主避障与2D目标导航实操。