RK3588 上 ROS2 Humble + 串口机械臂驱动（ROS2安装流程 + V1版本Serial驱动）

目标：把我这版"能跑通闭环"的 ROS2 串口驱动写成一篇可复现、可维护、方便后续大改结构的工程记录。

环境：RK3588（Ubuntu 22.04 / ROS2 Humble）

代码：一个包里两个可执行文件：串口驱动 + 键盘 Teleop

注：这个版本写的不好，数据帧没有对齐，下个版本我在构思更新的代码框架来对齐数据帧。现在虽然能跑，但是磕磕绊绊，而且代码很臃肿，逻辑杂乱。

0.ROS2安装

为了解决网络问题，先在/etc/hosts文件末尾绑定IP与网址：

bash 复制代码

185.199.108.133 raw.githubusercontent.com
185.199.109.133 raw.githubusercontent.com
185.199.110.133 raw.githubusercontent.com
185.199.111.133 raw.githubusercontent.com

这里的IP是从域名查询网站上搜索githubusercontent.com查询得到的，后续可能会变化，如果发现通信失败了就重新查询重新改（一般似乎并不会变，这里的IP和几年前我设置的一样）

然后开始安装：

0) 基础检查（建议先做）

复制代码

lsb_release -a
uname -m

期望看到：

Ubuntu 22.04
架构 aarch64

1) 设置 locale

复制代码

sudo apt update
sudo apt install -y 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

2) 添加 ROS 2 软件源与 key（Jammy）

复制代码

sudo apt install -y software-properties-common curl gnupg lsb-release
sudo add-apt-repository universe

添加 key：

复制代码

sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
  -o /usr/share/keyrings/ros-archive-keyring.gpg

添加源（注意这里的 $(dpkg --print-architecture) 会在 aarch64 上输出 arm64）：

复制代码

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

3) 安装 ROS 2 Humble

两种安装档位：

全家桶

复制代码

sudo apt install -y ros-humble-desktop

轻量

复制代码

sudo apt install -y ros-humble-ros-base

desktop含 RViz2 等。

4) 配置环境变量（每次开终端自动生效）

复制代码

echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

验证：

bash 复制代码

ros2 topic list

5) 安装常用构建与开发工具

复制代码

sudo apt install -y \
  python3-colcon-common-extensions \
  python3-rosdep \
  python3-vcstool \
  python3-argcomplete \
  build-essential \
  cmake \
  git

初始化 rosdep（只需要做一次）：

复制代码

sudo rosdep init
rosdep update

6) 快速自检：运行 demo

开两个终端：

终端 1：

bash 复制代码

source /opt/ros/humble/setup.bash
ros2 run demo_nodes_cpp talker

终端 2：

复制代码

source /opt/ros/humble/setup.bash
ros2 run demo_nodes_cpp listener

如果能互相通信查看一下ros2 node list，如果存在这两个刚开的节点，ROS2 基本环境 OK。

1. 系统目标与数据流

我需要一条稳定的链路，把 ROS2 的 topic/service 映射到机械臂的串口协议，同时保证：

串口不会因为粘包/多帧导致解析错乱
运动控制不会因为"重复发旧目标"刷爆控制器
键盘/手柄这类"非实时输入"不会导致目标跳变（回弹）
查询（poll）不会干扰运动导致卡顿

最终的数据流是：

驱动节点 robot_serial_driver
- ROS → 串口：
  - /robot/ee_target（PoseStamped）→ @X,Y,Z,A,B,C\r\n
  - /robot/joint_target（JointState，目前我用它承载 xyzabc）→ &x,y,z,a,b,c\r\n
  - /robot/start|stop|home|reset|disable|...（Trigger）→ !COMMAND\r\n
- 串口 → ROS：
  - #GETLPOS 回 ok ... → /robot/ee_pose
  - #GETJPOS 回 ok ... → /robot/joint_states
- 调试口：
  - 串口每行原样 → /robot/driver_rx
  - 每次写串口原样 → /robot/driver_tx
键盘节点 ee_keyboard_teleop_xyz
- /robot/ee_pose（反馈）→ 生成连续目标 → 发布 /robot/ee_target

2. 工程结构与运行方式

工作区：

bash 复制代码

ws_ROBOT/
  src/robot_serial_driver/
    src/robot_serial_driver_node.cpp
    src/ee_keyboard_teleop_xyz.cpp
    CMakeLists.txt
    package.xml
  build/ install/ log/

编译：

bash 复制代码

colcon build --packages-select robot_serial_driver
source install/setup.bash

查串口设备：

bash 复制代码

ls -l /dev/ttyUSB* /dev/ttyACM* 2>/dev/null

启动驱动（改成自己的端口）：

bash 复制代码

ros2 run robot_serial_driver robot_serial_driver_node --ros-args \
  -p port:=/dev/ttyACM1 -p baud:=115200

启动键盘 teleop：

bash 复制代码

ros2 run robot_serial_driver ee_keyboard_teleop_xyz_node

发系统命令（示例 disable）：

bash 复制代码

ros2 service call /robot/disable std_srvs/srv/Trigger "{}"

3. 源码解构

3.1 驱动节点：`RobotSerialDriver`（robot_serial_driver_node.cpp）

3.1.1 启动后做的事：构造函数里把系统"架起来"

(1) 读参数（节拍器 + 安全阀 + 流控）

cpp 复制代码

cmd_timeout_s_ = declare_parameter<double>("cmd_timeout_s", 0.5);

port_ = declare_parameter<std::string>("port", "/dev/ttyACM0");
baud_ = declare_parameter<int>("baud", 115200);

poll_hz_ = declare_parameter<double>("poll_hz", 10.0);
pause_poll_while_sending_ = declare_parameter<bool>("pause_poll_while_sending", true);

send_check_hz_ = declare_parameter<double>("send_check_hz", 200.0);
min_send_interval_ms_ = declare_parameter<int>("min_send_interval_ms", 25);

min_free_size_to_send_ = declare_parameter<int>("min_free_size_to_send", 1);
suppress_poll_while_moving_ms_ = declare_parameter<int>("suppress_poll_while_moving_ms", 300);

frame_id_ = declare_parameter<std::string>("frame_id", "base_link");

这里最关键的概念是：

send_check_hz_ 是"检查循环频率"，不是运动发送频率
真正运动发送频率由 min_send_interval_ms_ 控制（默认 25ms ≈ 40Hz）

(2) 建 ROS 接口（功能口 + 调试口）

发布：

cpp 复制代码

rx_pub_ = create_publisher<std_msgs::msg::String>("/robot/driver_rx", 10);
tx_pub_ = create_publisher<std_msgs::msg::String>("/robot/driver_tx", 10);

auto pose_qos = rclcpp::QoS(rclcpp::KeepLast(5)).reliable();
ee_pose_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("/robot/ee_pose", pose_qos);

joint_pub_ = create_publisher<sensor_msgs::msg::JointState>("/robot/joint_states", 10);

cpp 复制代码

ee_target_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
  "/robot/ee_target", rclcpp::QoS(10),
  std::bind(&RobotSerialDriver::onEeTarget, this, _1));

joint_target_sub_ = create_subscription<sensor_msgs::msg::JointState>(
  "/robot/joint_target", rclcpp::QoS(10),
  std::bind(&RobotSerialDriver::onJointTarget, this, _1));

服务（Trigger → !COMMAND\r\n）统一走 makeTriggerService()，例如：

cpp 复制代码

srv_disable_ = makeTriggerService("/robot/disable", "!DISABLE");

(3) 定时器：一个轮询 timer + 一个发送检查 timer

cpp 复制代码

poll_timer_ = create_wall_timer(1.0/poll_hz_,  bind(&RobotSerialDriver::pollTick, this));
send_timer_ = create_wall_timer(1.0/send_check_hz_, bind(&RobotSerialDriver::sendLatestEeTargetIfDue, this));

pollTick()：查询状态（GETLPOS/GETJPOS）
sendLatestEeTargetIfDue()：若有新目标且满足闸门，就发 @...

(4) 打开串口 + RX 线程

cpp 复制代码

openSerialOrThrow();
running_.store(true);
rx_thread_ = std::thread([this]{ rxLoop(); });

这意味着：ROS 主线程负责 timer/sub/srv；串口读取在独立线程里做。

3.1.2 它"什么时候发什么"：poll 状态机（稳但简单）

放弃用两个并发定时器分别 poll JPOS/LPOS，而是用一个状态机串行化请求：

cpp 复制代码

enum class PollState { WANT_LPOS, WAIT_LPOS, WANT_JPOS, WAIT_JPOS };

void pollTick() {
  if (pause_poll_while_sending_ && sending_motion_) return;
  if (shouldSuppressPoll()) return;

  if (poll_state_ == WANT_LPOS) {
    last_query_ = LPOS;
    if (sendRaw("#GETLPOS\r\n")) poll_state_ = WAIT_LPOS;
    return;
  }

  if (poll_state_ == WANT_JPOS) {
    last_query_ = JPOS;
    if (sendRaw("#GETJPOS\r\n")) poll_state_ = WAIT_JPOS;
    return;
  }
}

收到 ok ... 后推进状态机（本质是"收到响应才发下一个请求"）：

cpp 复制代码

void onOkForLastQuery() {
  if (poll_state_ == WAIT_LPOS) poll_state_ = WANT_JPOS;
  else if (poll_state_ == WAIT_JPOS) poll_state_ = WANT_LPOS;
}

好处（我当时选择它的原因）：

串口回包顺序就算抖动也没关系，因为同一时刻只有一个查询在 flight
last_query_ 足够可靠，不会出现"ok 6数到底是谁的返回"的错配

3.1.3 它"怎么收串口"：rxLoop 切帧策略（解决粘连的核心）

最终用 CRLF 切帧，并且在 streambuf 里循环吐尽所有完整帧：

cpp 复制代码

asio::read_until(serial_, buf, "\r\n", ec);

while (true) {
  auto it = std::search(begin, end, "\r\n", "\r\n"+2);
  if (it == end) break;

  std::string line(begin, it);
  buf.consume(distance(begin,it)+2);

  line = trim_crlf(line);
  if (line.empty()) continue;

  rx_pub_->publish(line);   // /robot/driver_rx
  handleLine(line);
}

这段就是"为什么我不再看到粘包"的根因修复点。

3.1.4 它"怎么解析"：handleLine 支持哪些固件返回

freeSize 的 15ok 形式（整数前缀 + ok）
标准 ok v1..v6（GETLPOS/GETJPOS）
单独 "ok"
纯数字 "15"

关键片段：

cpp 复制代码

// "15ok"
if (auto sp = split_int_prefix(line)) {
  if (is_integer_string(sp->first) && sp->second == "ok") {
    last_free_size_.store(std::stoi(sp->first));
    return;
  }
}

// "ok v1..v6"
if (starts_with(line, "ok ")) {
  parse nums...
  if (last_query_ == JPOS) publishJpos(nums);
  else if (last_query_ == LPOS) publishLpos(nums);

  onOkForLastQuery();
  return;
}

3.1.5 它"发布什么"：LPOS / JPOS 转 ROS 的单位约定

LPOS（mm + deg）：
复制代码
```
ok 227.50 0.00 324.50 -0.00 90.00 0.00
```
JPOS（deg）：
复制代码
```
ok 0.00 0.00 90.00 0.00 0.00 0.00
```

LPOS → /robot/ee_pose（已经统一为 ROS 常用：m + quaternion）：

XYZ：mm → m
A/B/C：deg → rad
姿态欧拉角顺序：ZYX，并且约定
- A = yaw(Z)
- B = pitch(Y)
- C = roll(X)
用 quatFromZYX(A,B,C) 生成 quaternion

代码对应：

cpp 复制代码

ps.pose.position.x = p[0] / 1000.0;
...

const double A = p[3] * M_PI / 180.0;
const double B = p[4] * M_PI / 180.0;
const double C = p[5] * M_PI / 180.0;

ps.pose.orientation = quatFromZYX(A, B, C);

JPOS → /robot/joint_states ：目前原样塞 position

cpp 复制代码

for (int i=0;i<6;++i) js.position[i] = j[i];

固件 JPOS 是"度"。如果后面要接 ROS 生态（robot_state_publisher/MoveIt），这里应该改成 rad。

3.1.6 它"怎么发运动"：/robot/ee_target 的缓存+闸门（避免刷屏）

订阅到目标时只缓存：

cpp 复制代码

void onEeTarget(...) {
  latest_ee_target_ = *msg;
  last_target_time_ = now;
  have_target_ = true;
  target_dirty_ = true;
}

真正发送发生在 sendLatestEeTargetIfDue()，按顺序：

timeout：超过 cmd_timeout_s_ 无新目标 → 停止发送
dirty：没新目标就不发（不重发同一条）
rate limit：min_send_interval_ms_
freeSize：队列空间不足不发
构造 @X,Y,Z,A,B,C\r\n（m→mm，quat→RPY→deg，A=yaw,B=pitch,C=roll）

关键片段：

cpp 复制代码

if (age_s > cmd_timeout_s_) {
  have_target_ = false;
  target_dirty_ = false;
  return;
}

if (!target_dirty_) return;

if (elapsed_ms < min_send_interval_ms_) return;

if (fs >= 0 && fs < min_free_size_to_send_) return;

quatToRPY(q, roll, pitch, yaw);
A_deg = yaw*180/pi; B_deg = pitch*180/pi; C_deg = roll*180/pi;

sendRaw("@X,Y,Z,A,B,C\r\n");
target_dirty_ = false;

3.1.7 `/robot/joint_target`：现在用它承载 `&x,y,z,a,b,c`

&j1..j6 不是关节角，而是：

前三项：位置 x,y,z（mm）
后三项：姿态 a,b,c（deg）
顺序同 LPOS：A=yaw(Z), B=pitch(Y), C=roll(X)（ZYX）

也就是说 & 实际上是"另一种笛卡尔命令"（不是关节空间）。这和 ROS JointState 的语义不一致，但作为固件接口的容器，勉强能用。

当前代码是"原样拼接 position[0..5]"：

cpp 复制代码

oss << "&"
    << msg->position[0] << ","
    ...
    << msg->position[5] << "\r\n";
sendRaw(oss.str());

这意味着：上层发布 /robot/joint_target 时，需要自己保证单位与顺序（mm + deg，xyzabc）。这点也属于未来重构要改的地方：定义一个专用消息或 topic（例如 /robot/cart_target_mmdeg），避免滥用 JointState。

3.2 键盘节点：`EeKeyboardTeleopMm`（ee_keyboard_teleop_xyz.cpp）

3.2.1 节点启动后做什么

termios raw 模式 + 非阻塞读键
订阅 /robot/ee_pose：拿到第一帧 pose 才初始化 start_，并锁定 orientation
定时器 60Hz：按键产生速度，积分得到偏移，发布 /robot/ee_target

第一次收到 pose 的初始化逻辑（原样）：

cpp 复制代码

if (!have_pose_) {
  have_pose_ = true;
  start_ = msg;
  locked_orientation_ = msg.pose.orientation;
  have_locked_orientation_ = true;
  off_x_mm_ = off_y_mm_ = off_z_mm_ = 0.0;
}

3.2.2 运动逻辑（start + off）

vx/vy/vz（mm/s）由按键 held 决定
off += v * dt
target = start + off/1000
orientation 默认锁定（防止姿态抖动）

3.2.3 回弹修复（核心 NEW 段）

用 idle_reset_delay_ms_ 做 idle 防抖，保证目标连续：

cpp 复制代码

static bool in_motion = false;

if (moving) {
  last_moving_time_ = now;
  if (!in_motion) { start_ = cur_; off=0; in_motion=true; }
} else {
  idle_ms = ...
  if (in_motion && idle_ms < idle_reset_delay_ms_) {
    // keep start/off (连续性)
  } else {
    start_ = cur_; off=0; in_motion=false;
    if (!send_when_idle_) return;
  }
}

4. 机械臂串口协议关键点梳理

!：系统控制（!START / !STOP / !HOME / !RESET / !DISABLE / ...）
#：查询与参数（#GETJPOS / #GETLPOS / #SET_* / ...）
@：笛卡尔运动（@X,Y,Z,A,B,C）
&：关节运动（&j1..j6 或可打断模式）
串口行结束符：\r\n
#GETJPOS / #GETLPOS：回 ok %.2f %.2f %.2f %.2f %.2f %.2f
运动命令返回：freeSize（一个整数，代表队列/缓冲剩余）
X/Y/Z：mm
A/B/C：欧拉角，度
欧拉角约定从源码确认：ZYX（Yaw-Pitch-Roll）
- R=Rz(A)Ry(B)Rx(C)R = R_z(A) R_y(B) R_x(C)R=Rz(A)Ry(B)Rx(C)
- A=yawA=\text{yaw}A=yaw，B=pitchB=\text{pitch}B=pitch，C=rollC=\text{roll}C=roll

5. 踩坑复盘

5.1 串口粘连：两帧拼成一帧

现象
/robot/driver_rx 偶尔出现两帧内容粘在同一条消息里。

根因

串口是字节流，read 一次可能返回多行；若不按 \r\n 切帧并消费缓冲，会粘连。

修复落点

模块：串口接收线程 rxLoop()
关键点：read_until("\r\n") + while 切帧 + buf.consume()

（复盘记录）问题现象与目标

现象：用键盘遥操作机械臂时，串口发给机械臂的目标位置（日志里的 @...）会出现不连续 ：
- 正向（例如按 d）递增到某个值后，突然回到更小值再递增（"回弹"）。
- 反向（例如按 a）递减过程中，也会出现突然跳回更大值再继续递减（倒退过程中的回弹）。
目标：让遥操作发布的目标位置序列在连续操作时保持连续、单调（按同方向时），不因为输入/反馈/串口查询产生跳变。

2) 架构梳理（我们确认了链路）

teleop 节点 ：读取键盘（termios 非阻塞），维护：
- cur_：订阅 /robot/ee_pose 得到的反馈位姿（QoS：transient_local + reliable）
- start_：运动锚点（用来"从某个参考点开始累计偏移"）
- off_x/y/z_mm_：按键累计的偏移量
- 发布 /robot/ee_target_pose
driver 节点 ：订阅目标位姿，转换成串口指令 @... 发给控制器；同时可选地周期发送 #GETLPOS 轮询当前位置并发布 /robot/ee_pose。

3) 第一轮定位：QoS/订阅是否导致"旧数据回放"

一开始怀疑：/robot/ee_pose 使用了 transient_local，teleop 也设置了 transient_local，可能导致 teleop 启动/重连时拿到旧 latched 值造成跳变。
teleop 订阅 /robot/ee_pose 的代码：
- KeepLast(1).transient_local().reliable()
结论：QoS 不再是主要矛盾（至少在这段复现日志里，回弹不是"历史消息回放"能解释的唯一原因），我们把重点转向 teleop 的状态机与串口时序。

4) 第二轮定位：回弹的核心机制（teleop 锚点被重置）

把日志模式与 teleop 逻辑对齐后，得到关键判断：

teleop 的目标计算本质是：
目标 = start_ + off_ （off_ 随按键每帧累加）
代码里有"只要检测到 moving=false 就立刻"：
- start_ = cur_
- off_ = 0
- 并把 prev_moving 置回 false
  这相当于立刻重置运动参考系。

而 moving 的来源是键盘"按住判定"（基于重复字符+超时），它会出现短暂空窗：

"先按 d 再按 a"，方向切换时非常容易出现 1～数帧没有键被判定 held；
或者终端 key-repeat 抖动、线程调度、串口输出/轮询干扰导致某些帧读不到字符；
一旦某帧 moving=false，teleop 就会重置 start_ 和 off_；
下一帧又 moving=true，又可能在"rising edge"再次用 cur_ 锚定；
cur_ 是反馈，可能滞后于刚发的目标（尤其当 driver 在运动中插入 #GETLPOS 时反馈节奏更乱）；
于是目标就出现跳回旧锚点附近，表现为看到的"回弹"。

后续补充的关键信息：

后半段倒退是按 a 的正常结果；
但倒退中出现回弹，说明不是"方向逻辑错了"，而是倒退过程中发生了锚点重置/反馈滞后触发的跳变。

5) 第三轮定位：driver 轮询 `#GETLPOS` 是放大器

#GETLPOS 会穿插在连续的 @... 目标流中。
回弹现象往往出现在 #GETLPOS 附近。

判断：

#GETLPOS 本身不一定"直接改目标"，但它会：
- 抢占串口带宽、打断发送节奏；
- 让反馈更新更不稳定、更滞后；
- 增大 teleop 键盘读取/线程调度抖动概率；
从而更容易触发 teleop 的 moving=false 短暂空窗 → 锚点重置 → 回弹。

贴关键代码：

cpp 复制代码

asio::read_until(serial_, buf, "\r\n", ec);

while (true) {
  auto it = std::search(data_begin, data_end, "\r\n", "\r\n" + 2);
  if (it == data_end) break;

  std::string line(data_begin, it);
  buf.consume(std::distance(data_begin, it) + 2);
  ...
  rx_pub_->publish(rx);
  handleLine(line);
}

验证

bash 复制代码

ros2 topic echo /robot/driver_rx

观察每条 data: 都只包含一帧（不会再出现 ...ok...ok... 同条）。

5.2 freeSize 的 `15ok`：流控信息拿不到

现象

运动命令后固件回 15ok，如果不解析就无法更新 last_free_size_，流控就"失明"。

修复落点

模块：协议解析 handleLine()
关键代码：

cpp 复制代码

if (auto sp = split_int_prefix(line)) {
  if (is_integer_string(sp->first) && sp->second == "ok") {
    last_free_size_.store(std::stoi(sp->first));
    return;
  }
}

验证

ros2 topic echo /robot/driver_rx 能看到 15ok
driver 内部 last_free_size_ 会更新（通过后续"发送被抑制/放行"的行为间接验证）

5.3 关掉 teleop 后 driver 仍持续发 `@...`

现象

teleop 退出后串口仍刷 @...，控制器一直被喂同目标。

根因

driver 若"定时重发最后目标"且无超时释放，就会一直发。

修复落点

模块：运动目标发送器 sendLatestEeTargetIfDue()
两个关键机制：
1. cmd_timeout_s_：无新目标自动停控
2. target_dirty_：只发新目标，不重发旧目标

（复盘记录）首先是必须重启 driver 才能初始化keyboard节点

key 节点（EeKeyboardTeleopMm）有一个硬门槛：

只有在订阅到一次 /robot/ee_pose 后才会 have_pose_=true 并初始化 start_
在 onTimer() 里如果 !have_pose_ 直接 return，因此 key 不会发布任何 /robot/ee_target 来推动系统进入"活起来"的状态

这会产生典型的启动时序/竞态：

driver 是否发布 /robot/ee_pose，取决于串口 poll 是否跑、以及 #GETLPOS 回包是否被正确解析并触发 publish
如果 key 启动时恰好错过了 driver 的第一帧（或 driver 当时根本没成功发布），key 就一直卡在 have_pose_=false
重启 driver后，driver 常会重新开始 poll 并更快产出一帧 /robot/ee_pose，key 立刻收到并初始化成功看起来像"必须重启 driver 才能初始化

key 把初始化完全绑定在能否及时收到第一帧 pose上导致系统对时序非常敏感。

贴关键代码：

cpp 复制代码

if (age_s > cmd_timeout_s_) {
  have_target_.store(false);
  target_dirty_.store(false);
  return;
}

if (!target_dirty_.load()) return;
...
if (ok) target_dirty_.store(false);

验证

bash 复制代码

ros2 topic echo /robot/driver_tx

停止按键后，应在 cmd_timeout_s 内不再出现 @...。

5.4 键盘遥操作"回弹/跳变"

现象

同方向按住移动时，目标会突然跳回再继续；方向切换时更明显。

根因（结合 teleop 的"held"实现）

moving 判定依赖"最近一次看到某个键的时间"
方向切换/抖动会导致 moving 短暂 false
如果 idle 立刻 start=cur、off=0，会用滞后反馈重新锚定 → 回弹

修复落点

模块：teleop 状态机（EeKeyboardTeleopMm::onTimer() NEW 段）
核心策略：
- 进入运动时只锚定一次
- idle 防抖（短暂空窗不 reset）

cpp 复制代码

if (in_motion && idle_ms < static_cast<double>(idle_reset_delay_ms_)) {
  // do nothing; keep start_/off_ so trajectory remains continuous
} else {
  start_ = cur_;
  off_x_mm_ = off_y_mm_ = off_z_mm_ = 0.0;
  in_motion = false;
  if (!send_when_idle_) return;
}

验证

观察 /robot/driver_tx 中 @X,... 连续、单调（同方向）不再回弹
或在上层把目标打印出来，看 start+off 连续

5.5 为什么 poll 会放大卡顿/回弹了，以及在驱动里怎么抑制

现象

运动时穿插 #GETLPOS/#GETJPOS 会抢串口、打断命令节奏
反馈不稳定会间接让 teleop 更容易锚定到滞后值

修复落点

模块：poll 抑制
关键代码：

发送期间暂停 poll：

cpp 复制代码

if (pause_poll_while_sending_ && sending_motion_.load()) return;

运动后窗口抑制 poll：

cpp 复制代码

if (shouldSuppressPoll()) return;

窗口判断：

cpp 复制代码

const double age_ms = (now - last_motion_send_time_).seconds() * 1000.0;
return age_ms < suppress_poll_while_moving_ms_;

验证

运动期间 /robot/driver_tx 中 #GET... 明显减少
体感卡顿减轻，回弹复现率下降

6. 调试指令

我基本靠这几条命令做"串口抓包 + ROS 链路观测"：

bash 复制代码

ros2 topic echo /robot/driver_tx
ros2 topic echo /robot/driver_rx

ros2 topic echo /robot/ee_pose
ros2 topic echo /robot/joint_states

ros2 service list | grep /robot
ros2 service call /robot/disable std_srvs/srv/Trigger "{}"

/driver_tx + /driver_rx 是驱动调试的重点。

7. 下一阶段重构方向

消息语义重构：不要再用 JointState 承载 &xyzabc
- 现在 onJointTarget() 实际是笛卡尔命令（mm+deg）
- 后续建议换成：
  - 新 topic：/robot/cart_target_mmdeg（自定义 msg 或 Pose+额外单位约定）
  - 或完全废弃 &，统一走 @（保持一个笛卡尔入口）
单位层集中管理（Unit Adapter）
- LPOS 已做 mm→m、deg→rad
- JPOS 目前仍是 deg 原样发布重构时应在同一层统一：固件单位 ↔ ROS 单位
把 driver 拆成 4 个类
- SerialTransport：asio open/read/write + 线程
- ProtocolCodec：parse line / build commands
- PollScheduler：状态机 + 查询节拍
- RosBridge：pub/sub/srv/timer，拼成节点

RK3588 上 ROS2 Humble + 串口机械臂驱动（ROS2安装流程 + V1版本Serial驱动）

0.ROS2安装

0) 基础检查（建议先做）

1) 设置 locale

2) 添加 ROS 2 软件源与 key（Jammy）

3) 安装 ROS 2 Humble

全家桶

轻量

4) 配置环境变量（每次开终端自动生效）

5) 安装常用构建与开发工具

6) 快速自检：运行 demo

1. 系统目标与数据流

2. 工程结构与运行方式

3. 源码解构

3.1 驱动节点：RobotSerialDriver（robot_serial_driver_node.cpp）

3.1.1 启动后做的事：构造函数里把系统"架起来"

3.1.2 它"什么时候发什么"：poll 状态机（稳但简单）

3.1.3 它"怎么收串口"：rxLoop 切帧策略（解决粘连的核心）

3.1.4 它"怎么解析"：handleLine 支持哪些固件返回

3.1.5 它"发布什么"：LPOS / JPOS 转 ROS 的单位约定

3.1.6 它"怎么发运动"：/robot/ee_target 的缓存+闸门（避免刷屏）

3.1.7 /robot/joint_target：现在用它承载 &x,y,z,a,b,c

3.2 键盘节点：EeKeyboardTeleopMm（ee_keyboard_teleop_xyz.cpp）

3.2.1 节点启动后做什么

3.2.2 运动逻辑（start + off）

3.2.3 回弹修复（核心 NEW 段）

4. 机械臂串口协议关键点梳理

5. 踩坑复盘

5.1 串口粘连：两帧拼成一帧

（复盘记录）问题现象与目标

2) 架构梳理（我们确认了链路）

3) 第一轮定位：QoS/订阅是否导致"旧数据回放"

4) 第二轮定位：回弹的核心机制（teleop 锚点被重置）

5) 第三轮定位：driver 轮询 #GETLPOS 是放大器

5.2 freeSize 的 15ok：流控信息拿不到

5.3 关掉 teleop 后 driver 仍持续发 @...

（复盘记录）首先是必须重启 driver 才能初始化keyboard节点

5.4 键盘遥操作"回弹/跳变"

5.5 为什么 poll 会放大卡顿/回弹了，以及在驱动里怎么抑制

6. 调试指令

7. 下一阶段重构方向

3.1 驱动节点：`RobotSerialDriver`（robot_serial_driver_node.cpp）

3.1.7 `/robot/joint_target`：现在用它承载 `&x,y,z,a,b,c`

3.2 键盘节点：`EeKeyboardTeleopMm`（ee_keyboard_teleop_xyz.cpp）

5) 第三轮定位：driver 轮询 `#GETLPOS` 是放大器

5.2 freeSize 的 `15ok`：流控信息拿不到

5.3 关掉 teleop 后 driver 仍持续发 `@...`