7. 追加队列模式(单机器人)
本期介绍如何用队列模式把多段运动指令一次性下发到控制器,由控制器按顺序连续执行,并在执行过程中监控队列剩余数量与机器人运行状态。
队列模式适合「上位机先生成一段完整轨迹、再交给控制器连续执行」的场景。相比逐条下发运动指令,队列模式不会出现两条指令之间的停顿,控制器的轨迹衔接更平滑。
⚠️ 安全警告
- 运行前确认机器人周围无人员和障碍物
- 首次运行请使用低速
- 建议先在仿真环境中验证,再上真机
- 随时准备按下急停按钮
1、工作原理
队列模式分为四个阶段:
打开队列模式 → 清空历史队列 → 本地追加指令(MoveJ / Timer / MoveL)
→ 本地队列长度确认 → 发送到控制器 → 监控执行完成 → 关闭队列模式关键点在于「本地追加」与「发送执行」是分离的两步。追加阶段只在上位机侧构建队列,只有调用发送接口后控制器才开始执行。
2、前置条件
本 Demo 只覆盖队列本身的构建与执行,不包含上电流程。运行前需要:
- 机器人已完成伺服上电,处于就绪状态
- 机器人处于允许运动的模式(远程模式)
- 队列中的目标点在软限位内且可达
若尚未上电,先运行 1. 上电流程 或 6. 无作业文件运行,再回到本页。
3、配置参数
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_SPEED | 60 | 关节速度百分比 |
MOVEL_SPEED | 600 | 直线速度 mm/s |
QUEUE_TIMER | 2 | 队列中插入的 Timer 时长(秒) |
MONITOR_MAX_COUNT | 120 | 监控轮询次数上限,防止死循环 |
MOTION_FINISH_CONFIRM_COUNT | 2 | 完成判据需连续确认的次数 |
4、打开、清空、关闭队列
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、追加运动指令
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 改成小写。
运动指令的构造与直接运动一致:
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 cmd6、确认本地队列长度
发送之前必须确认队列非空,否则会出现「发送成功但什么都没执行」:
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}次获取队列长度失败")size = get_queue_size(fd)
print("当前队列数量:", size)
if size == 0:
raise Exception("队列添加失败")7、发送并监控执行
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」且「机器人运行状态不是运行」,并且连续确认多次:
def monitor_queue(fd):
# 队列剩余数量为0,且机器人状态不是运行状态,
# 并连续确认 MOTION_FINISH_CONFIRM_COUNT 次后,认为队列执行完成
...只用队列长度为 0 判断是不够的:控制器可能已经取出最后一条运动指令,但机器人仍在运动过程中。必须同时检查运行状态,并连续确认,避免状态刷新延迟导致的过早判定。
MONITOR_MAX_COUNT = 120 是轮询次数上限。队列内容被改长后需要同步调大,否则会在轨迹尚未走完时退出监控。
8、主流程
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、运行与预期输出
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。