ROS2 建图和导航实战

发表于 2025/09/02 更新于 2025/09/23

作者 wsxq2

23 分钟阅读

ROS2 建图和导航实战

概述

我们在 ROS1 建图和导航实战中使用的 ROS 版本为 ROS1 noetic，基于 Ubuntu 20.04, 使用的 SLAM 算法是 gmapping 和 cartographer，而导航算法是 navigation。在其中的“下一步计划”章节中，提到了“尝试 ROS2 及其新特性”，本文即是相应的尝试。本文使用的 ROS 版本为 ROS2 humble，基于 Ubuntu 22.04，SLAM 算法为 cartographer，导航算法为 nav2。

此外，本次没有再使用 ros-mobile-robots/diffbot 作为参考，主要使用 AI 基于核心文件生成相关代码，以便快速进行测试，同时保持项目的简洁性。本项目的源码参见 https://github.com/wsxq2/nav_car

硬件框图

本文使用的硬件和 ROS1 建图和导航实战完全相同，硬件框图没有任何变化，但为了方便阅读，这里依然展示如下：

flowchart TB
    n7["STM32单片机"] -- RS485 --- n8["电机驱动器1"]
    n8 --- n19["电机1"]
    n7 -- "<span style=color:>RS485</span>" --- n20["电机驱动器2"]
    n20 --- n21["电机2"]
    n7 -- ETH --- n22["工控机"]
    n18["激光雷达"] -- ETH --- n22
    n17["遥控手柄无线接收器"] -- USB --- n22
    n23["IMU"] -- RS232 --- n22
    n24["遥控手柄"] -. 无线 .- n17
    n25["WIFI网卡"] -- USB --- n22
    n22 -- USB --- n7
    n7 -- SWD --- n22
    n7@{ shape: rect}
    n8@{ shape: rect}
    n19@{ shape: rect}
    n20@{ shape: rect}
    n21@{ shape: rect}
    n22@{ shape: rect}
    n18@{ shape: rect}
    n17@{ shape: rect}
    n23@{ shape: rect}
    n24@{ shape: rect}
    n25@{ shape: rect}

软件框图

由于 ROS2 和 ROS1 存在较大差异，所以本次实战的软件框图也和之前 ROS1 的存在较大差异，本次实战对应的软件框图如下所示：

flowchart TB
 subgraph s4["工控机"]
        n13["hardware component"]
        n14["ros2 control"]
        n16["SLAM"]
        n17["joy_linux"]
        n21["IMU_driver"]
        n20["lidar_driver"]
        n23["teleop_twist_joy"]
        n24["导航"]
        n25["rviz2"]
  end
    n7["STM32单片机"] <-- 原始转速、实时位置 --> n8["电机驱动器"]
    n13 <-- ROS格式的转速、实时位置 --> n14
    n14 -- odom --> n16
    n18["激光雷达"] -- 原始激光雷达数据 --> n20
    n19["IMU"] -- 原始IMU数据 --> n21
    n21 -- imu --> n16
    n20 -- scan --> n16
    n13 <-- 易于传输的转速、实时位置 --> n7
    n17 -- joy --> n23
    n23 -- cmd_vel --> n14
    n22["遥控手柄"] --> n17
    n16 -- map --> n24
    n24 -- cmd_vel --> n14
    n25 -- dst_pose --> n24
    n16 -- cur_pose --> n24

    n13@{ shape: rect}
    n14@{ shape: rect}
    n16@{ shape: rect}
    n17@{ shape: rect}
    n21@{ shape: rect}
    n20@{ shape: rect}
    n23@{ shape: rect}
    n24@{ shape: rect}
    n25@{ shape: rect}
    n7@{ shape: rect}
    n8@{ shape: rect}
    n18@{ shape: rect}
    n19@{ shape: rect}
    n22@{ shape: rect}

其中遥控手柄走的无线链路，对端通过 USB 接收器接收其命令，USB 接收器插在工控机的 USB 接口上，在 Ubuntu 系统中体现一个设备文件：/dev/input/js0。ROS2 组件中的 joy_linux 包提供了这类遥控手柄的底层驱动，即读取/dev/input/js0文件，然后发布 joy 主题。然后再由 teleop_twist_joy 包将该主题转换为 cmd_vel 主题，下发到 ros2 control，以实现小车的运动控制，即让小车“走”起来。

在 ROS2 中，运动控制主要由 ros2 control 框架负责，该框架要求实现一个 hardware component，以和底层硬件通信，完成相应的控制和状态获取。在这里，我们实现的 hardware component 将和 STM32 单片机进行通信，需要按照约定的通信协议进行编码。

ros2 control 会将读取的实时位置（转动的角度）转换为 odom 主题，提供给 SLAM 算法，SLAM 算法还会订阅激光雷达驱动发布 scan 主题，以及 IMU 驱动发布的 imu 主题，实现定位和建图，然后 SLAM 算法会将定位结果和建图结果提供给导航算法，导航算法会在这些信息的基础上结合从 rviz2 获取的目标位姿进行路径规划，完成后发布 cmd_vel 主题到 ros2 control，从而控制小车自动行走到目的地。

由于使用的工控机使用的模组是 NVIDIA Jetson Xavier NX，而官方没有提供 ubuntu 22.04 的系统镜像（最高仅支持到 ubuntu 20.04），因此我们使用了 docker，从而实现在 Ubuntu 20.04 上运行 Ubuntu 22.04 环境。

单片机

单片机代码没有任何变化，这是因为我们约定了通信协议，分离了两个系统，只要协议正确即可通信。

hardware component

这部分的代码可直接由 AI 生成，使用的提示词如下：

我需要完成 ROS2 control 中 hardware component 的代码，我的硬件只有两个使用转速方式控制的轮子，其通信接口为TCP，硬件是客户端，所以我需要写服务端，其监听地点0.0.0.0，端口号3011，接收连接后按照以下协议读取：
字段功能数据类型字节数字节偏移备注
HEAD 帧头 U16 2 0 固定为0x55AA
LEN 帧长 U16 2 2 单位为字节，整个帧的长度，不同命令的帧长不同
CMD 命令 U8 1 4 0x02表示状态上报
MOTOR_LEFT 左电机转速 INT32 4 5 单位为0.001rpm，向前时应为正
MOTOR_RIGHT 右电机转速 INT32 4 9 单位为0.001rpm，向前时应为正
POS_LEFT 左电机里程 INT32 4 13 单位为0.001度
POS_RIGHT 右电机里程 INT32 4 17 单位为0.001度
CRC16 校验 U16 2 21 不包括帧头和自身,采用 MODBUS RTU 算法
读取的内容在进行单位转换等处理后放置到相应的成员变量中，然后发送命令时同样需要进行单位转换等，然后通过以下协议发送到硬件：
字段功能数据类型字节数字节偏移备注
HEAD 帧头 U16 2 0 固定为0x55AA
LEN 帧长 U16 2 2 单位为字节，整个帧的长度，不同命令的帧长不同
CMD 命令 U8 1 4 0x01表示运动控制
MOTOR_LEFT 左电机转速 INT32 4 5 单位为0.001rpm,向前时应为正
MOTOR_RIGHT 右电机转速 INT32 4 9 单位为0.001rpm,向前时应为正
RESET_ODOM 是否复位里程计 U8 1 13 0: 不复位，1: 复位
CRC16 校验 U16 2 14 不包括帧头和自身,采用 MODBUS RTU 算法
协议字节序均为大端。需要支持异常处理，网络断开时自动重连，并打印相关日志。

字段	功能	数据类型	字节数	字节偏移	备注
HEAD	帧头	U16	2	0	固定为0x55AA
LEN	帧长	U16	2	2	单位为字节，整个帧的长度，不同命令的帧长不同
CMD	命令	U8	1	4	0x02表示状态上报
MOTOR_LEFT	左电机转速	INT32	4	5	单位为0.001rpm，向前时应为正
MOTOR_RIGHT	右电机转速	INT32	4	9	单位为0.001rpm，向前时应为正
POS_LEFT	左电机里程	INT32	4	13	单位为0.001度
POS_RIGHT	右电机里程	INT32	4	17	单位为0.001度
CRC16	校验	U16	2	21	不包括帧头和自身,采用 MODBUS RTU 算法

字段	功能	数据类型	字节数	字节偏移	备注
HEAD	帧头	U16	2	0	固定为0x55AA
LEN	帧长	U16	2	2	单位为字节，整个帧的长度，不同命令的帧长不同
CMD	命令	U8	1	4	0x01表示运动控制
MOTOR_LEFT	左电机转速	INT32	4	5	单位为0.001rpm,向前时应为正
MOTOR_RIGHT	右电机转速	INT32	4	9	单位为0.001rpm,向前时应为正
RESET_ODOM	是否复位里程计	U8	1	13	0: 不复位，1: 复位
CRC16	校验	U16	2	14	不包括帧头和自身,采用 MODBUS RTU 算法

使用 Claude Sonnet 4 生成，生成的代码基本无需改动。

ros2 control

ROS2 Control 框架的核心部分就两个：一个是前述的 hardware component，用于和硬件通信；另一个是 controller，即控制器，实现运动学的正逆解。

具体到我们的小车，需要使用的控制器即 diff_drive_controller。使用控制器的要点是正确编写其 YAML 配置文件，此次使用配置文件如下（基于 ROS1 做了些许修改）：

  
controller_manager:
  ros__parameters:
    update_rate: 50  # Hz

    # Joint state broadcaster - broadcasts all joint states
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    # Base swerve drive controller
    mobile_base_controller:
      type: diff_drive_controller/DiffDriveController

joint_state_broadcaster:
  ros__parameters:
    extra_joints: 
      - rear_caster_wheel_joint
      - rear_caster_rotation_joint

mobile_base_controller:
  ros__parameters:
    left_wheel_names: ["back_left_wheel_joint"]
    right_wheel_names: ["back_right_wheel_joint"]
    
    wheel_separation: 0.446  # Distance between left and right wheels (m)
    wheels_per_side: 1
    wheel_radius: 0.16      # Wheel radius (m)
    odom_frame_id: "odom"
    base_frame_id: "base_footprint"
    pose_covariance_diagonal: [0.01, 0.01, 0.001, 0.001, 0.001, 0.3]
    twist_covariance_diagonal: [0.01, 0.01, 0.001, 0.001, 0.001, 0.3]
    
    # Velocity and acceleration limits for the robot
    linear:
      x:
        has_velocity_limits    : true
        max_velocity           : 1.2   # m/s
        min_velocity           : -1.2  # m/s
        has_acceleration_limits: true
        max_acceleration       : 1.0   # m/s^2
        min_acceleration       : -1.0  # m/s^2
    angular:
      z:
        has_velocity_limits    : true
        max_velocity           : 3.14   # rad/s
        min_velocity           : -3.14  # rad/s
        has_acceleration_limits: true
        max_acceleration       : 3.14   # rad/s^2
        min_acceleration       : -3.14  # rad/s^2

URDF

由于小车的机械结构未发生变化，所以 URDF 文件也无需发生实质性的修改，只需从 ROS1 移植到 ROS2 即可。如何移植呢？直接使用 AI 即可。

传感器标定

此次实战主要使用的传感器是激光雷达和里程计（IMU 暂未使用），激光雷达的外参和内参相比 ROS1 时均未发生变化，里程计由于重写了 hardware component，所以需要重新标定。标定过程和结果暂不详述。

TF

和 ROS1 中的情形相同，主要涉及三部分 TF：map->odom, odom->base_footprint, base_footprint->laser_link及轮子相关 TF。其中map->odom由 cartographer 算法提供，odom->base_footprint由 ros2 control 中的 diff_drive_controller 提供，base_footprint->laser_link及轮子相关 TF 由 URDF 定义，然后由 ros2 control 中的 joint_state_broadcaster/JointStateBroadcaster 提供。

ROS2 中可使用 ros2 run tf2_tools view_frames 和 ros2 run tf2_ros tf2_echo odom base_footprint 等命令查看 TF 变换细节。

SLAM

SLAM 算法主要使用 cartographer。有意思的是，ROS2 中内置了 cartographer，无需手动下载源码编译安装，即可以直接使用 apt 安装：

apt install ros-humble-cartographer ros-humble-cartographer-ros -y

正确使用 cartographer 的要点主要有两个部分：一是要保证传感器输入是合理的；二是要保证 lua 配置（cartographer 使用 lua 文件作为配置文件）是合理的。这里说的是“合理”而非“正确”，这是因为“正确”要求太高了，且通常没有绝对的正确。cartographer 的输入只有激光雷达是必须的，其他都是可选的，但这次我们用到了里程计，所以里程计也是重要的输入。通常在完成前述的“传感器标定”后，输入就应该是合理的了。所以配置文件才是重点。

配置文件方面，我们直接采用了 ROS1 实战中的配置，发现是可用的，这是极好的，避免花费大量时间调整参数。

遇到过的问题

在 Windows 上远程桌面（rdp）时，对端 Ubuntu 经常自动锁屏，从而需要输入密码？
设置中关闭自动锁屏功能即可（Settings->Privacy->Screen->Automatic Screen Lock）

通过 ssh 访问方式访问 github 的 repo 时，在执行 git push 命令后会卡死？

尝试了 ssh 的代理，即在 ~/.ssh/config 中添加类似 ProxyCommand nc -X connect -x 192.168.3.107:7890 %h %p的配置，但效果不大（准确地说，是没有任何效果）。

最终发现在 vscode 中，通过 HTTPS 方式访问 github 的 repo，在执行git push后无需输入密码，且可正常使用代理，所以后续可以使用 vscode。无需输入密码的原因是 vscode 自动处理了认证，详见Using Git source control in VS Code 和 git config -l 输出中的如下内容：

credential.helper=!f() { /home/dev/.vscode-server/bin/dfaf44141ea9deb3b4096f7cd6d24e00c147a4b1/node /tmp/vscode-remote-containers-5e5ecc7c-9ab1-4d29-a4c3-529026230425.js git-credential-helper $*; }; f

如果没有使用 vscode 的条件，且既希望不卡又能安全地进行认证，则可以使用 github 官方的 token 方式认证 HTTPS repo，token 方式有两种：局部的和全局的。前者可以设定授予的具体权限，如创建一个指定 repo 能 push 的 token，由于其粒度较细，所以官方更推荐。后者则具有整个用户的所有权限，如对当前用户任意 repo 的 push 权限。详见 Managing your personal access tokens - GitHub Docs。

在开发过程中，使用了 docker 搭建开发环境（结合了 VSCode 的 dev container 和 docker compose），但一开始使用的是 X86_64 的台式机，所以构建的容器也是 X86_64 平台的，但实际运行平台是 ARM64（jetson），因此，为了尽可能还原工控机的环境，需要让 docker compose 支持多平台构建。docker 官方对此提供了完善的支持，详见以下链接：

Multi-platform | Docker Docs。需要注意每次运行前需要执行docker run --privileged --rm tonistiigi/binfmt --install all以启动虚拟化环境，否则会报错 exec format error。可以添加相应服务到 docker-compose.yaml 文件中，如下所示：

  
name: navcar-${USER}
services:
  emulator:
    image: tonistiigi/binfmt
    privileged: true
    command: --install arm64
    network_mode: bridge
    restart: "no"
  navcar-dev:
    depends_on:
      - emulator
    build:
    ...

docker-compose.yaml 中指定构建平台
docker-compose.yaml 中指定运行平台

多个用户使用同一主机上的 docker compose 进行同一项目的开发时，需要将项目名取为不同的名称，否则会冲突。可在项目名中直接使用USER环境变量以作区分，如：
1 2 3 4 name: navcar-${USER} services: navcar-dev: ...
ROS2 跨 docker 容器和主机如何进行通信？具体情形描述如下：一个 ROS2 运行于 arm64 工控机（jetson）上的 Docker 容器中，另一个 ROS2 直接运行于 x86_64 架构的 PC 上，两边均是 Ubuntu 22.04 系统。
其实非常简单，满足以下条件即可：
1. docker 容器在启动时设置--net=host（或者在 docker-compose.yaml 中做相应配置）
2. 让工控机和 PC 处于同一局域网
3. 让两边的环境变量ROS_DOMAIN_ID相同
4. 让两边的环境变量ROS_LOCALHOST_ONLY为 0
遇到问题时，可以使用 ros2 multicast send/receive 进行粗测，再使用例程 ros2 run demo_nodes_cpp listener/talker细测。如果两者都可以的话，说明就是 OK 的。
注意，即使OK了，ros2 topic list 命令也不会列出对端的 topic，这是为了降低带宽，仅在有人订阅时才进行相应的通信（进行通信后方可使用前述命令查看到对端的 topic）。
另可参见吃灰树莓派第二篇|不设置主从机，看ROS2如何进行多机通信_ros2通讯延迟、测试-CSDN博客。
如果通信不稳定，可以考虑 Using Fast DDS Discovery Server as discovery protocol [community-contributed] — ROS 2 Documentation: Humble documentation
关于 joystick（手柄）的底层驱动，我应该选择 joy 包还是 joy_linux 包？
前者使用的是 Linux 中的事件设备，例如/dev/input/event1，后者使用的是 Linux 中的 js 设备，例如/dev/input/js0，所以看实际情况做出正确的选择。
相关依据参见源码和 ROS Package: joy 中的“Technical note”章节。
遥控手柄无法控制小车？
遥控手柄控制小车涉及多个环节，需要逐层排查，务必戒骄戒躁。可按以下步骤进行：
1. 先尝试 jstest 命令（该命令可通过 sudo apt install joystick 获得），检查/dev/input/js0是否能正常接收数据。如果加sudo才可以，则需要给当前用户添加相应的权限，比如sudo chown dev:dev /dev/input/js0
2. 能正常接收后，再尝试 joy 包或者 joy_linux 包，或者其他相关包，看能否正确发布 joy 主题
3. 如果可以，再尝试 teleop_twist_joy 包，看能否正确发布 cmd_vel 主题
另请参见 https://wiki.ros.org/joy/Troubleshooting
使用 vscode + dev container 时如何避免自动复制 .gitconfig？具体描述如下：我使用的是 windows 笔记本，使用 vscode 通过 remote ssh 连接到远程的 Linux 主机（Ubuntu 系统），然后再在 vscode 中点击“Reopen folder in Container”，这时 windows 上的 .gitconfig 会被复制到容器中。
可以通过修改 windows 上 vscode 的设置关闭此行为，该设置项为dev.containers.copyGitConfig
如何查看解析 .env 文件后的 docker-compose.yaml？
docker compose config 可以查看解析 .env 文件之后的版本，添加 --environment 可以查看会导入到容器的所有环境变量
ros2 node list 无数据且 ros2 topic list -v 显示未知的节点？
重启 ros2 daemon 或者添加--no-daemon参数，详见 The command “ros2 node list” did not display any nodes · Issue #1616 · ros2/ros2
std::reference_wrapper是什么？有何用途？
它来存储引用，解决原本的引用无法在类（或结构体）成员中定义的问题，它有点类似指针，但使用时必须正确初始化，否则会报编译错误，从而保证其永远不为空，但可能引用无效存储，成为“悬挂引用”
std::move是什么，有何作用？
高效传递资源，避免拷贝，详见 std::move - cppreference.com。
vector::empace_back是什么，有何作用？
使用右值引用放置 item 到 vector 中，减少 copy，提高效率，详见std::vector<T,Allocator>::emplace_back - cppreference.com
colcon可以配置其行为吗？
可以。有三种配置文件：包目录中的colcon.pkg、工作空间或家目录下的.meta、工作空间或家目录下的defaults.yaml，前两者均和包相关，对特定包进行配置，后者则可以进行全局配置，且支持对各个子命令进行配置，详见 Configuration — colcon documentation
colcon test-reulst命令得到的结果中，skipped 的数量不正确，尤其是对于 humble 版本下的 GTEST_SKIP?
是官方 bug，未找到解决方案，详见 colcon test-result ‘skipped’ count is inaccurate · Issue #18 · colcon/colcon-test-result

ROS

本文由作者按照 CC BY 4.0 进行授权