ROS/ROS2机器人开发
文档版本: V1.0 | 适用平台: RK3588 (C-3588LQ V1.0 核心板 + SHIMETAPI-M5-R2A V1.0 底板) Linux 内核: 6.1 | 推荐系统: Ubuntu 22.04 (ROS2 Humble) | 分类: 高级主题
目录
1. 概述
1.1 功能简介
ROS2（Robot Operating System 2）不是操作系统，而是跑在 Linux 上的机器人开发中间件框架，提供标准化的通信机制、工具链和生态系统。RK3588 平台的硬件足以跑完整的 ROS2 栈：
- 8 核 CPU（4×Cortex-A76 + 4×Cortex-A55）：满足导航、SLAM 等计算密集型任务
- NPU（6 TOPS）：加速目标检测、语义分割、SLAM 前端等 AI 推理任务
- Mali-G610 MP4 GPU：支持 RViz2 三维可视化渲染
- 16 通道 PWM：精确电机控制
- 3×CAN FD：工业级传感器和执行器通信
- V4L2/libcamera：立体摄像头接入
- I2C/UART/USB：IMU、LiDAR、编码器等传感器接口
本文档指导在 RK3588 上完成 ROS2 Humble 的安装、传感器驱动集成、NPU 加速推理、导航栈配置以及可视化调试的完整流程。
1.2 应用场景
| 场景 | 说明 | RK3588 优势 |
|---|---|---|
| 自主导航机器人 | 室内外自主避障、路径规划 | 8核并行处理 Nav2 栈 |
| 视觉感知系统 | 目标检测、跟踪、避障 | NPU 6 TOPS 加速 YOLOv8 推理 |
| 巡检机器人 | 定点巡检、异常检测 | NPU + GPU 协同处理 |
| 协作机械臂 | 运动规划、力控 | CAN FD 高速通信 |
| 多传感器融合 | LiDAR + 摄像头 + IMU | 高带宽总线、充足算力 |
| 教育与原型验证 | ROS2 教学、快速原型 | 完整生态系统支持 |
1.3 工作原理
RK3588 上的 ROS2 架构分为以下层次：
┌─────────────────────────────────────────────────┐
│ 应用层 (ROS2 Nodes) │
│ Nav2导航 SLAM 视觉感知 电机控制 传感器融合 │
├─────────────────────────────────────────────────┤
│ ROS2 中间件 (DDS 通信) │
│ Topic Service Action Parameter │
├─────────────────────────────────────────────────┤
│ 系统层 (Ubuntu 22.04 / Linux 6.1) │
├──────────┬──────────┬──────────┬─────────────────┤
│ NPU 驱动 │ GPU 驱动 │ V4L2/ │ PWM/CAN/I2C/ │
│ (RKNN) │ (Mesa) │ libcamera│ UART 驱动 │
├──────────┴──────────┴──────────┴─────────────────┤
│ RK3588 SoC 硬件层 │
│ 8核CPU | NPU 6TOPS | Mali-G610 | 丰富外设接口 │
└─────────────────────────────────────────────────┘数据流示例（视觉导航）：摄像头 → V4L2 驱动 → ROS2 图像话题 → NPU 推理节点 → 目标位姿 → Nav2 导航 → PWM 电机控制
2. 硬件准备
2.4 运行环境（本机）
| 项 | 实际值 |
|---|---|
| ROS/ROS2 | 未安装（/opt/ros 不存在，ros2/colcon 无命令，apt 无 ros 相关包） |
结论：出厂镜像不含任何 ROS 组件，需要按 3.1 节完整安装（Humble deb 源或源码编译）。本机 Ubuntu/Debian 源可用（apt 正常），aarch64 架构的 ROS2 Humble 官方支持 Debian 12。
2.1 硬件清单
| 序号 | 硬件名称 | 规格/型号 | 接口 | 用途 |
|---|---|---|---|---|
| 1 | 核心板 | C-3588LQ V1.0 | — | 主计算单元 |
| 2 | 底板 | SHIMETAPI-M5-R2A V1.0 | — | 外设扩展 |
| 3 | 电源适配器 | 12V/5A DC | DC 5.5mm | 供电 |
| 4 | 立体摄像头 | USB3.0 摄像头 | USB 3.0 | 视觉感知 |
| 5 | LiDAR | UART 或 USB LiDAR | UART/USB | 激光测距 |
| 6 | IMU | MPU6050/BMI270 | I2C | 姿态感知 |
| 7 | 直流电机+编码器 | 12V DC 电机 | PWM+GPIO | 底盘驱动 |
| 8 | CAN 收发器 | TJA1050 | CAN FD | 传感器通信 |
| 9 | NVMe SSD | 256GB+ | M.2 | 系统存储 |
| 10 | microSD 卡 | 64GB+ | SD 卡槽 | 可选启动介质 |
2.2 传感器连接
LiDAR 连接（以 UART 方式为例）：
- LiDAR TX → 底板 UART2_RX (GPIO1_B0)
- LiDAR RX → 底板 UART2_TX (GPIO1_B1)
- LiDAR VCC → 5V
- LiDAR GND → GND
IMU 连接（I2C 方式）：
- SDA → I2C3_SDA (GPIO1_B4)
- SCL → I2C3_SCL (GPIO1_B5)
- VCC → 3.3V
- GND → GND
电机 PWM 连接：
- 电机驱动板 ENA → PWM0 (GPIO0_B5)
- 电机驱动板 IN1 → PWM1 (GPIO0_B6)
- 电机驱动板 IN2 → GPIO 控制引脚
2.3 硬件确认
系统启动后执行以下命令确认硬件接口状态，每条命令后面注释里写了预期输出，逐项对照即可：
# 确认系统信息
uname -a
# 预期输出包含: 6.1.x aarch64 GNU/Linux
# 确认 CPU 核心数
nproc
# 预期输出: 8
# 确认 NPU 设备
ls /dev/rknpu*
# 预期输出: /dev/rknpu0
# 确认 PWM 通道
ls /sys/class/pwm/
# 预期输出: pwmchip0 pwmchip1 ...
# 确认 I2C 总线
i2cdetect -l
# 预期输出: i2c-1, i2c-3 ...
# 确认 CAN 接口
ip link show type can
# 预期输出: can0, can1, can2
# 确认串口设备
ls /dev/ttyS*
# 预期输出: /dev/ttyS0 ~ /dev/ttyS8
# 确认 USB 摄像头
lsusb | grep -i camera
# 预期输出: 显示 USB 摄像头设备
# 确认 GPU
cat /sys/class/devfreq/fde40000.gpu/governor
# 预期输出: simple_ondemand 或 performance3. 软件准备
3.1 ROS2 Humble 安装
RK3588 使用 ARM64 架构，Ubuntu 22.04 已有 ROS2 Humble 的预编译二进制包，直接用 apt 安装：
# 设置 locale
sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
# 添加 ROS2 GPG key
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
# 添加 ROS2 apt 源
echo "deb [arch=$(dpkg --print-architecture) \
signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
http://packages.ros.org/ros2/ubuntu \
$(. /etc/os-release && echo $UBUNTU_CODENAME) main" | \
sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
# 安装 ROS2 Humble 桌面版（含 RViz2 等工具）
sudo apt update
sudo apt install ros-humble-desktop
# 安装开发工具
sudo apt install ros-humble-devel python3-colcon-common-extensions
# 设置环境变量（加入 .bashrc）
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc最后一步别省：把 source 写进 .bashrc，以后开终端 ros2 命令才能直接用。
3.2 依赖安装
# 基础依赖
sudo apt install -y \
python3-pip \
python3-colcon-common-extensions \
python3-rosdep2 \
python3-vcstools \
git \
cmake \
build-essential
# ROS2 常用包
sudo apt install -y \
ros-humble-nav2-bringup \
ros-humble-navigation2 \
ros-humble-nav2-map-server \
ros-humble-nav2-lifecycle-manager \
ros-humble-cartographer-ros \
ros-humble-cartographer-rviz \
ros-humble-teleop-twist-keyboard \
ros-humble-teleop-twist-joy \
ros-humble-robot-state-publisher \
ros-humble-joint-state-publisher \
ros-humble-xacro \
ros-humble-tf2-tools \
ros-humble-diagnostic-updater \
ros-humble-compressed-image-transport
# RKNN 推理相关
sudo apt install -y \
python3-numpy \
python3-opencv
# 初始化 rosdep
sudo rosdep init
rosdep update3.3 内核配置确认
确保以下内核模块在 Linux 6.1 内核中已启用（通常默认已开启）：
# PWM 支持
lsmod | grep pwm_rk
# 如果未加载: sudo modprobe pwm-rk3588
# CAN 支持
lsmod | grep can_dev
# 如果未加载: sudo modprobe can_dev && sudo modprobe kvaser_usb
# V4L2 支持
lsmod | grep videobuf2
# 预期已加载
# I2C 支持
lsmod | grep i2c_rk3x
# 预期已加载
# 查看内核版本详细信息
cat /proc/version
# 预期: Linux version 6.1.x4. 操作步骤
4.1 创建 ROS2 工作空间
# 创建工作空间
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
# 初始化工作空间
colcon build --symlink-install
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc4.2 编写 ROS2 包
# 创建功能包
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python rk3588_robot \
--dependencies rclpy std_msgs sensor_msgs geometry_msgs \
--node robot_driver4.3 传感器驱动节点
以下示例展示 UART LiDAR 驱动节点（Python）：
#!/usr/bin/env python3
"""RK3588 LiDAR 驱动节点 - 通过 UART 获取激光扫描数据"""
import serial
import struct
import math
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Header
import rclpy
class LidarDriverNode(Node):
"""UART LiDAR 驱动节点"""
def __init__(self):
super().__init__('lidar_driver')
self.declare_parameter('port', '/dev/ttyS2')
self.declare_parameter('baudrate', 115200)
self.declare_parameter('frame_id', 'laser_frame')
port = self.get_parameter('port').value
baudrate = self.get_parameter('baudrate').value
self.frame_id = self.get_parameter('frame_id').value
self.serial_conn = serial.Serial(port, baudrate, timeout=0.1)
self.publisher_ = self.create_publisher(LaserScan, '/scan', 10)
self.timer = self.create_timer(0.1, self.scan_callback)
self.get_logger().info(f'LiDAR driver started on {port}')
def scan_callback(self):
"""读取 LiDAR 数据并发布 LaserScan 消息"""
scan = LaserScan()
scan.header = Header()
scan.header.stamp = self.get_clock().now().to_msg()
scan.header.frame_id = self.frame_id
scan.angle_min = 0.0
scan.angle_max = 2.0 * math.pi
scan.angle_increment = (2.0 * math.pi) / 360.0
scan.time_increment = 0.0
scan.scan_time = 0.1
scan.range_min = 0.12
scan.range_max = 12.0
# 实际项目中应解析 UART 原始帧数据
# 此处为数据帧解析示意
raw_data = self.serial_conn.read(720)
ranges = []
for i in range(0, len(raw_data) - 1, 2):
distance = struct.unpack('<H', raw_data[i:i+2])[0] / 1000.0
ranges.append(distance if 0.12 < distance < 12.0 else float('inf'))
scan.ranges = ranges
if ranges:
self.publisher_.publish(scan)
def destroy_node(self):
self.serial_conn.close()
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = LidarDriverNode()
executor = MultiThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()IMU 驱动节点（Python）：
#!/usr/bin/env python3
"""RK3588 IMU 驱动节点 - 通过 I2C 读取姿态数据"""
import smbus2
import struct
import math
from rclpy.node import Node
from sensor_msgs.msg import Imu
from std_msgs.msg import Header
import rclpy
class ImuDriverNode(Node):
"""I2C IMU 驱动节点"""
MPU6050_ADDR = 0x68
PWR_MGMT_1 = 0x6B
def __init__(self):
super().__init__('imu_driver')
self.declare_parameter('i2c_bus', 3)
self.declare_parameter('frame_id', 'imu_link')
bus_num = self.get_parameter('i2c_bus').value
self.frame_id = self.get_parameter('frame_id').value
self.bus = smbus2.SMBus(bus_num)
# 唤醒 MPU6050
self.bus.write_byte_data(self.MPU6050_ADDR, self.PWR_MGMT_1, 0x00)
self.publisher_ = self.create_publisher(Imu, '/imu/data', 10)
self.timer = self.create_timer(0.02, self.imu_callback) # 50Hz
self.get_logger().info('IMU driver started on I2C bus %d' % bus_num)
def imu_callback(self):
"""读取 IMU 数据并发布"""
msg = Imu()
msg.header = Header()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = self.frame_id
# 读取加速度计数据
accel_data = self.bus.read_i2c_block_data(self.MPU6050_ADDR, 0x3B, 6)
ax, ay, az = struct.unpack('>hhh', bytes(accel_data))
ax, ay, az = ax / 16384.0, ay / 16384.0, az / 16384.0
# 读取陀螺仪数据
gyro_data = self.bus.read_i2c_block_data(self.MPU6050_ADDR, 0x43, 6)
gx, gy, gz = struct.unpack('>hhh', bytes(gyro_data))
gx, gy, gz = gx / 131.0, gy / 131.0, gz / 131.0
msg.linear_acceleration.x = ax * 9.81
msg.linear_acceleration.y = ay * 9.81
msg.linear_acceleration.z = az * 9.81
msg.angular_velocity.x = math.radians(gx)
msg.angular_velocity.y = math.radians(gy)
msg.angular_velocity.z = math.radians(gz)
self.publisher_.publish(msg)
def destroy_node(self):
self.bus.close()
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = ImuDriverNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()4.4 NPU 推理集成
利用 RK3588 的 6 TOPS NPU 加速目标检测推理，通过 RKNN Runtime 封装为 ROS2 节点：
// rk3588_npu_detector.cpp
// RK3588 NPU 加速目标检测节点 (C++)
// 使用 RKNN Runtime API 进行 YOLOv8 推理
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <vision_msgs/msg/detection2_d_array.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include "rknn_api.h"
#include <vector>
#include <memory>
struct Detection {
int class_id;
float confidence;
float x, y, w, h;
};
class NpuDetectorNode : public rclcpp::Node {
public:
NpuDetectorNode() : Node("npu_detector") {
this->declare_parameter("model_path", "");
this->declare_parameter("confidence_threshold", 0.5f);
this->declare_parameter("input_width", 640);
this->declare_parameter("input_height", 640);
std::string model_path = this->get_parameter("model_path").as_string();
confidence_thresh_ = this->get_parameter("confidence_threshold").as_double();
// 加载 RKNN 模型
FILE* fp = fopen(model_path.c_str(), "rb");
fseek(fp, 0, SEEK_END);
model_size_ = ftell(fp);
fseek(fp, 0, SEEK_SET);
model_data_ = new unsigned char[model_size_];
fread(model_data_, 1, model_size_, fp);
fclose(fp);
// 初始化 RKNN 模型
int ret = rknn_init(&ctx_, model_data_, model_size_, 0, nullptr);
if (ret < 0) {
RCLCPP_ERROR(this->get_logger(), "RKNN init failed: %d", ret);
}
// 设置 NPU 为高性能模式
rknn_core_mask core_mask = RKNN_NPU_CORE_0_1_2;
rknn_set_core_mask(ctx_, core_mask);
image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
"/camera/image_raw", 10,
std::bind(&NpuDetectorNode::image_callback, this, std::placeholders::_1));
detection_pub_ = this->create_publisher<vision_msgs::msg::Detection2DArray>(
"/detections", 10);
RCLCPP_INFO(this->get_logger(),
"NPU detector initialized (model: %s, cores: 3)", model_path.c_str());
}
~NpuDetectorNode() {
if (ctx_ >= 0) rknn_destroy(ctx_);
delete[] model_data_;
}
private:
void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
auto cv_img = cv_bridge::toCvCopy(msg, "rgb8");
cv::Mat resized;
cv::resize(cv_img->image, resized, cv::Size(640, 640));
// 准备输入张量
rknn_input inputs[1];
memset(inputs, 0, sizeof(inputs));
inputs[0].index = 0;
inputs[0].type = RKNN_TENSOR_UINT8;
inputs[0].size = 640 * 640 * 3;
inputs[0].fmt = RKNN_TENSOR_NHWC;
inputs[0].buf = resized.data;
rknn_inputs_set(ctx_, 1, inputs);
// 推理
rknn_run(ctx_, nullptr);
// 获取输出
rknn_output outputs[3];
memset(outputs, 0, sizeof(outputs));
outputs[0].want_float = 1;
outputs[1].want_float = 1;
outputs[2].want_float = 1;
rknn_outputs_get(ctx_, 3, outputs, nullptr);
// 后处理（简化版）
vision_msgs::msg::Detection2DArray det_array;
det_array.header = msg->header;
float* output_data = (float*)outputs[0].buf;
int num_detections = outputs[0].size / sizeof(float) / 84;
for (int i = 0; i < num_detections; i++) {
float conf = output_data[i * 84 + 4];
if (conf > confidence_thresh_) {
vision_msgs::msg::Detection2D det;
det.results.resize(1);
det.results[0].hypothesis.class_id = std::to_string((int)output_data[i * 84 + 5]);
det.results[0].hypothesis.score = conf;
det.bbox.center.position.x = output_data[i * 84 + 0] * msg->width / 640.0;
det.bbox.center.position.y = output_data[i * 84 + 1] * msg->height / 640.0;
det.bbox.size_x = output_data[i * 84 + 2] * msg->width / 640.0;
det.bbox.size_y = output_data[i * 84 + 3] * msg->height / 640.0;
det_array.detections.push_back(det);
}
}
rknn_outputs_release(ctx_, 3, outputs);
detection_pub_->publish(det_array);
}
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr detection_pub_;
rknn_context ctx_ = -1;
unsigned char* model_data_ = nullptr;
uint32_t model_size_ = 0;
float confidence_thresh_ = 0.5f;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<NpuDetectorNode>());
rclcpp::shutdown();
return 0;
}4.5 PWM 电机控制
#!/usr/bin/env python3
"""RK3588 PWM 电机控制节点"""
import os
from rclpy.node import Node
from geometry_msgs.msg import Twist
import rclpy
class MotorControlNode(Node):
"""通过 PWM 控制电机的 ROS2 节点"""
def __init__(self):
super().__init__('motor_control')
self.declare_parameter('pwm_period', 1000000) # ns
self.declare_parameter('wheel_base', 0.25) # m
self.declare_parameter('wheel_radius', 0.05) # m
self.period = self.get_parameter('pwm_period').value
self.wheel_base = self.get_parameter('wheel_base').value
self.wheel_radius = self.get_parameter('wheel_radius').value
# 初始化 PWM 通道
self.pwm_paths = ['/sys/class/pwm/pwmchip0', '/sys/class/pwm/pwmchip1']
for path in self.pwm_paths:
self.export_pwm(path)
self.subscription = self.create_subscription(
Twist, '/cmd_vel', self.cmd_vel_callback, 10)
self.get_logger().info('Motor control node started')
def export_pwm(self, pwm_path):
"""导出 PWM 通道"""
pwm_num = pwm_path.split('/')[-1]
num = pwm_num.replace('pwmchip', '')
export_path = f'/sys/class/pwm/pwmchip{num}/pwm{num}'
if not os.path.exists(export_path):
with open(f'/sys/class/pwm/pwmchip{num}/export', 'w') as f:
f.write(num)
with open(f'{export_path}/period', 'w') as f:
f.write(str(self.period))
with open(f'{export_path}/duty_cycle', 'w') as f:
f.write('0')
with open(f'{export_path}/enable', 'w') as f:
f.write('1')
def set_pwm(self, chip_idx, duty_ns):
"""设置 PWM 占空比"""
num = chip_idx
path = f'/sys/class/pwm/pwmchip{num}/pwm{num}/duty_cycle'
with open(path, 'w') as f:
f.write(str(max(0, min(int(duty_ns), self.period))))
def cmd_vel_callback(self, msg: Twist):
"""处理速度指令，差速运动学解算"""
v = msg.linear.x
w = msg.angular.z
# 差速模型解算
v_left = v - w * self.wheel_base / 2.0
v_right = v + w * self.wheel_base / 2.0
# 线速度 → PWM 占空比（简化映射）
max_speed = 1.0 # m/s
left_duty = int((v_left / max_speed + 1.0) / 2.0 * self.period)
right_duty = int((v_right / max_speed + 1.0) / 2.0 * self.period)
self.set_pwm(0, left_duty)
self.set_pwm(1, right_duty)
self.get_logger().debug(f'L: {left_duty} R: {right_duty}')
def main(args=None):
rclpy.init(args=args)
node = MotorControlNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()4.6 Nav2 导航配置
Nav2 是 ROS2 官方维护的导航栈，负责定位、路径规划和运动控制。先创建 Nav2 导航 Launch 文件：
# launch/nav2_launch.py
"""RK3588 Nav2 导航启动文件"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
nav2_dir = get_package_share_directory('nav2_bringup')
map_file = LaunchConfiguration('map')
params_file = LaunchConfiguration('params')
return LaunchDescription([
DeclareLaunchArgument('map', default_value='maps/robot_map.yaml'),
DeclareLaunchArgument('params',
default_value='config/nav2_params.yaml'),
# 地图服务器
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_dir, 'launch', 'map_launch.py')),
launch_arguments={'map': map_file}.items()
),
# Nav2 参数文件
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_dir, 'launch', 'navigation_launch.py')),
launch_arguments={
'params_file': params_file,
'use_sim_time': 'false'
}.items()
),
])Nav2 参数配置（针对 RK3588 优化）：
# config/nav2_params.yaml
amcl:
ros__parameters:
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: 'base_link'
global_frame_id: 'map'
model_type: 'diff'
controller_server:
ros__parameters:
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
progress_checker_plugins: ['progress_checker']
goal_checker_plugins: ['general_goal_checker']
controller_plugins: ['FollowPath']
progress_checker:
plugin: 'nav2_controller::SimpleProgressChecker'
required_movement_radius: 0.5
movement_time_allowance: 10.0
general_goal_checker:
plugin: 'nav2_controller::SimpleGoalChecker'
stateful: true
x_tolerance: 0.2
y_tolerance: 0.2
yaw_tolerance: 0.25
FollowPath:
plugin: 'dwb_core::DWBLocalPlanner'
max_vel_x: 0.3
min_vel_x: 0.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.3
acc_lim_x: 2.5
acc_lim_theta: 3.2
vx_samples: 10
vy_samples: 0
vtheta_samples: 20
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: 'odom'
robot_base_frame: 'base_link'
use_sim_time: false
robot_radius: 0.2
resolution: 0.05
plugins: ['obstacle_layer', 'voxel_layer', 'inflation_layer']
obstacle_layer:
plugin: 'nav2_costmap_2d::ObstacleLayer'
enabled: true
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: true
marking: true
global_costmap:
ros__parameters:
update_frequency: 2.0
publish_frequency: 1.0
global_frame: 'map'
robot_base_frame: 'base_link'
use_sim_time: false
robot_radius: 0.2
resolution: 0.05
track_unknown_space: true
plugins: ['static_layer', 'obstacle_layer', 'inflation_layer']
bt_navigator:
ros__parameters:
use_sim_time: false
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 100
default_server_timeout: 20
wait_for_service_timeout: 1000
action_server_result_timeout: 900.0
bt_xml_filename: 'navigate_w_replanning_and_recovery.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_reinitialize_global_localization_action_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_recovery_node_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_throttle_action_bt_node4.7 Launch 集成与启动
# launch/robot_launch.py
"""RK3588 机器人完整启动文件"""
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
return LaunchDescription([
# LiDAR 驱动
Node(
package='rk3588_robot',
executable='lidar_driver',
name='lidar_driver',
parameters=[{
'port': '/dev/ttyS2',
'baudrate': 115200,
'frame_id': 'laser_frame'
}],
output='screen'
),
# IMU 驱动
Node(
package='rk3588_robot',
executable='imu_driver',
name='imu_driver',
parameters=[{
'i2c_bus': 3,
'frame_id': 'imu_link'
}],
output='screen'
),
# NPU 目标检测
Node(
package='rk3588_robot',
executable='npu_detector',
name='npu_detector',
parameters=[{
'model_path': '/opt/models/yolov8n.rknn',
'confidence_threshold': 0.5
}],
output='screen'
),
# 电机控制
Node(
package='rk3588_robot',
executable='motor_control',
name='motor_control',
parameters=[{
'pwm_period': 1000000,
'wheel_base': 0.25,
'wheel_radius': 0.05
}],
output='screen'
),
# TF 变换
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[{
'robot_description': open(
os.path.join(
get_package_share_directory('rk3588_robot'),
'urdf', 'robot.urdf')).read()
}],
output='screen'
),
])启动机器人系统：
# 启动所有驱动和导航
cd ~/ros2_ws
colcon build --packages-select rk3588_robot
source install/setup.bash
# 完整启动
ros2 launch rk3588_robot robot_launch.py
# 单独启动导航
ros2 launch nav2_bringup navigation_launch.py \
params_file:=config/nav2_params.yaml启动后在另一个终端执行 ros2 topic list，能看到 /scan、/imu/data、/detections 等话题，说明各节点都在正常发布数据。
5. 代码详解
5.1 ROS2 节点架构
RK3588 机器人系统的 ROS2 节点按功能分层组织：
rk3588_robot_system/
├── sensors/ # 传感器层
│ ├── lidar_driver # LiDAR UART 驱动 → /scan
│ ├── imu_driver # IMU I2C 驱动 → /imu/data
│ └── camera_driver # 摄像头 V4L2 驱动 → /camera/image_raw
├── perception/ # 感知层
│ ├── npu_detector # NPU 目标检测 → /detections
│ ├── pointcloud_process # 点云处理 → /processed_cloud
│ └── sensor_fusion # 多传感器融合 → /fused_pose
├── navigation/ # 导航层
│ ├── slam_toolbox # SLAM 建图
│ ├── nav2_bringup # Nav2 导航栈
│ └── behavior_tree # 行为树决策
├── control/ # 控制层
│ ├── motor_control # PWM 电机控制
│ └── can_interface # CAN 设备通信
└── ui/ # 可视化层
└── rviz2_launch # RViz2 可视化5.2 Topic 通信
Topic（话题）是 ROS2 里最常用的发布/订阅通信方式，调试时用下面这些命令观察数据流：
# 查看当前所有 topic
ros2 topic list
# 查看 topic 消息频率
ros2 topic hz /scan
ros2 topic hz /camera/image_raw
ros2 topic hz /cmd_vel
# 查看 topic 消息内容
ros2 topic echo /imu/data --once
ros2 topic echo /detections --once
# 手动发布速度指令测试
ros2 topic pub /cmd_vel geometry_msgs/Twist \
"{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.1}}" \
--rate 105.3 Service 服务
# 查看所有 service
ros2 service list
# 手动建图服务
ros2 service call /slam_toolbox/resume std_srvs/srv/Empty
# 地图保存服务
ros2 run nav2_map_server map_saver_cli \
-f ~/maps/robot_map --ros-args -p save_map_timeout:=10.0
# 生命周期管理
ros2 run nav2_lifecycle_manager lifecycle_manager5.4 Action 动作
# 发送导航目标
ros2 action send_goal /navigate_to_pose \
nav2_msgs/action/NavigateToPose \
"{pose: {header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 1.0, z: 0.0}, orientation: {w: 1.0}}}, behavior_tree: ''}"
# 发送Follow Waypoints动作
ros2 action send_goal /follow_waypoints \
nav2_msgs/action/FollowWaypoints \
"{poses: [{header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 0.0}}}, {header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 1.0}}}]}"5.5 TF2 坐标变换
#!/usr/bin/env python3
"""RK3588 TF2 坐标变换发布示例"""
import math
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import rclpy
class RobotTfPublisher(Node):
"""发布机器人各坐标系之间的 TF 变换"""
def __init__(self):
super().__init__('robot_tf_publisher')
self.tf_broadcaster = TransformBroadcaster(self)
self.timer = self.create_timer(0.02, self.publish_tf)
self.get_logger().info('TF publisher started')
def publish_tf(self):
now = self.get_clock().now().to_msg()
# map → odom
self.send_transform(now, 'map', 'odom',
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)
# odom → base_link
self.send_transform(now, 'odom', 'base_link',
0.0, 0.0, 0.15, 0.0, 0.0, 0.0, 1.0)
# base_link → laser_frame (LiDAR 在机器人上方)
self.send_transform(now, 'base_link', 'laser_frame',
0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 1.0)
# base_link → imu_link
self.send_transform(now, 'base_link', 'imu_link',
0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 1.0)
def send_transform(self, stamp, parent, child,
tx, ty, tz, qx, qy, qz, qw):
t = TransformStamped()
t.header.stamp = stamp
t.header.frame_id = parent
t.child_frame_id = child
t.transform.translation.x = tx
t.transform.translation.y = ty
t.transform.translation.z = tz
t.transform.rotation.x = qx
t.transform.rotation.y = qy
t.transform.rotation.z = qz
t.transform.rotation.w = qw
self.tf_broadcaster.sendTransform(t)
def main(args=None):
rclpy.init(args=args)
node = RobotTfPublisher()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()6. 常见问题
问题 1：ROS2 安装后找不到包
现象：执行 ros2 run 或 ros2 launch 时报错 "Package not found"。
原因：未正确 source ROS2 环境或工作空间。
解决：
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
# 建议写入 ~/.bashrc
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc问题 2：RViz2 显示黑屏或无响应
现象：RViz2 启动后窗口空白、渲染异常或卡死。
原因：Mali-G610 GPU 驱动未加载或 Mesa 版本不兼容。
解决：
# 检查 GPU 驱动
ls /usr/lib/aarch64-linux-gnu/dri/
# 确认有 mali 或 libMali.so
# 设置 GPU 性能模式
echo performance > /sys/class/devfreq/fde40000.gpu/governor
# 如果使用软件渲染（降级方案）
export LIBGL_ALWAYS_SOFTWARE=1
rviz2问题 3：NPU 推理速度低于预期
现象：RKNN 模型推理延迟远超预期，帧率低于 5 FPS。
原因：未使用多 NPU 核心或模型未量化。
解决：
# 确认 NPU 驱动版本
cat /sys/kernel/debug/rknpu/version
# 使用多核推理（RK3588 支持 3 核 NPU）
# 在代码中设置 rknn_core_mask = RKNN_NPU_CORE_0_1_2
# 确认模型为 INT8 量化版本
# 使用 rknn-toolkit2 将 FP32 模型量化为 INT8
python3 -c "from rknn.api import RKNN; print('RKNN toolkit OK')"问题 4：LiDAR 串口读取数据为空
现象：/scan topic 没有消息或消息全为 inf。
原因：串口权限不足、波特率不匹配或设备节点错误。
解决：
# 检查串口权限
ls -l /dev/ttyS2
# 如果权限不足
sudo chmod 666 /dev/ttyS2
# 或添加用户到 dialout 组
sudo usermod -aG dialout $USER
# 确认波特率（与 LiDAR 规格一致）
stty -F /dev/ttyS2 115200 raw
# 手动测试串口
cat /dev/ttyS2 | xxd | head问题 5：CAN 接口无法发送数据
现象：CAN 收发器无响应，candump 无输出。
原因：CAN 控制器未启用或波特率配置错误。
解决：
# 启用 CAN 接口并设置波特率
sudo ip link set can0 up type can bitrate 500000
# 如果 CAN FD 支持
sudo ip link set can0 up type can bitrate 500000 dbitrate 2000000 fd on
# 确认接口状态
ip -details link show can0
# 手动发送测试帧
cansend can0 123#DEADBEEF
# 接收测试
candump can0问题 6：PWM 输出无信号
现象：电机不转动，示波器无 PWM 波形。
原因：PWM 通道未导出或周期/占空比设置不正确。
解决：
# 手动导出 PWM 通道
echo 0 > /sys/class/pwm/pwmchip0/export
# 设置周期（1kHz = 1000000ns）
echo 1000000 > /sys/class/pwm/pwmchip0/pwm0/period
# 设置占空比（50% = 500000ns）
echo 500000 > /sys/class/pwm/pwmchip0/pwm0/duty_cycle
# 使能输出
echo 1 > /sys/class/pwm/pwmchip0/pwm0/enable
# 检查输出
cat /sys/class/pwm/pwmchip0/pwm0/duty_cycle
cat /sys/class/pwm/pwmchip0/pwm0/enable7. 相关文档
| 文档 | 说明 |
|---|---|
| ROS2 Humble 官方文档 | https://docs.ros.org/en/humble/ |
| Nav2 官方文档 | https://docs.nav2.org/ |
| RKNN Runtime API 参考 | RK 官方 NPU 开发指南 |
| Linux 6.1 内核 PWM 驱动 | 内核源码 Documentation/driver-api/pwm |
| libcamera 官方文档 | https://libcamera.org/docs.html |