Files

T

xiaoshuai b52cee9e39 fix

2026-08-05 18:17:25 +08:00

28 KiB

Raw Blame History

100 · 嵌入式部署:RDK X5 + RK3506 + ROS2(完全实操指南)

本篇目标:从"硬件到手"到"三机 ROS2 互通、RK3506 控制真实电机"全流程跑通。假设你手上有:1 台 PC + 1 台 RDK X5 + 2 台 RK3506。本指南按 step-by-step 走,每步给出预期输出 + 排错。

第 0 章: 准备与检查清单
第 1 章: 三层架构详解
第 2 章: PC 主控完整配置
第 3 章: RDK X5 边缘 AI 配置
第 4 章: RK3506 精简 ROS2 配置(核心)
第 5 章: 三机 LAN DDS 组网
第 6 章: 跨机 Demo 联调
第 7 章: RK3506 接真实电机 / 编码器
第 8 章: ros2_control 闭环
第 9 章: 故障排查大全
第 10 章: 性能调优 Checklist
附录 A: 采购清单
附录 B: 关键命令速查

0. 准备与检查清单

0.1 你需要的硬件

PC 一台(Ubuntu 22.04 / Windows + Docker)
RDK X5 一台(已烧 Ubuntu 22.04 官方镜像)
RK3506 两台(已烧 Linux 镜像,默认账户 root 或 user)
网线 + 路由器 / 交换机(三个设备同网段)
RK3506 ↔ PC / RDK X5 的 USB-TTL 串口线(debug 用)

0.2 网络规划(提前固定)

PC       192.168.1.10
RDK X5   192.168.1.20
RK3506-1 192.168.1.31
RK3506-2 192.168.1.32

子网掩码 255.255.255.0
ROS_DOMAIN_ID = 0   (三层共用)

💡 强烈建议固定 IP(路由器 DHCP 静态分配或各板 /etc/network/interfaces 写死),避免重启后地址变了连不上。

0.3 先确认每台设备能 SSH / 访问

# PC
ssh user@192.168.1.10   # 应该能登

# RDK X5
ssh user@192.168.1.20

# RK3506
ssh user@192.168.1.31
ssh user@192.168.1.32

每台板子先 ping 互通:

ping -c 3 192.168.1.20   # 在 PC 上 ping RDK X5
ping -c 3 192.168.1.31   # 在 PC 上 ping RK3506 #1

预期: 0% packet loss。 若不通: 检查网线、IP、子网掩码、路由器是否拦 IGMP。

1. 三层架构详解

1.1 为什么分层?

层	设备	跑什么	不跑什么	为什么
PC (主控)	x86 8GB+	MoveIt2 / Nav2 / RViz / 仿真 / 编译	真电机驱动	算力大但不便实时控制硬件
RDK X5 (边缘 AI)	ARM + 5 TOPS NPU	YOLO / SAM / Whisper / SLAM	MoveIt2 大规划	NPU 加速推理,分担 PC 算力
RK3506 (实时控制)	ARM 3 核 512MB	JointState / PID 闭环 / 编码器读取	AI 推理 / RViz	资源紧,但本地控制实时性好

1.2 数据流(典型抓取场景)

                  ┌─────────────────────────────────────────┐
                  │                PC                       │
                  │  RViz 可视化 + 决策 + MoveIt2 规划        │
                  └─────┬───────────────────────┬───────────┘
                        │ /grasp_pose           │ /joint_trajectory
                        │ (DDS)                  │ (DDS)
┌───────────────────────▼─────────┐    ┌─────────▼───────────┐
│           RDK X5                │    │      RK3506 #1       │
│  vision_node:                  │    │  joint_state_publisher│
│  YOLO 检测 → publish /grasp    │    │  编码器 → /joint_states│
│  (可选) VLA 推理(NPU 加速)      │    │  订阅 /cmd_vel 控制电机│
└─────────────────────────────────┘    └─────────────────────┘
                  ┌─────────────────────┐
                  │     RK3506 #2       │
                  │  NPU 推理 / 备用控制│
                  │  (YOLO-Lite / SAM) │
                  └─────────────────────┘

1.3 通信方式(三选一)

方式	优点	缺点	用法
Multicast 默认	零配置,自动发现	路由器/防火墙可能拦	同网段 + 默认
Unicast discovery server	跨网段可用	要起 discovery server	多 VLAN / 跨子网
Unicast `ROS_STATIC_PEERS`	简单、稳	节点列表要预先列	小规模 LAN

推荐先试 multicast,不通再切 unicast。下面 Step 5 详解。

2. PC 主控完整配置

2.1 你的工作环境(选项 A: Linux)

假设你用 Ubuntu 22.04 真机(不是 Docker)。

# 2.1.1 装 ROS2 Humble
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
  -o /usr/share/keyrings/ros-archive-keyring.gpg
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
sudo apt update
sudo apt install ros-humble-desktop python3-colcon-common-extensions -y

# 验证
source /opt/ros/humble/setup.bash
ros2 --help   # 应该输出 ros2 命令帮助

2.2 你的工作环境(选项 B: Windows + Docker)——本仓库主路径

如果你用的是本仓库的 Docker 工作流,环境已就绪:

docker exec -it ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && exec bash"

后续章节都假设你在 Docker 容器内操作。PC 的"ROS_DOMAIN_ID=0"和"DDS 自动发现"对 LAN 上其他设备是开放的——因为 docker-compose 用 network_mode: host,容器用宿主机网络栈。

2.3 PC 上启动本仓库的 bringup 全套

docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 launch bringup full_demo_launch.py"

预期输出(日志截取):

[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-...
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [talker-1]: process started with pid [56]
[INFO] [listener-2]: process started with pid [58]
[INFO] [talker-3]: process started with pid [60]
[INFO] [listener-4]: process started with pid [62]
[talker-3] [INFO] [...] talker_cpp started -> topic=chatter, period=500ms
[listener-4] [INFO] [...] listener_cpp subscribed <- chatter

11 个节点跑起来。如果 LAN 上有 RDK X5 / RK3506 节点,它们会自动被发现。

2.4 PC 端跨机网络验证

docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"

预期(假设 RDK X5 还没装,只有 PC 自己):

/joint_state_publisher_cpp
/talker_py
/talker_cpp
/listener_py
/listener_cpp
/tf2_listener_cpp
/fake_camera_py
/image_processor_py
/fibonacci_action_server_py
/add_two_ints_server_py

如果 RDK X5 装好 ROS2 后,它的节点也会出现在这里。

3. RDK X5 边缘 AI 配置

3.1 准备工作

# SSH 到 RDK X5
ssh user@192.168.1.20

# 检查系统
uname -a
# Linux rdk-x5 5.10.xxx ...

cat /etc/os-release
# Ubuntu 22.04.4 LTS ... (地瓜官方镜像)

# 网络
ip addr show | grep inet
# 应该看到 192.168.1.20/24

3.2 装 ROS2(完整版)

# 同 PC 步骤,装桌面版(包含 RViz 远程)
sudo apt install ros-humble-desktop -y

# 加装本仓库相关包
sudo apt install -y \
  ros-humble-cv-bridge \
  ros-humble-tf2-ros \
  ros-humble-tf2-tools \
  ros-humble-ros-gz \
  ros-humble-nav2-bringup \
  ros-humble-moveit \
  ros-humble-image-transport \
  ros-humble-vision-msgs \
  ros-humble-sensor-msgs \
  ros-humble-geometry-msgs \
  ros-humble-rmw-fastrtps-cpp \
  python3-colcon-common-extensions

预计下载 ~500MB,装 5-10 分钟。

3.3 拷本仓库源码 + colcon build

# 从 PC 拷(在 PC 上)
scp -r D:\xs\ros2 user@192.168.1.20:~/ros2_ws
# 注意:D:\xs\ros2 拷到 RDK X5 后变成 ~/ros2_ws

# 在 RDK X5 上(SSH 进)
cd ~/ros2_ws
source /opt/ros/humble/setup.bash

# ARM 编译,首次 5-10 分钟
colcon build --symlink-install \
  --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup

# 预期输出(末尾)
Summary: 7 packages finished [8m 32s]

⚠️ arm64 兼容性: 全部 7 个包都用纯 Python + 标准 C++,没有架构专属代码,ARM 上 build 应该一次过。如果遇到 Could NOT find X,检查 apt 包名。

3.4 RDK X5 上启动 vision demo

# SSH 到 RDK X5
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

# 设 ROS_DOMAIN_ID(虽然默认是 0,但显式更稳)
export ROS_DOMAIN_ID=0

# 启动 fake_camera (sensor_msgs/Image 发布者)
ros2 run py_vision_demo fake_camera

# 预期
[INFO] [fake_camera_py]: fake_camera_py started: 320x240 @ 10fps -> /image_raw

3.5 PC 上验证 RDK X5 节点可见

# 在 PC / 容器内
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"

# 应该看到:
# /fake_camera_py             (来自 RDK X5)
# /talker_py /listener_py ... (本地)

# 看 RDK X5 上 fake_camera 发的 /image_raw 频率
ros2 topic hz /image_raw --no-daemon
# 预期:average rate: 10.000

3.6 RDK X5 跑 YOLO 推理(扩展)

如果 RDK X5 装了 ultralytics + YOLO 模型:

# 安装
pip install ultralytics --break-system-packages  # 如果是 Buildroot 系统不带 pip,先 apt install python3-pip

# 准备模型
# 把 yolov8n.pt 拷到 ~/models/

# 写一个 yolo_detector 节点(参考 py_vision_demo 写法):
# 订阅 /camera/color/image_raw (或 fake_camera 的 /image_raw)
# 推理 → publish /detections (vision_msgs/Detection2DArray)

这部分需要你写新包,参考 src/py_vision_demo/。

4. RK3506 精简 ROS2 配置(核心)

4.1 为什么 RK3506 要"精简"

配置	占用 RAM
Linux 基础系统	~80 MB
ROS2 rclpy 客户端	~50-80 MB
FastDDS 参与者(单节点)	~30-50 MB
默认 daemon (`ros2 daemon`)	~50 MB ← 必须关
RViz / rqt	~300 MB ← 绝对不装
MoveIt2 / Nav2	200-500 MB ← 不装

不精简 = 直接 OOM。

4.2 准备 RK3506

ssh user@192.168.1.31
uname -a
# Linux rk3506 5.10.xxx ...

free -h
#               total        used        free      shared  buff/cache   available
# Mem:           484Mi        95Mi       240Mi       1.0Mi       148Mi       380Mi

df -h /
# /dev/root  7.4G  1.2G  5.8G  /

# 看是不是 aarch64(必须)
uname -m
# aarch64

4.3 装精简 ROS2

# 4.3.1 装 ROS2 apt 源(同 PC)
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
  -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=arm64 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
sudo apt update

# 4.3.2 装精简 ROS2 包(注意:没有 desktop)
sudo apt install -y \
  ros-humble-ros-base \
  ros-humble-cv-bridge \
  ros-humble-tf2-ros \
  ros-humble-sensor-msgs \
  ros-humble-geometry-msgs \
  ros-humble-rmw-fastrtps-cpp \
  ros-humble-ros2-control \
  python3-colcon-common-extensions

# 检查内存
free -h
# 装完后大约用掉 ~250-280 MB(系统+ROS2+daemon)

4.4 关闭 ROS2 daemon(省 ~50MB)

ROS2 默认启动 ros2 daemon 加速 ros2 ... 命令调用,但每节点起一个 daemon 浪费内存。

# 停 + 禁启 daemon
pkill -9 -f ros2_daemon 2>/dev/null

# 注:`alias ros2='ros2 --no-daemon'` 写到 ~/.bashrc **不会传给子脚本**(脚本里 `ros2` 不展开 alias)。
#      Humble 里 `ros2 topic ... --no-daemon` 这个 flag 已被 deprecated,改用下面的方案:
#      设 CYCLONE_DDS_URI / FASTRTPS_DEFAULT_PROFILES_FILE 走静态发现,从根本上不用 daemon

# 验证: 不再起 daemon
ps aux | grep -v grep | grep -i daemon | head -3
# 应该空

4.5 加 FastDDS 单播发现配置

RK3506 默认走 multicast,但某些网络环境 multicast 不通(如路由器拦了)。改用 unicast 显式指定 PC + RDK X5 更稳。

mkdir -p ~/.ros

cat > ~/.ros/fastdds.xml <<'EOF'
<?xml version="1.0" encoding="UTF-8" ?>
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
  <transport_descriptors>
    <transport_descriptor>
      <transport_id>udp_transport</transport_id>
      <type>UDPv4</type>
    </transport_descriptor>
  </transport_descriptors>

  <participant profile_name="rk3506_participant" is_default_profile="true">
    <rtps>
      <builtin>
        <initialPeersList>
          <initialPeer>
            <address>192.168.1.10</address>
            <port>7400</port>
          </initialPeer>
          <initialPeer>
            <address>192.168.1.20</address>
            <port>7400</port>
          </initialPeer>
        </initialPeersList>

        <discovery_config>
          <discoveryProtocol>SIMPLE</discoveryProtocol>
          <!-- discoveryStrategy 决定 SIMPLE 的语义:STATIC(只连 initialPeersList)/ NONE(全不主动连)/ MULTICAST(默认)
               跟 <initialPeersList> 配合才能实现"单播静态发现" -->
          <discoveryStrategy>STATIC</discoveryStrategy>
          <leaseDuration>30</leaseDuration>
        </discovery_config>

        <metatrafficUnicastLocatorList>
          <locator>
            <udpv4>
              <address>0.0.0.0</address>
              <port>7400</port>
            </udpv4>
          </locator>
        </metatrafficUnicastLocatorList>
      </builtin>
    </rtps>
  </participant>
</profiles>
EOF

# 加进 .bashrc
cat >> ~/.bashrc <<'EOF'
export ROS_DOMAIN_ID=0
export ROS_LOCALHOST_ONLY=0
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
export FASTRTPS_DEFAULT_PROFILES_FILE=~/.ros/fastdds.xml
EOF

source ~/.bashrc

💡 discoveryStrategy=STATIC 让 RK3506 只找列表里的两个 IP,不会到处广播,省网络+省电。

4.6 把本仓库拷到 RK3506 + build

# 在 PC 上(假设在 D:\xs\ros2)
scp -r D:\xs\ros2 user@192.168.1.31:~/ros2_ws

# SSH 到 RK3506
ssh user@192.168.1.31
cd ~/ros2_ws
source /opt/ros/humble/setup.bash

# 编译(ARM,首次 5-10 分钟)
colcon build --symlink-install \
  --packages-select py_pubsub cpp_pubsub cpp_robot_tf2 bringup

# 预期
Summary: 4 packages finished [6m 12s]

⚠️ 如果 RK3506 资源太紧编译失败,只 build 1-2 个必要的包:
colcon build --packages-select cpp_robot_tf2

4.7 RK3506 上启动 joint_state_publisher

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
export ROS_DOMAIN_ID=0

# 跑 cpp_robot_tf2 的关节发布者(executable 名是 joint_state_publisher_cpp,不是 joint_state_publisher)
ros2 run cpp_robot_tf2 joint_state_publisher_cpp

# 预期
[joint_state_publisher_cpp]: joint_state_publisher_cpp started, joints: 3

注: 这是仿真关节(3 关节 sin 运动),真实电机时改成读编码器(见第 7 章)。

4.8 PC 上验证 RK3506 节点可见

# 在 PC / Docker 内
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"
# 应该看到:
# /joint_state_publisher_cpp    ← 来自 RK3506 (192.168.1.31)
# /talker_py /listener_py ...   ← 本地 PC 节点

看不到时: 看下面第 9 章故障排查。

5. 三机 LAN DDS 组网

5.1 验证 multicast 通(默认方式)

# 在 RK3506 上
sudo tcpdump -i eth0 -n udp port 7400 -c 20
# 应该看到来自 PC (192.168.1.10) 和 RDK X5 (192.168.1.20) 的 UDP 包

# Ctrl+C 停止

如果 multicast 不通: 走 4.5 单播配置。

5.2 三机互相"看见"节点

操作	PC	RDK X5	RK3506 #1
`ros2 node list --no-daemon`	见 PC + RDK + RK3506 节点	见 PC + RDK + RK3506 节点	见 PC + RDK + RK3506 节点

预期: 三机每台的 node list 输出完全一致(都看到 3 台的节点)。

5.3 跨机 topic 频率

# 在 RK3506 上看 /chatter 频率(由 PC 的 talker 发布)
ros2 topic hz /chatter --no-daemon
# average rate: 4.000  (4Hz = 2 talker × 2Hz)

延迟: 跨机 topic echo 一般 < 5ms(LAN);> 50ms 说明网络出问题。

6. 跨机 Demo 联调

6.1 场景 A:PC 视觉 → RK3506 控制(简化版)

# === 在 PC (Docker) ===
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 run py_vision_demo fake_camera"

# === 在 RK3506 ===
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 topic echo /image_raw --no-daemon  # 看到 PC 发的图像

# === 在 PC ===
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 run py_vision_demo image_processor"
# 看到 PC 自己处理 + (如果有 RDK X5 / RK3506 也跑 image_processor,都能收到)

6.2 场景 B:RDK X5 视觉 + RK3506 电机(推荐起手)

# === 在 RDK X5 (SSH) ===
ssh user@192.168.1.20
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run py_vision_demo fake_camera  # 或你写的 YOLO 节点

# === 在 RK3506 #1 (SSH) ===
ssh user@192.168.1.31
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run cpp_robot_tf2 joint_state_publisher_cpp
# (后续:接真实电机时改读编码器)

# === 在 PC 上监控全部 ===
docker exec ros2_dev bash -lc "ros2 node list"
# 看到 /fake_camera (RDK X5), /joint_state_publisher (RK3506,launch 重命名去掉了 _cpp 后缀)

docker exec ros2_dev bash -lc "ros2 topic hz /image_raw /joint_states"
# /image_raw: ~10Hz
# /joint_states: ~20Hz

6.3 场景 C:三机跑本仓库 full_demo

# === PC ===
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 launch bringup full_demo_launch.py"
# 11 个 PC 节点

# === RDK X5 ===
ssh user@192.168.1.20
ros2 run py_vision_demo fake_camera  # 1 节点

# === RK3506 #1 ===
ssh user@192.168.1.31
ros2 run cpp_robot_tf2 joint_state_publisher_cpp  # 1 节点

# === RK3506 #2 ===
ssh user@192.168.1.32
ros2 run cpp_robot_tf2 tf2_listener_cpp  # 1 节点

# === 在任一设备看 ===
ros2 node list --no-daemon
# 看到 PC 的 11 + RDK X5 的 1 + RK3506 的 2 = 14 节点!

7. RK3506 接真实电机 / 编码器

7.1 硬件接线(典型 6-DoF 机械臂)

总控舵机 (例如 LewanSoul/Hiwonder LX-16):
  - 1 根总线(Bus Servo): 6 个舵机串联,数据线 Tx/Rx 共用
  - 电源 7-12V(单独供电,不要从 RK3506 取)
  - 接线到 RK3506 串口(UART): TXD/RXD/GND

编码器(可选,闭环舵机内部已有):
  - SPI 接口(Magnetic encoder AS5048A)
  - 或通过总线舵机协议反馈

RK3506:
  - /dev/ttyS0 / /dev/ttyS1 (硬件串口)
  - /dev/ttyUSB0 (USB-TTL 调试线)

7.2 写一个电机驱动节点(C++)

参考本仓库 cpp_robot_tf2/src/joint_state_publisher.cpp(模板)。新建 motor_driver 包:

// ~/ros2_ws/src/motor_driver/src/motor_driver.cpp
#include <chrono>
#include <cstdio>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

class MotorDriver : public rclcpp::Node {
public:
  MotorDriver() : rclcpp::Node("motor_driver") {
    pub_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);

    // 打开串口(假设 /dev/ttyS0 是总线舵机)
    fd_ = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd_ < 0) {
      RCLCPP_FATAL(this->get_logger(), "Cannot open /dev/ttyS0");
      return;
    }
    struct termios opts;
    tcgetattr(fd_, &opts);
    cfsetispeed(&opts, B115200);
    cfsetospeed(&opts, B115200);
    opts.c_cflag |= (CLOCAL | CREAD);
    opts.c_cflag &= ~PARENB;
    opts.c_cflag &= ~CSTOPB;
    opts.c_cflag &= ~CSIZE;
    opts.c_cflag |= CS8;
    tcsetattr(fd_, TCSANOW, &opts);

    timer_ = this->create_wall_timer(50ms, std::bind(&MotorDriver::tick, this));

    // 订阅 cmd
    sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
      "/cmd_joint_states", 10,
      [this](const sensor_msgs::msg::JointState::SharedPtr msg) {
        // 发送舵机命令(总线舵机协议)
        send_servo_cmd(msg);
      });
  }

private:
  void tick() {
    // 1) 读舵机当前角度(总线舵机协议 INSTRUCTION 0x02 read position)
    // 2) 构造 JointState 消息
    auto msg = sensor_msgs::msg::JointState();
    msg.header.stamp = this->now();
    msg.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"};
    msg.position.resize(6);
    // 读 6 个舵机当前角(伪代码)
    for (int i = 0; i < 6; i++) msg.position[i] = read_servo_angle(i);
    pub_->publish(msg);
  }

  void send_servo_cmd(const sensor_msgs::msg::JointState::SharedPtr& msg) {
    // 总线舵机协议: 0x55 0x55 len id cmd param...
    // 这里写 6 个舵机的目标角度
    for (size_t i = 0; i < msg->position.size() && i < 6; i++) {
      uint8_t id = i + 1;  // 舵机 ID 1-6
      uint16_t angle = static_cast<uint16_t>((msg->position[i] + 3.14159) / 6.28318 * 1000);
      uint8_t buf[8] = {0x55, 0x55, 0x08, id, 0x03, 0x1E, (uint8_t)(angle & 0xFF), (uint8_t)(angle >> 8)};
      write(fd_, buf, 8);
    }
  }

  double read_servo_angle(int id) {
    // 实际读位置:发送读指令 → 等待响应 → 解析
    return 0.0;  // 简化
  }

  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_;
  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr sub_;
  rclcpp::TimerBase::SharedPtr timer_;
  int fd_ = -1;
};

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

7.3 上电测试(务必小心!)

1. 舵机先**单独供电**,不上机械臂(用编程器/数据线模式调零)
2. RK3506 接 USB-TTL 调试线,登录 ssh
3. 启动 motor_driver 节点(默认发布 0° 关节角)
4. 手动旋转舵机,看 /joint_states 是否变化
5. 测试 `/cmd_joint_states` 订阅(在 PC 上 ros2 topic pub)
   ros2 topic pub /cmd_joint_states sensor_msgs/msg/JointState "{name: ['j1'], position: [1.57]}"
6. 确认舵机收到命令并转动
7. 装上机械臂,运行 MoveIt2 demo

8. ros2_control 闭环

8.1 ros2_control 是什么

ros2_control 是电机驱动的抽象层。你写一个 HardwareInterface,RK3506 的真驱动接进去, 高层节点(PC 的 MoveIt2)只看到标准接口(position_cmd / position_state),不关心底层是舵机 / 步进电机 / 谐波减速。

  ┌──────────────┐       ┌──────────────────────────┐       ┌─────────────┐
  │ MoveIt2      │ ──→ │ ros2_control              │ ──→ │ 真电机驱动  │
  │ /joint_      │  std │ ControllerManager         │       │ (Bus Servo)│
  │ trajectory   │  if  │ (JointTrajectoryController)│     │             │
  └──────────────┘       └──────────────────────────┘       └─────────────┘

8.2 在 RK3506 上跑 controller_manager

sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers -y

写一个 motor_hw_interface 包(本仓库后续可加),声明 6 个关节的 hardware interface。

8.3 PC 上跑 MoveIt2(规划)

sudo apt install ros-humble-moveit -y

# 启动 MoveIt(配 6-DoF SRDF)
ros2 launch moveit2_tutorials demo.launch.py

PC 算轨迹 → 通过 FollowJointTrajectory Action 发给 RK3506 → RK3506 controller_manager 执行。

9. 故障排查大全

9.1 看不到其他机器的节点

# 步骤 1: 网络通?
ping -c 3 192.168.1.31

# 步骤 2: 端口 7400 通?
nc -zv 192.168.1.31 7400

# 步骤 3: multicast 通?
tcpdump -i eth0 -n udp port 7400 -c 5

# 步骤 4: ROS_DOMAIN_ID 一致?
echo $ROS_DOMAIN_ID   # 三机都要一样

9.2 multicast 不通

切单播 (4.5 节):

# 分隔符是英文逗号 `,`,不是分号 `;`(FastDDS 默认)
export ROS_STATIC_PEERS="192.168.1.10:7400,192.168.1.20:7400"

9.3 RK3506 内存不足

free -h
# 看哪些进程吃内存
ps aux --sort=-%mem | head -10

# 杀掉
pkill -9 -f ros2_daemon
pkill -9 -f ros2  # 注意:会杀掉所有 ROS2 进程,先确认其他节点不跑

9.4 编译失败

# 列报错包名,查 apt 是否装齐
dpkg -l | grep ros-humble-rclcpp

# 装缺失依赖
sudo rosdep init && rosdep update
sudo rosdep install -i --from-paths src/

# 清理重 build
cd ~/ros2_ws && rm -rf build install log
colcon build --symlink-install

9.5 节点启动后立刻死

ros2 run <pkg> <node> --log-level debug
# 看具体 traceback

常见原因:

参数缺失(use_sim_time 未设)
缺少文件(URDF 路径错)
网络端口冲突

10. 性能调优 Checklist

项	目标	命令
RAM 占用	< 350MB	`free -h`
CPU 空闲	> 60%	`top -bn1 \| head -20`
TF lookup 延迟	< 5ms	`ros2 topic delay /tf --no-daemon`
DDS heartbeat	< 100ms	`ros2 daemon stop; ros2 --no-daemon ...`
电机响应延迟	< 50ms	录 topic 时间戳,看从 PC 发出到 RK3506 转动

附录 A: 采购清单

项	数量	备注
RDK X5	1	地瓜机器人官方
RK3506 开发板	2	注意要 aarch64 Linux 镜像
6-DoF 机械臂套件	1	推荐 LewanSoul/Hiwonder 铝架
总线舵机(6 个)	1 套	型号 LX-16 / LX-224
舵机电源(7-12V 5A+)	1	必须独立供电
USB-TTL 串口线	2	RK3506 debug 用
网线 + 路由器	1 套	三机同网段
急停按钮	1	接 RK3506 GPIO

附录 B: 关键命令速查

# 三机通用
source /opt/ros/humble/setup.bash
ros2 node list --no-daemon
ros2 topic list --no-daemon
ros2 topic hz /topic --no-daemon
ros2 topic echo /topic --no-daemon

# RK3506 内存
free -h
ps aux --sort=-%mem | head

# 网络
ip addr show
ping -c 3 <ip>
nc -zv <ip> 7400

# DDS 配置
cat ~/.ros/fastdds.xml
echo $ROS_STATIC_PEERS

# colcon build
cd ~/ros2_ws && colcon build --packages-select <pkg>
cd ~/ros2_ws && rm -rf build install log && colcon build

# ros2_control
ros2 control list_controllers
ros2 control list_hardware_interfaces

# MoveIt2
ros2 launch moveit2_tutorials demo.launch.py

下一步建议

读完本篇你能做到:

✅ 三机 ROS2 互通 ✅ RK3506 接真实电机 ✅ PC + RDK X5 + RK3506 完整工作流

接着学习:

ros2_control 完整配置(doc/99-embodied-ai.md 路径)
MoveIt2 SRDF 生成 + 路径规划
Gazebo 物理仿真验证(在 PC 上)
NPU 推理在 RDK X5 / RK3506 #2 上跑(YOLO-Lite)

如需在本仓库加 ros2_control_demo / motor_driver 包,后续 PR 即可。

📖 阅读路径导航

💡 这是仓库 doc/ 下所有文档的推荐阅读顺序。返回 README 总导航

⏱ 本文预计阅读时间: 90 分钟 📍 当前位置: 第 23 / 24 篇

⏮ 上一篇: VLA / 机器人开发路线图

28 KiB Raw Blame History Unescape Escape