8. 追加队列模式（单机器人）
本期将介绍如何在单机器人场景下开启追加队列模式、向本地队列追加指令、将队列发送到控制器执行，并在确认全部指令完成后安全关闭队列模式。
当前 Demo 只向队列中追加延时指令和运动指令，程序会同时监控队列模式开关、控制器剩余队列长度和机器人运行状态，避免仅依赖单一状态判断队列是否完成。
完整流程如下：
- 确认伺服已经进入状态 3，机器人当前处于停止状态。
- 保存控制器原始模式，并在需要时切换到远程模式。
- 开启追加队列模式，同时确认控制器已进入队列模式。
- 向本地队列追加配置数量的延时指令和运动指令。
- 将全部本地队列发送到控制器并立即执行。
- 轮询队列模式、剩余指令数和机器人运行状态，确认队列执行完成。
- 关闭追加队列模式，并在安全条件满足时恢复控制器原始模式。
- 发生异常时优先使用不下电停止接口终止队列，再关闭队列模式。
⚠️ 安全警告
运行前必须确认：
- 队列占用： 控制器当前没有其他队列任务、运动任务或暂停任务
- 清空风险： 开启或关闭追加队列模式都会清空控制器中的已有队列
- 伺服状态： 伺服已经上电并进入状态 3
- 控制器模式： 控制器允许切换到远程模式并执行追加队列
- 异常停止： 队列状态异常时，应优先确认机器人状态，再执行不下电停止
- 运动扩展： 使用MOVJ 或 MOVL运动指令时，必须使用真实示教点，并完成路径和现场安全检查
不要在生产队列仍然存在或机器人正在执行其他任务时运行本 Demo，否则开启队列模式可能清空或干扰现有任务。
1、配置控制器和队列参数
首先配置控制器 IP 地址和 SDK 服务端口，运行前必须确认程序连接的是预期控制器。
cpp
/**
* @name 用户必须根据运行环境修改
* 以下配置决定 SDK 能否连接到实际控制器，运行前必须核对。
* @{
*/
const std::string robot_ip = "192.168.3.243"; ///< 实际控制器的 IP 地址。
const std::string robot_port = "6001"; ///< 实际控制器的 SDK 服务端口，必须与控制器配置一致。
/** @} */示例默认追加 3 条延时 2 秒的指令，预计总执行时间约为 6 秒。修改指令数量或持续时间后，需要同步确认队列完成等待超时时间。
cpp
/**
* @name 客户需根据本 Demo 修改
* 以下配置决定队列中的延时指令内容及完成等待时间；修改队列内容时必须同步确认超时时间。
* @{
*/
constexpr int TIMER_COMMAND_COUNT = 3; ///< 追加到本地队列的延时指令数量。
constexpr double TIMER_COMMAND_SECONDS = 2.0; ///< 每条延时指令的持续时间，单位为秒。
constexpr int QUEUE_COMPLETE_TIMEOUT_SECONDS = 30; ///< 等待追加队列执行完成的总超时时间，单位为秒。
/** @} */状态切换、轮询和通信重试参数应结合控制器响应速度进行设置。
cpp
constexpr int QUEUE_STATE_TIMEOUT_SECONDS = 10; ///< 等待队列模式切换或安全停止的总超时时间，单位为秒。
constexpr int POLL_INTERVAL_MS = 200; ///< 队列模式及执行状态轮询间隔，单位为毫秒。
constexpr int QUERY_RETRY_INTERVAL_MS = 500; ///< 状态查询失败后的重试间隔，单位为毫秒。
constexpr int MAX_QUERY_FAILURES = 3; ///< 状态查询允许的最大连续失败次数。
constexpr int STOPPED_CONFIRMATION_COUNT = 5; ///< 未观察到执行状态时，判定完成所需的连续停止状态次数。
constexpr int REMOTE_MODE = 1; ///< 追加队列直接控制使用的远程模式。2、定义队列状态快照
判断队列是否完成需要同时观察多个状态。程序使用 QueueSnapshot 保存一次采样得到的队列模式、剩余指令数量和机器人运行状态。
cpp
/**
* @brief 一次队列状态采样结果。
*/
struct QueueSnapshot
{
bool queue_mode_open = false; ///< 追加队列模式是否开启。
int remaining_commands = 0; ///< 控制器中尚未完成的队列指令数量。
int robot_running_state = 0; ///< 当前机器人运行状态码。
};3、封装 SDK 返回值检查和查询重试
普通控制接口使用 check_sdk_result 统一检查返回值；状态查询使用 query_with_retry 处理短暂通信失败。查询返回 DISCONNECT 时会立即停止重试，因为同一连接句柄已经无法继续使用。
cpp
bool check_sdk_result(Result result, const std::string& operation)
{
if (result == SUCCESS)
{
return true;
}
std::cerr << operation << "失败，错误码："
<< static_cast<int>(result) << std::endl;
return false;
}
template <typename Query>
bool query_with_retry(const std::string& operation, Query query)
{
for (int attempt = 1; attempt <= MAX_QUERY_FAILURES; ++attempt)
{
const Result result = query();
if (result == SUCCESS)
{
return true;
}
std::cerr << operation << "失败，第 " << attempt << "/"
<< MAX_QUERY_FAILURES << " 次，错误码："
<< static_cast<int>(result) << std::endl;
if (result == DISCONNECT)
{
std::cerr << "控制器连接已经断开，停止重试" << std::endl;
return false;
}
if (attempt < MAX_QUERY_FAILURES)
{
std::this_thread::sleep_for(
std::chrono::milliseconds(QUERY_RETRY_INTERVAL_MS));
}
}
std::cerr << operation << "连续失败，停止当前流程" << std::endl;
return false;
}伺服状态、机器人运行状态、控制器模式、队列模式和剩余队列长度分别使用轻量包装函数查询。这些函数复用同一套重试策略，并提供明确的查询名称。
cpp
bool query_servo_state(SOCKETFD fd, int& state)
{
return query_with_retry("获取伺服状态", [&]() {
return get_servo_state(fd, state);
});
}
bool query_running_state(SOCKETFD fd, int& state)
{
return query_with_retry("获取机器人运行状态", [&]() {
return get_robot_running_state(fd, state);
});
}
bool query_current_mode(SOCKETFD fd, int& mode)
{
return query_with_retry("获取机器人当前模式", [&]() {
return get_current_mode(fd, mode);
});
}
bool query_queue_mode(SOCKETFD fd, bool& status)
{
return query_with_retry("获取追加队列模式状态", [&]() {
return queue_motion_get_status(fd, status);
});
}
bool query_queue_remaining(SOCKETFD fd, int& remaining_commands)
{
return query_with_retry("获取控制器剩余队列长度", [&]() {
return queue_motion_get_queuelen(fd, remaining_commands);
});
}4、封装完整队列状态采样函数
read_queue_snapshot 依次读取队列模式、剩余指令数量和机器人运行状态，并检查剩余数量是否为负数、运行状态是否位于 0～2 范围内。
cpp
bool read_queue_snapshot(SOCKETFD fd, QueueSnapshot& snapshot)
{
if (!query_queue_mode(fd, snapshot.queue_mode_open))
return false;
if (!query_queue_remaining(fd, snapshot.remaining_commands))
return false;
if (!query_running_state(fd, snapshot.robot_running_state))
return false;
if (snapshot.remaining_commands < 0)
{
std::cerr << "控制器返回非法剩余队列长度："
<< snapshot.remaining_commands << std::endl;
return false;
}
if (snapshot.robot_running_state < 0
|| snapshot.robot_running_state > 2)
{
std::cerr << "控制器返回未知运行状态："
<< snapshot.robot_running_state << std::endl;
return false;
}
return true;
}运行状态码在本 Demo 中的含义为：状态 0 表示停止，状态 1 表示暂停，状态 2 表示运行。
5、封装远程模式准备函数
追加队列直接控制要求控制器处于远程模式。prepare_remote_mode 保存原始模式，在需要时切换到模式 1，并重新查询确认切换结果。
cpp
bool prepare_remote_mode(SOCKETFD fd, int& original_mode)
{
if (!query_current_mode(fd, original_mode))
return false;
if (original_mode < 0 || original_mode > 2)
{
std::cerr << "控制器返回未知模式：" << original_mode << std::endl;
return false;
}
if (original_mode == REMOTE_MODE)
{
std::cout << "控制器已处于远程模式" << std::endl;
return true;
}
if (!check_sdk_result(
set_current_mode(fd, REMOTE_MODE),
"切换控制器到远程模式"))
return false;
int confirmed_mode = -1;
if (!query_current_mode(fd, confirmed_mode))
return false;
if (confirmed_mode != REMOTE_MODE)
{
std::cerr << "远程模式确认失败，当前模式："
<< confirmed_mode << std::endl;
return false;
}
std::cout << "控制器已切换到远程模式" << std::endl;
return true;
}6、开启并确认追加队列模式
queue_motion_set_status(fd, true) 会开启追加队列模式，同时清空控制器中的已有队列。接口返回成功后，还需要持续查询队列模式状态，确认控制器已经真正完成切换。
wait_for_queue_mode 单独封装队列模式轮询和超时；start_append_queue 则负责发送开启命令并调用等待函数确认结果。这样可以区分“命令发送成功”和“模式已经生效”两个阶段。
cpp
bool wait_for_queue_mode(
SOCKETFD fd,
bool expected_status,
std::chrono::seconds timeout =
std::chrono::seconds(QUEUE_STATE_TIMEOUT_SECONDS))
{
using clock = std::chrono::steady_clock;
const auto deadline = clock::now() + timeout;
while (clock::now() < deadline)
{
bool current_status = false;
if (!query_queue_mode(fd, current_status))
return false;
if (current_status == expected_status)
{
std::cout << "追加队列模式已"
<< (expected_status ? "开始" : "结束") << std::endl;
return true;
}
std::this_thread::sleep_for(
std::chrono::milliseconds(POLL_INTERVAL_MS));
}
std::cerr << "等待追加队列模式"
<< (expected_status ? "开始" : "结束")
<< "超时" << std::endl;
return false;
}
bool start_append_queue(SOCKETFD fd)
{
if (!check_sdk_result(
queue_motion_set_status(fd, true),
"开始追加队列模式"))
{
return false;
}
return wait_for_queue_mode(fd, true);
}7、追加队列指令
append_relative_movej 按运动配置调用 queue_motion_push_back_moveJ，将MoveJ运动指令追加到上位机本地队列。
cpp
/**
* @brief 向本地追加队列写入基于当前关节位置的相对 MoveJ 指令
* @param[in] fd 控制器连接句柄
* @retval true 运动指令追加成功
* @retval false 指令追加失败
*
* @details 查询当前关节坐标后，将关节 1 增加 10° 作为目标位置。
* @warning 本函数会生成实际运动；调用前必须确认目标点可达、无碰撞且现场安全。
*/
bool append_relative_movej(SOCKETFD fd)
{
/// @brief 保存待追加的关节运动参数。
MoveCmd movej;
movej.coord = 0; ///< 使用关节坐标系。
/// @brief 指定目标位置由数值数组直接提供。
movej.targetPosType = PosType::data;
/// @brief 读取当前关节位置，作为相对运动的安全起点。
if (!check_sdk_result(get_current_position(fd, 0, movej.targetPosValue), "获取当前的关节坐标"))
{
return false;
}
movej.targetPosValue[0] += 10; ///< 关节 1 相对当前位置向正方向移动 10°。
movej.velocity = 50; ///< 关节速度百分比，取值范围为 [1, 100]。
movej.acc = 50; ///< 关节运动加速度百分比。
movej.dec = 50; ///< 关节运动减速度百分比。
movej.pl = 5; ///< 轨迹平滑系数。
/// @brief 将已完成参数设置的 MoveJ 指令写入本地追加队列。
std::cout << "正在追加关节运动指令" << std::endl;
return check_sdk_result(queue_motion_push_back_moveJ(fd, movej), "追加MoveJ运动指令");
}append_relative_moveL 按运动配置调用 queue_motion_push_back_moveL，将MoveL运动指令追加到上位机本地队列。
cpp
/**
* @brief 向本地追加队列写入基于当前直角坐标的相对 MoveL 指令
* @param[in] fd 控制器连接句柄
* @retval true 运动指令追加成功
* @retval false 指令追加失败
*
* @details 查询当前直角坐标后，将 X 轴增加 10 mm 作为目标位置。
* @warning 本函数会生成实际运动；调用前必须确认目标点可达、无碰撞且现场安全。
*/
bool append_relative_moveL(SOCKETFD fd)
{
/// @brief 保存待追加的直线运动参数。
MoveCmd moveL;
moveL.coord = 1; ///< 使用直角坐标系。
/// @brief 指定目标位置由数值数组直接提供。
moveL.targetPosType = PosType::data;
/// @brief 读取当前直角坐标，作为相对运动的安全起点。
if (!check_sdk_result(get_current_position(fd, 1, moveL.targetPosValue), "获取当前的直角坐标"))
{
return false;
}
moveL.targetPosValue[0] += 10; ///< X 轴相对当前位置向正方向移动 10 mm。
moveL.velocity = 500; ///< 直线速度，取值范围为 [1, 1000] mm/s。
moveL.acc = 50; ///< 直线运动加速度百分比。
moveL.dec = 50; ///< 直线运动减速度百分比。
moveL.pl = 5; ///< 轨迹平滑系数。
/// @brief 将已完成参数设置的 MoveL 指令写入本地追加队列。
std::cout << "正在追加直线运动指令" << std::endl;
return check_sdk_result(queue_motion_push_back_moveL(fd, moveL), "追加MoveL运动指令");
}append_timer_commands 按配置数量循环调用 queue_motion_push_back_timer，将延时指令追加到上位机本地队列。任一指令追加失败时立即停止，避免发送内容不完整的队列。
cpp
/**
* @brief 向本地追加队列写入配置数量的延时指令
* @param[in] fd 控制器连接句柄
* @retval true 全部延时指令均追加成功
* @retval false 任一延时指令追加失败
*/
bool append_timer_commands(SOCKETFD fd)
{
for (int index = 0; index < TIMER_COMMAND_COUNT; ++index)
{
if (!check_sdk_result(
queue_motion_push_back_timer(fd, TIMER_COMMAND_SECONDS),
"追加第 " + std::to_string(index + 1) + " 条延时指令"))
{
return false;
}
}
std::cout << "正在追加延时指令" << std::endl;
return true;
}8、发送队列指令
execute_append_queue 将本地队列发送到控制器。size = 0 表示发送本地队列中的全部指令，isContinue = false 表示发送后立即执行。
cpp
bool execute_append_queue(SOCKETFD fd)
{
if (!check_sdk_result(
queue_motion_send_to_controller(fd, 0, false),
"发送并执行追加队列"))
{
return false;
}
std::cout << "追加队列已经发送到控制器" << std::endl;
return true;
}9、封装队列完成等待函数
仅检测剩余指令数为 0，可能会把“队列尚未开始”误判为“队列已经完成”。wait_for_queue_completion 同时检查以下条件：
- 追加队列模式在执行期间始终保持开启
- 机器人曾进入运行状态，或剩余指令数曾大于 0
- 最终剩余指令数为 0，机器人运行状态为 0
- 未观察到执行状态时，连续多次确认停止状态
- 配置的延时指令预计总时长已经过去
cpp
bool wait_for_queue_completion(
SOCKETFD fd,
std::chrono::seconds timeout =
std::chrono::seconds(QUEUE_COMPLETE_TIMEOUT_SECONDS))
{
using clock = std::chrono::steady_clock;
const auto start_time = clock::now();
const auto deadline = start_time + timeout;
const auto earliest_completion = start_time + std::chrono::milliseconds(
static_cast<int>(
TIMER_COMMAND_COUNT * TIMER_COMMAND_SECONDS * 1000.0)
- POLL_INTERVAL_MS);
bool execution_observed = false;
bool executing_reported = false;
bool paused_reported = false;
int completed_confirmation_count = 0;
while (clock::now() < deadline)
{
QueueSnapshot snapshot;
if (!read_queue_snapshot(fd, snapshot))
return false;
if (!snapshot.queue_mode_open)
{
std::cerr << "队列执行期间追加队列模式被关闭" << std::endl;
return false;
}
const bool executing = snapshot.robot_running_state == 2
|| snapshot.remaining_commands > 0;
if (executing)
{
execution_observed = true;
completed_confirmation_count = 0;
if (!executing_reported)
{
std::cout << "已检测到追加队列正在执行，剩余指令数："
<< snapshot.remaining_commands << std::endl;
executing_reported = true;
}
}
if (snapshot.robot_running_state == 1)
{
completed_confirmation_count = 0;
if (!paused_reported)
{
std::cout << "追加队列当前处于暂停状态" << std::endl;
paused_reported = true;
}
}
else
{
paused_reported = false;
}
if (snapshot.remaining_commands == 0
&& snapshot.robot_running_state == 0)
{
++completed_confirmation_count;
const bool execution_state_confirmed = execution_observed
|| completed_confirmation_count >= STOPPED_CONFIRMATION_COUNT;
const bool expected_duration_elapsed =
clock::now() >= earliest_completion;
if (execution_state_confirmed && expected_duration_elapsed)
{
std::cout << "已检测到追加队列执行完成" << std::endl;
return true;
}
}
std::this_thread::sleep_for(
std::chrono::milliseconds(POLL_INTERVAL_MS));
}
std::cerr << "等待追加队列执行完成超时" << std::endl;
return false;
}10、关闭队列模式和异常安全停止
正常完成后，end_append_queue 调用 queue_motion_set_status(fd, false) 关闭追加队列模式，并等待状态确认。关闭队列模式同样会清空队列，因此只能在确认执行完成或安全停止后调用。
cpp
bool end_append_queue(SOCKETFD fd)
{
if (!check_sdk_result(
queue_motion_set_status(fd, false),
"结束追加队列模式"))
{
return false;
}
return wait_for_queue_mode(fd, false);
}流程失败时，stop_queue_if_active 会先查询机器人状态。检测到运行或暂停时，调用 queue_motion_stop_not_power_off 执行不下电停止，并等待机器人进入状态 0；之后检查并关闭仍然开启的队列模式。
cpp
bool stop_queue_if_active(SOCKETFD fd)
{
int running_state = 0;
if (!query_running_state(fd, running_state))
return false;
if (running_state != 0)
{
std::cerr << "检测到队列仍在执行或暂停，正在执行不下电停止"
<< std::endl;
if (!check_sdk_result(
queue_motion_stop_not_power_off(fd),
"异常清理：停止追加队列"))
{
return false;
}
using clock = std::chrono::steady_clock;
const auto deadline = clock::now()
+ std::chrono::seconds(QUEUE_STATE_TIMEOUT_SECONDS);
while (clock::now() < deadline)
{
if (!query_running_state(fd, running_state))
return false;
if (running_state == 0)
break;
std::this_thread::sleep_for(
std::chrono::milliseconds(POLL_INTERVAL_MS));
}
if (running_state != 0)
{
std::cerr << "停止追加队列超时" << std::endl;
return false;
}
}
bool queue_mode_open = false;
if (!query_queue_mode(fd, queue_mode_open))
return false;
if (queue_mode_open)
return end_append_queue(fd);
return true;
}不下电停止只用于终止队列执行，不代表已经排除导致异常的根本原因。清理完成后仍需检查控制器状态和现场环境。
11、封装单机器人队列完整流程
run_demo 负责组织全部生命周期，并执行以下前置检查：
- 伺服状态必须为 3
- 机器人运行状态必须为 0
- 控制器模式必须有效并能够切换到远程模式
通过检查后，程序依次开启队列、追加指令、发送执行、等待完成并关闭队列。任一步骤失败时调用异常安全停止函数；结束前只有确认机器人停止，才会恢复控制器原始模式。
cpp
bool run_demo(SOCKETFD fd)
{
int servo_state = 0;
if (!query_servo_state(fd, servo_state))
{
return false;
}
if (servo_state != 3)
{
std::cerr << "伺服未处于运行状态，当前状态：" << servo_state << std::endl;
std::cerr << "请先完成上电流程" << std::endl;
return false;
}
int initial_running_state = 0;
if (!query_running_state(fd, initial_running_state))
{
return false;
}
if (initial_running_state != 0)
{
std::cerr << "机器人当前已有运动或暂停任务，状态码："
<< initial_running_state << std::endl;
std::cerr << "为避免清空或干扰现有队列，本示例停止执行" << std::endl;
return false;
}
int original_mode = -1;
if (!prepare_remote_mode(fd, original_mode))
{
return false;
}
bool success = start_append_queue(fd);
if (success)
{
success = append_relative_movej(fd); //调用MoveJ运动指令
}
if (success)
{
success = append_relative_moveL(fd); //调用MoveL运动指令
}
if (success)
{
success = append_timer_commands(fd); //调用延时指令
}
if (success)
{
success = execute_append_queue(fd);
}
if (success)
{
success = wait_for_queue_completion(fd);
}
if (success)
{
success = end_append_queue(fd);
}
if (!success)
{
stop_queue_if_active(fd);
}
int final_running_state = -1;
if (!query_running_state(fd, final_running_state))
{
success = false;
}
else if (final_running_state == 0 && original_mode != REMOTE_MODE)
{
if (!check_sdk_result(
set_current_mode(fd, original_mode),
"恢复控制器原始模式"))
{
success = false;
}
}
else if (final_running_state != 0)
{
std::cerr << "机器人状态未确认停止，未自动恢复控制器模式"
<< std::endl;
success = false;
}
}12、主程序连接并执行队列流程
主函数连接控制器后调用 run_demo，无论队列流程成功还是失败，最后都会调用 disconnect_robot 并检查断开结果。
cpp
/**
* @brief Demo 程序入口：连接控制器并执行单机器人追加队列示例。
* @return Demo 成功且连接正常断开时返回 0，否则返回 1。
*/
int main()
{
demo::enable_console_utf8();
const SOCKETFD fd = connect_robot(robot_ip, robot_port);
if (fd <= 0)
{
std::cerr << "控制器连接失败" << std::endl;
return 1;
}
std::cout << "控制器连接成功" << std::endl;
bool success = run_demo(fd);
if (!check_sdk_result(
disconnect_robot(fd),
"断开控制器连接"))
{
success = false;
}
else
{
std::cout << "控制器连接已断开" << std::endl;
}
return success ? 0 : 1;
}