Skip to content
🤖 AI 助手 / 自动化工具:先读 《AI 助手先读》 《Agent 开发指引》 《生成前信息采集清单》;机器可读入口 /llms.txt;技能包(SKILL)直链:SKILL.md

7. 追加队列模式(单机器人) ​

本期介绍如何用队列模式把多段运动指令一次性下发到控制器,由控制器按顺序连续执行,并在执行过程中监控队列剩余数量与机器人运行状态。

队列模式适合「上位机先生成一段完整轨迹、再交给控制器连续执行」的场景。相比逐条下发运动指令,队列模式不会出现两条指令之间的停顿,控制器的轨迹衔接更平滑。

⚠️ 安全警告

  • 运行前确认机器人周围无人员和障碍物
  • 首次运行请使用低速
  • 建议先在仿真环境中验证,再上真机
  • 随时准备按下急停按钮

1、工作原理 ​

队列模式分为四个阶段:

text
打开队列模式 → 清空历史队列 → 本地追加指令(MoveJ / Timer / MoveL)
    → 本地队列长度确认 → 发送到控制器 → 监控执行完成 → 关闭队列模式

关键点在于「本地追加」与「发送执行」是分离的两步。追加阶段只在上位机侧构建队列,只有调用发送接口后控制器才开始执行。

2、前置条件 ​

本 Demo 只覆盖队列本身的构建与执行,不包含上电流程。运行前需要:

  • 机器人已完成伺服上电,处于就绪状态
  • 机器人处于允许运动的模式(远程模式)
  • 队列中的目标点在软限位内且可达

若尚未上电,先运行 1. 上电流程 或 6. 无作业文件运行,再回到本页。

3、配置参数 ​

python
ip = "192.168.3.239"            # 改为你的控制器 IP
port = "6001"                   # 改为你的控制器通信端口

point1 = [450, 250, 300, 3.1415, 0, 0, 0]
point2 = [500, 200, 500, 3.1415, 0, 0, 0]

MOVEJ_SPEED = 60
MOVEL_SPEED = 600
MOVE_ACC = 50
MOVE_DEC = 50

QUEUE_TIMER = 2
STATE_QUERY_RETRY = 3
QUEUE_QUERY_RETRY = 3
MONITOR_MAX_COUNT = 120
MOTION_FINISH_CONFIRM_COUNT = 2
参数值说明
MOVEJ_SPEED60关节速度百分比
MOVEL_SPEED600直线速度 mm/s
QUEUE_TIMER2队列中插入的 Timer 时长(秒)
MONITOR_MAX_COUNT120监控轮询次数上限,防止死循环
MOTION_FINISH_CONFIRM_COUNT2完成判据需连续确认的次数

4、打开、清空、关闭队列 ​

python
def open_queue(fd):
    for i in range(STATE_QUERY_RETRY):
        result = nrc_interface.queue_motion_set_status(fd, True)
        if result == 0:
            print("队列模式打开成功")
            return
        print(f"第{i+1}次打开队列模式失败,返回值:{result}")
        if i < STATE_QUERY_RETRY - 1:
            time.sleep(WAIT_RETRY)
    raise Exception(f"连续{STATE_QUERY_RETRY}次打开队列模式失败")


def clear_queue(fd):
    for i in range(STATE_QUERY_RETRY):
        result = nrc_interface.queue_motion_clear_Data(fd)
        if result == 0:
            print("历史队列清空成功")
            return
        print(f"第{i+1}次清空队列失败,返回值:{result}")
        if i < STATE_QUERY_RETRY - 1:
            time.sleep(WAIT_RETRY)
    raise Exception(f"连续{STATE_QUERY_RETRY}次清空队列失败")

open_queue 与 close_queue 是同一接口的两个方向:queue_motion_set_status(fd, True) 打开,queue_motion_set_status(fd, False) 关闭。清空历史队列是必要步骤,否则上次残留的指令会被一起执行。

clear_Data 的大小写与 SDK 头文件保持一致,Python 封装未做重命名,写成 clear_data 会报 AttributeError。

5、追加运动指令 ​

python
def push_motion(fd):
    cmd1 = create_move_cmd(point1, MOVEJ_SPEED)
    cmd2 = create_move_cmd(point2, MOVEL_SPEED)

    check_result(nrc_interface.queue_motion_push_back_moveJ(fd, cmd1), "添加MoveJ")
    check_result(nrc_interface.queue_motion_push_back_timer(fd, QUEUE_TIMER), "添加Timer")
    check_result(nrc_interface.queue_motion_push_back_moveL(fd, cmd2), "添加MoveL")

    print("队列指令添加完成")

三个追加接口的区别:

接口作用参数
queue_motion_push_back_moveJ追加关节运动MoveCmd
queue_motion_push_back_moveL追加直线运动MoveCmd
queue_motion_push_back_timer追加延时等待秒数

注意接口名中的 moveJ / moveL 是大写 J、L,这是 SDK 原始命名,不能按 PEP8 改成小写。

运动指令的构造与直接运动一致:

python
def create_move_cmd(pos, vel):
    cmd = nrc_interface.MoveCmd()
    for i in range(len(pos)):
        cmd.targetPosValue[i] = pos[i]
    cmd.coord = 1        # 使用指定坐标系执行运动,保证目标位置按照预期坐标系解释
    cmd.velocity = vel   # 根据运动类型使用不同速度,MoveJ和MoveL可分别控制运动节奏
    cmd.acc = MOVE_ACC   # 统一使用配置的加速度,避免在代码中分散修改运动参数
    cmd.dec = MOVE_DEC   # 统一使用配置的减速度,避免机器人停止时产生过大的运动冲击
    cmd.isSync = True    # 使用同步执行方式,确保运动指令按照队列顺序进行执行
    return cmd

6、确认本地队列长度 ​

发送之前必须确认队列非空,否则会出现「发送成功但什么都没执行」:

python
def get_queue_size(fd):
    for i in range(QUEUE_QUERY_RETRY):
        ret = nrc_interface.queue_motion_size(fd, 0)
        if isinstance(ret, list) and len(ret) >= 2 and ret[0] == 0:
            return ret[1]
        print(f"第{i+1}次获取队列长度失败")
        if i < QUEUE_QUERY_RETRY - 1:
            time.sleep(WAIT_RETRY)
    raise Exception(f"连续{QUEUE_QUERY_RETRY}次获取队列长度失败")
python
size = get_queue_size(fd)
print("当前队列数量:", size)

if size == 0:
    raise Exception("队列添加失败")

7、发送并监控执行 ​

python
def send_queue(fd):
    for i in range(STATE_QUERY_RETRY):
        result = nrc_interface.queue_motion_send_to_controller(fd, 0, False)
        if result == 0:
            print("队列发送成功")
            return
        print(f"第{i+1}次发送队列失败,返回值:{result}")
        if i < STATE_QUERY_RETRY - 1:
            time.sleep(WAIT_RETRY)
    raise Exception(f"连续{STATE_QUERY_RETRY}次发送队列失败")

完成判据是「队列剩余数量为 0」且「机器人运行状态不是运行」,并且连续确认多次:

python
def monitor_queue(fd):
    # 队列剩余数量为0,且机器人状态不是运行状态,
    # 并连续确认 MOTION_FINISH_CONFIRM_COUNT 次后,认为队列执行完成
    ...

只用队列长度为 0 判断是不够的:控制器可能已经取出最后一条运动指令,但机器人仍在运动过程中。必须同时检查运行状态,并连续确认,避免状态刷新延迟导致的过早判定。

MONITOR_MAX_COUNT = 120 是轮询次数上限。队列内容被改长后需要同步调大,否则会在轨迹尚未走完时退出监控。

8、主流程 ​

python
def main():
    fd = None
    queue_open = False

    try:
        fd = connect_robot()

        open_queue(fd)
        queue_open = True

        clear_queue(fd)
        push_motion(fd)

        size = get_queue_size(fd)
        print("当前队列数量:", size)
        if size == 0:
            raise Exception("队列添加失败")

        send_queue(fd)
        monitor_queue(fd)

        close_queue(fd)
        queue_open = False

        print("Demo7执行完成")
    ...

queue_open 标记用于异常路径:只有确实打开过队列,except 分支才尝试关闭,避免对未打开的队列调用关闭接口产生二次错误。

9、运行与预期输出 ​

bash
python 07_queue_single.py

预期输出依次出现:机器人连接成功、队列模式打开成功、历史队列清空成功、队列指令添加完成、当前队列数量: 3、队列发送成功、执行过程中反复打印 剩余队列: X 机器人状态: Y、队列运动完成、队列模式关闭成功、Demo7执行完成、连接关闭。

队列长度为 3 是因为追加了 MoveJ、Timer、MoveL 三条指令。

10、常见问题 ​

当前队列数量为 0

push_motion 中某个接口返回非 0 被 check_result 抛出,但被上层吞掉。先确认追加阶段的三次调用都返回 0。

队列发送成功但机器人不动

检查伺服是否已上电、机器人是否处于允许运动的模式。本 Demo 不含上电流程,这是最常见的原因。

monitor_queue 提前退出

MONITOR_MAX_COUNT 偏小,或完成判据中的连续确认次数被改为 1。队列越长,轮询上限越要提高。

AttributeError: clear_data

接口真实名称是 queue_motion_clear_Data,注意 Data 为驼峰写法。

下一步

需要把作业文件或控制器日志备份到上位机,参见 8. 上传下载文件。

完整可运行脚本见 devkit:04-上位机/Python/RTL-24.03(当前)/04-示例/基础示例/07-追加队列模式/07_queue_single.py。

← 上一页:6. 无作业文件运行 | 返回基础应用 | 下一步:8. 上传下载文件 →