纳博特科技开放平台

中文

English
한국어

中文

English
한국어

Appearance

9. 追加队列模式（双机器人） ​

本期将介绍如何通过一个 SDK 连接控制同一控制器下的两台机器人，为两台机器人分别准备追加队列，并在两组队列都准备完成后启动执行、联合监控状态和安全清理资源。

当前 Demo 为两台机器人分别追加延时指令和运动指令。两台机器人共用一个连接句柄，但所有机器人级接口都需要传入对应的机器人编号。

完整流程如下：

  1. 连接双机器人控制器并开启多机器人并行模式。
  2. 分别确认两台机器人空闲，记录各自原始模式并切换到运行模式。
  3. 分别确认伺服状态；需要上电时提示操作人员从示教器执行上电。
  4. 为两台机器人分别开启追加队列模式并追加指令。
  5. 两组队列全部准备完成后，按顺序快速发送两条执行命令。
  6. 在同一个轮询循环中分别监控两台机器人的队列状态，直到两组队列都完成。
  7. 分别关闭队列、恢复原始模式，最后关闭多机器人并行模式并断开连接。

⚠️ 安全警告

运行前必须确认：

  • 机器人编号： ROBOT_ONE_NUMBER 和 ROBOT_TWO_NUMBER 与控制器中的实际编号一致，且不能相同
  • 任务空闲： 两台机器人都没有其他运动、暂停或队列任务
  • 清空风险： 开启或关闭某台机器人的队列模式都会清空该机器人的已有队列
  • 人工上电： 多机器人并行模式下控制器不接受 SDK 上电命令，需要操作人员在示教器上确认并执行上电
  • 协同安全： 如果延时指令替换为 MOVJ/MOVL，两台机器人的目标点必须分别示教，并完成共享空间干涉和协同路径检查
  • 急停可用： 两台机器人的急停、安全回路和防护装置均处于正常状态

两条执行命令由同一线程按顺序快速发送，不是控制器层面的绝对同步启动。如果业务要求严格同步，需要使用控制器支持的同步机制重新设计流程。

1、配置控制器和机器人编号 ​

两台机器人共用控制器 IP、SDK 端口和连接句柄，但调用机器人级接口时必须传入正确编号。

cpp
/**
 * @name 用户必须根据运行环境修改
 * 以下配置决定 SDK 连接目标及两台机器人的控制器编号，运行前必须核对。
 * @{
 */
const std::string ROBOT_IP = "192.168.3.243";  ///< 双机器人控制器的实际 IP 地址。
const std::string ROBOT_PORT = "6001";         ///< 实际 SDK 服务端口，必须与控制器配置一致。
constexpr int ROBOT_ONE_NUMBER = 1;             ///< 第一台机器人的控制器编号。
constexpr int ROBOT_TWO_NUMBER = 2;             ///< 第二台机器人的控制器编号。
/** @} */

机器人编号配置错误可能导致指令发送给非预期机器人。正式运行前，应在控制器配置和示教器中逐一核对编号。

2、配置队列和状态等待参数 ​

每台机器人默认追加 3 条延时 2 秒的指令，单组队列预计执行约 6 秒。两组队列使用同一个总完成超时时间。

cpp
constexpr int TIMER_COMMAND_COUNT = 3;              ///< 每台机器人追加的延时指令数量。
constexpr double TIMER_COMMAND_SECONDS = 2.0;       ///< 每条延时指令的持续时间，单位为秒。
constexpr int QUEUE_COMPLETE_TIMEOUT_SECONDS = 30;  ///< 等待两组队列全部完成的总超时时间，单位为秒。

constexpr int SERVO_STATE_TIMEOUT_SECONDS = 90;     ///< 等待伺服状态切换及人工上电的总超时时间，单位为秒。
constexpr int QUEUE_STATE_TIMEOUT_SECONDS = 10;     ///< 等待队列模式切换或安全停止的总超时时间，单位为秒。

constexpr int POLL_INTERVAL_MS = 500;               ///< 伺服及队列状态轮询间隔，单位为毫秒。
constexpr int QUERY_RETRY_INTERVAL_MS = 800;        ///< 状态查询失败后的重试间隔，单位为毫秒。
constexpr int MAX_QUERY_FAILURES = 3;               ///< 状态查询允许的最大连续失败次数。
constexpr int STOPPED_CONFIRMATION_COUNT = 5;       ///< 未观察到执行状态时，判定完成所需的连续停止状态次数。

修改指令数量或持续时间后，需要同步确认完成超时和最早完成时间是否合理。人工上电时间较长时，还应调整伺服状态等待时间。

SDK 语义常量用于表示运行模式和伺服状态，通常不应修改：

cpp
constexpr int EXECUTION_MODE = 2;  ///< 队列执行所需的控制器运行模式。
constexpr int SERVO_STOPPED = 0;   ///< 伺服停止状态码。
constexpr int SERVO_READY = 1;     ///< 伺服就绪状态码。
constexpr int SERVO_ALARM = 2;     ///< 伺服报警状态码。
constexpr int SERVO_RUNNING = 3;   ///< 伺服运行状态码。

3、定义单台机器人的运行时状态 ​

双机器人流程需要分别保存两台机器人的编号、日志前缀、原始模式和队列执行进度。程序使用 RobotRuntime 保存每台机器人的独立状态。

cpp
/**
 * @brief 单台机器人在双队列流程中的运行时状态。
 */
struct RobotRuntime
{
    int number = 0;                       ///< 机器人在控制器中的编号。
    std::string log_prefix;               ///< 区分两台机器人输出信息的日志前缀。
    int original_mode = -1;               ///< 进入 Demo 前的控制器模式。
    bool queue_started = false;           ///< 是否已经开启追加队列模式。
    bool execution_observed = false;      ///< 是否观察到队列处于执行状态。
    bool executing_reported = false;      ///< 是否已经输出开始执行提示。
    bool paused_reported = false;         ///< 是否已经输出暂停提示。
    bool completed = false;               ///< 是否已经确认队列执行完成。
    int completed_confirmation_count = 0; ///< 连续检测到停止且无剩余指令的次数。
};

QueueSnapshot 保存单台机器人一次采样得到的队列模式、剩余指令数和运行状态。

cpp
struct QueueSnapshot
{
    bool queue_mode_open = false;
    int remaining_commands = 0;
    int robot_running_state = 0;
};

4、封装系统级和机器人级返回值检查 ​

双机器人程序既包含系统级接口，例如开启并行模式和断开连接；也包含机器人级接口，例如切换某台机器人的模式和控制其队列。

程序分别封装 check_system_result 和 check_sdk_result。机器人级错误信息带有 [机器人1] 或 [机器人2] 前缀，系统级错误使用 [系统] 前缀。

cpp
bool check_sdk_result(
    Result result,
    const RobotRuntime& robot,
    const std::string& operation)
{
    if (result == SUCCESS)
        return true;

    std::cerr << robot.log_prefix << operation << "失败，错误码："
              << static_cast<int>(result) << std::endl;
    return false;
}

bool check_system_result(Result result, const std::string& operation)
{
    if (result == SUCCESS)
        return true;

    std::cerr << "[系统] " << operation << "失败，错误码："
              << static_cast<int>(result) << std::endl;
    return false;
}

5、封装机器人级查询重试和状态快照 ​

query_with_retry 接收目标机器人的运行时信息，在查询失败时输出对应机器人前缀。连接断开时立即停止重试，其他临时错误按配置等待后再次查询。

cpp
template <typename Query>
bool query_with_retry(
    const RobotRuntime& robot,
    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 << robot.log_prefix << operation << "失败，第 "
                  << attempt << "/" << MAX_QUERY_FAILURES
                  << " 次，错误码：" << static_cast<int>(result) << std::endl;

        if (result == DISCONNECT)
        {
            std::cerr << robot.log_prefix
                      << "控制器连接已经断开，停止重试" << std::endl;
            return false;
        }

        if (attempt < MAX_QUERY_FAILURES)
        {
            std::this_thread::sleep_for(
                std::chrono::milliseconds(QUERY_RETRY_INTERVAL_MS));
        }
    }

    std::cerr << robot.log_prefix << operation
              << "连续失败，停止当前流程" << std::endl;
    return false;
}

各查询包装函数调用带 _robot 后缀的接口，并传入 robot.number：

cpp
bool query_servo_state(SOCKETFD fd, const RobotRuntime& robot, int& state)
{
    return query_with_retry(robot, "获取伺服状态", [&]() {
        return get_servo_state_robot(fd, robot.number, state);
    });
}

bool query_running_state(SOCKETFD fd, const RobotRuntime& robot, int& state)
{
    return query_with_retry(robot, "获取运行状态", [&]() {
        return get_robot_running_state_robot(fd, robot.number, state);
    });
}

bool query_current_mode(SOCKETFD fd, const RobotRuntime& robot, int& mode)
{
    return query_with_retry(robot, "获取当前模式", [&]() {
        return get_current_mode_robot(fd, robot.number, mode);
    });
}

bool query_queue_mode(SOCKETFD fd, const RobotRuntime& robot, bool& status)
{
    return query_with_retry(robot, "获取追加队列模式状态", [&]() {
        return queue_motion_get_status_robot(fd, robot.number, status);
    });
}

bool query_queue_remaining(
    SOCKETFD fd,
    const RobotRuntime& robot,
    int& remaining_commands)
{
    return query_with_retry(robot, "获取剩余队列长度", [&]() {
        return queue_motion_get_queuelen_robot(
            fd, robot.number, remaining_commands);
    });
}

read_queue_snapshot 将三项队列状态读入一个快照，并验证剩余数量和运行状态值。单独封装后，完成判断函数只处理已经校验的数据。

cpp
bool read_queue_snapshot(
    SOCKETFD fd,
    const RobotRuntime& robot,
    QueueSnapshot& snapshot)
{
    if (!query_queue_mode(fd, robot, snapshot.queue_mode_open))
        return false;
    if (!query_queue_remaining(fd, robot, snapshot.remaining_commands))
        return false;
    if (!query_running_state(fd, robot, snapshot.robot_running_state))
        return false;

    if (snapshot.remaining_commands < 0)
    {
        std::cerr << robot.log_prefix << "控制器返回非法剩余队列长度："
                  << snapshot.remaining_commands << std::endl;
        return false;
    }
    if (snapshot.robot_running_state < 0
        || snapshot.robot_running_state > 2)
    {
        std::cerr << robot.log_prefix << "控制器返回未知运行状态："
                  << snapshot.robot_running_state << std::endl;
        return false;
    }

    return true;
}

6、等待伺服状态并准备单台机器人 ​

多机器人并行模式或模式切换可能触发安全下电。程序在最终运行模式确定后，分别确认两台机器人的伺服状态。

wait_for_servo_state 持续查询指定机器人的伺服状态，直到进入期望状态、检测到报警或等待超时。

cpp
bool wait_for_servo_state(
    SOCKETFD fd,
    const RobotRuntime& robot,
    int expected_state,
    const std::string& expected_state_name)
{
    using clock = std::chrono::steady_clock;
    const auto deadline = clock::now()
        + std::chrono::seconds(SERVO_STATE_TIMEOUT_SECONDS);

    while (clock::now() < deadline)
    {
        int servo_state = -1;
        if (!query_servo_state(fd, robot, servo_state))
            return false;

        if (servo_state == expected_state)
        {
            std::cout << robot.log_prefix << "伺服已进入"
                      << expected_state_name << std::endl;
            return true;
        }

        if (servo_state == SERVO_ALARM)
        {
            std::cerr << robot.log_prefix
                      << "伺服在状态切换过程中进入报警状态，请检查示教器报警"
                      << std::endl;
            return false;
        }

        std::this_thread::sleep_for(
            std::chrono::milliseconds(POLL_INTERVAL_MS));
    }

    std::cerr << robot.log_prefix << "等待伺服进入"
              << expected_state_name << "超时" << std::endl;
    return false;
}

prepare_robot 对指定机器人执行以下操作：

  1. 确认当前运行状态为 0，没有运动或暂停任务。
  2. 保存原始模式，并在需要时切换到运行模式 2。
  3. 查询伺服状态；状态 0 时先切换为就绪，状态 1 时直接等待人工上电。
  4. 状态 2 时停止流程，不自动清错；状态 3 时直接确认完成。
cpp
bool prepare_robot(SOCKETFD fd, RobotRuntime& robot)
{
    int running_state = 0;
    if (!query_running_state(fd, robot, running_state))
        return false;
    if (running_state != 0)
    {
        std::cerr << robot.log_prefix
                  << "当前已有运动或暂停任务，状态码："
                  << running_state << std::endl;
        return false;
    }

    if (!query_current_mode(fd, robot, robot.original_mode))
        return false;
    if (robot.original_mode < 0 || robot.original_mode > 2)
    {
        std::cerr << robot.log_prefix << "控制器返回未知模式："
                  << robot.original_mode << std::endl;
        return false;
    }

    if (robot.original_mode != EXECUTION_MODE)
    {
        if (!check_sdk_result(
                set_current_mode_robot(fd, robot.number, EXECUTION_MODE),
                robot,
                "切换到运行模式"))
            return false;

        int confirmed_mode = -1;
        if (!query_current_mode(fd, robot, confirmed_mode))
            return false;
        if (confirmed_mode != EXECUTION_MODE)
        {
            std::cerr << robot.log_prefix << "运行模式确认失败，当前模式："
                      << confirmed_mode << std::endl;
            return false;
        }
    }

    int servo_state = SERVO_STOPPED;
    if (!query_servo_state(fd, robot, servo_state))
        return false;

    bool needs_power_on = false;
    switch (servo_state)
    {
    case SERVO_STOPPED:
        if (!check_sdk_result(
                set_servo_state_robot(fd, robot.number, SERVO_READY),
                robot,
                "设置伺服为就绪状态"))
            return false;
        if (!wait_for_servo_state(fd, robot, SERVO_READY, "就绪状态"))
            return false;
        needs_power_on = true;
        break;

    case SERVO_READY:
        std::cout << robot.log_prefix << "伺服已经处于就绪状态" << std::endl;
        needs_power_on = true;
        break;

    case SERVO_RUNNING:
        std::cout << robot.log_prefix << "伺服已经处于运行状态" << std::endl;
        break;

    case SERVO_ALARM:
        std::cerr << robot.log_prefix
                  << "伺服处于报警状态，请先排除报警，不自动清错"
                  << std::endl;
        return false;

    default:
        std::cerr << robot.log_prefix << "控制器返回未知伺服状态："
                  << servo_state << std::endl;
        return false;
    }

    if (needs_power_on)
    {
        std::cout << robot.log_prefix
                  << "多机器人并行模式下控制器不接受 SDK 上电命令；"
                  << "请现在从示教器执行上电，程序最多等待 "
                  << SERVO_STATE_TIMEOUT_SECONDS << " 秒" << std::endl;

        if (!wait_for_servo_state(fd, robot, SERVO_RUNNING, "运行状态"))
            return false;
    }

    std::cout << robot.log_prefix
              << "准备完成：伺服运行、机器人空闲、运行模式" << std::endl;
    return true;
}

7、分别准备两组追加队列 ​

wait_for_queue_mode、start_queue、append_timer_commands 和 execute_queue 都接收 RobotRuntime，通过其中的编号调用对应机器人接口。

开启或关闭队列模式会清空对应机器人的队列。start_queue 在开启接口成功后将 queue_started 设为 true，便于异常清理阶段判断哪些队列需要关闭。

cpp
bool start_queue(SOCKETFD fd, RobotRuntime& robot)
{
    if (!check_sdk_result(
            queue_motion_set_status_robot(fd, robot.number, true),
            robot,
            "开始追加队列模式"))
    {
        return false;
    }

    robot.queue_started = true;
    return wait_for_queue_mode(fd, robot, true);
}

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, const RobotRuntime& robot)
{
    for (int index = 0; index < TIMER_COMMAND_COUNT; ++index)
    {
        if (!check_sdk_result(
                queue_motion_push_back_timer_robot(
                    fd, robot.number, TIMER_COMMAND_SECONDS),
                robot,
                "追加第 " + std::to_string(index + 1) + " 条延时指令"))
        {
            return false;
        }
    }
    std::cout << "正在追加延时指令" << std::endl;
    return true;
}

bool execute_queue(SOCKETFD fd, const RobotRuntime& robot)
{
    if (!check_sdk_result(
            queue_motion_send_to_controller_robot(
                fd, robot.number, 0, false),
            robot,
            "发送并执行追加队列"))
    {
        return false;
    }

    std::cout << robot.log_prefix << "追加队列已经发送并开始执行"
              << std::endl;
    return true;
}

程序必须先为两台机器人都完成队列开启和指令追加，再进入执行阶段。这样可以减少第一台机器人已经开始执行、第二台机器人却仍在准备队列的时间差。

8、分别更新状态并等待两组队列完成 ​

update_completion_state 只处理一台机器人的一次状态更新，并将执行观察、暂停提示、完成确认次数和最终完成结果写回该机器人的 RobotRuntime。

cpp
bool update_completion_state(
    SOCKETFD fd,
    RobotRuntime& robot,
    std::chrono::steady_clock::time_point earliest_completion)
{
    QueueSnapshot snapshot;
    if (!read_queue_snapshot(fd, robot, snapshot))
        return false;

    if (!snapshot.queue_mode_open)
    {
        std::cerr << robot.log_prefix
                  << "执行期间追加队列模式被关闭" << std::endl;
        return false;
    }

    const bool executing = snapshot.robot_running_state == 2
        || snapshot.remaining_commands > 0;
    if (executing)
    {
        robot.execution_observed = true;
        robot.completed_confirmation_count = 0;
        if (!robot.executing_reported)
        {
            std::cout << robot.log_prefix
                      << "已检测到队列正在执行，剩余指令数："
                      << snapshot.remaining_commands << std::endl;
            robot.executing_reported = true;
        }
    }

    if (snapshot.robot_running_state == 1)
    {
        robot.completed_confirmation_count = 0;
        if (!robot.paused_reported)
        {
            std::cout << robot.log_prefix << "追加队列当前处于暂停状态"
                      << std::endl;
            robot.paused_reported = true;
        }
    }
    else
    {
        robot.paused_reported = false;
    }

    if (snapshot.remaining_commands == 0
        && snapshot.robot_running_state == 0)
    {
        ++robot.completed_confirmation_count;
        const bool execution_state_confirmed = robot.execution_observed
            || robot.completed_confirmation_count >= STOPPED_CONFIRMATION_COUNT;
        const bool expected_duration_elapsed =
            std::chrono::steady_clock::now() >= earliest_completion;

        if (execution_state_confirmed && expected_duration_elapsed)
        {
            robot.completed = true;
            std::cout << robot.log_prefix << "已检测到追加队列执行完成"
                      << std::endl;
        }
    }

    return true;
}

wait_for_both_queues 在同一个循环中遍历两台机器人，只更新尚未完成的队列。两台机器人的 completed 都变为 true 后才返回成功。

cpp
bool wait_for_both_queues(
    SOCKETFD fd,
    std::array<RobotRuntime, 2>& robots,
    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);

    while (clock::now() < deadline)
    {
        bool all_completed = true;
        for (RobotRuntime& robot : robots)
        {
            if (!robot.completed)
            {
                all_completed = false;
                if (!update_completion_state(fd, robot, earliest_completion))
                    return false;
            }
        }

        if (all_completed || (robots[0].completed && robots[1].completed))
        {
            std::cout << "[系统] 两台机器人的追加队列均已执行完成"
                      << std::endl;
            return true;
        }

        std::this_thread::sleep_for(
            std::chrono::milliseconds(POLL_INTERVAL_MS));
    }

    for (const RobotRuntime& robot : robots)
    {
        if (!robot.completed)
        {
            std::cerr << robot.log_prefix << "等待追加队列执行完成超时"
                      << std::endl;
        }
    }
    return false;
}

9、分别关闭队列、异常停止和恢复模式 ​

end_queue 只关闭已经成功开启的队列，并在关闭成功后清除 queue_started 标志。关闭队列模式会清空对应机器人的队列，只能在完成或安全停止后执行。

cpp
bool end_queue(SOCKETFD fd, RobotRuntime& robot)
{
    if (!robot.queue_started)
        return true;

    if (!check_sdk_result(
            queue_motion_set_status_robot(fd, robot.number, false),
            robot,
            "结束追加队列模式"))
        return false;

    if (!wait_for_queue_mode(fd, robot, false))
        return false;

    robot.queue_started = false;
    return true;
}

异常时，stop_and_close_queue 针对每台机器人分别调用不下电停止接口，等待状态 0 后再关闭该机器人的队列。

cpp
bool stop_and_close_queue(SOCKETFD fd, RobotRuntime& robot)
{
    int running_state = 0;
    if (!query_running_state(fd, robot, running_state))
        return false;

    if (running_state != 0)
    {
        std::cerr << robot.log_prefix
                  << "队列仍在执行或暂停，正在执行不下电停止" << std::endl;
        if (!check_sdk_result(
                queue_motion_stop_not_power_off_robot(fd, robot.number),
                robot,
                "异常清理：停止追加队列"))
            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, robot, 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 << robot.log_prefix << "停止追加队列超时" << std::endl;
            return false;
        }
    }

    return end_queue(fd, robot);
}

restore_robot_mode 只有在机器人确认停止时才恢复原模式。单独封装后，可以为两台机器人分别判断是否需要恢复，避免一台机器人仍在运行时切换其控制器模式。

cpp
bool restore_robot_mode(SOCKETFD fd, const RobotRuntime& robot)
{
    if (robot.original_mode < 0 || robot.original_mode == EXECUTION_MODE)
        return true;

    int running_state = -1;
    if (!query_running_state(fd, robot, running_state))
        return false;
    if (running_state != 0)
    {
        std::cerr << robot.log_prefix
                  << "状态未确认停止，不恢复原始模式" << std::endl;
        return false;
    }

    return check_sdk_result(
        set_current_mode_robot(fd, robot.number, robot.original_mode),
        robot,
        "恢复控制器原始模式");
}

10、封装机器人检查函数 ​

为了确保当前的追加队列模式下，机器人的数量确定是两台，因此在本节函数中增加了两层机器人数量检查环节，依次检测依次验证机器人类型、7 个关节坐标及运行状态。

cpp
/**
 * @brief 检查指定机器人是否能够返回有效的机器人本体数据
 * @param[in] fd 控制器连接句柄
 * @param[in] robot 待检查机器人的运行时信息
 * @retval true 机器人类型、关节坐标和运行状态均连续三次查询有效
 * @retval false 任一查询失败，或返回的数据不符合预期范围
 *
 * @details 每轮检测依次验证机器人类型、7 个关节坐标及运行状态，避免仅凭单个接口判断机器人存在。
 * @note 这是只读检测，不改变控制器模式、伺服状态或队列状态。
 */
bool probe_robot_instance(
	SOCKETFD fd,
	const RobotRuntime& robot)
{
	/*
	 * 连续检测3次，避免某些接口第一次返回缓存值或瞬时默认值。
	 */
	constexpr int PROBE_COUNT = 3;

	for (int attempt = 1; attempt <= PROBE_COUNT; ++attempt)
	{
		/*
		 * 1. 获取机器人类型
		 */
		int robot_type = -1;

		const Result type_result =
			get_robot_type_robot(
				fd,
				robot.number,
				robot_type);

		if (type_result != SUCCESS)
		{
			std::cerr << robot.log_prefix
				<< "机器人类型查询失败，错误码："
				<< static_cast<int>(type_result)
				<< std::endl;
			return false;
		}

		/*
		 * 根据当前SDK头文件，机器人类型有效范围为1~14。
		 */
		if (robot_type < 1 || robot_type > 14)
		{
			std::cerr << robot.log_prefix
				<< "返回了非法机器人类型："
				<< robot_type << std::endl;
			return false;
		}

		/*
		 * 2. 获取当前关节坐标
		 */
		std::vector<double> joint_position;

		const Result position_result =
			get_current_position_robot(
				fd,
				robot.number,
				0,
				joint_position);

		if (position_result != SUCCESS)
		{
			std::cerr << robot.log_prefix
				<< "关节坐标查询失败，错误码："
				<< static_cast<int>(position_result)
				<< std::endl;
			return false;
		}

		if (joint_position.size() != 7)
		{
			std::cerr << robot.log_prefix
				<< "关节坐标长度异常，期望7，实际为："
				<< joint_position.size() << std::endl;
			return false;
		}

		for (double value : joint_position)
		{
			if (!std::isfinite(value))
			{
				std::cerr << robot.log_prefix
					<< "关节坐标中包含无效数值"
					<< std::endl;
				return false;
			}
		}

		/*
		 * 3. 获取运行状态
		 */
		int running_state = -1;

		const Result state_result =
			get_robot_running_state_robot(
				fd,
				robot.number,
				running_state);

		if (state_result != SUCCESS)
		{
			std::cerr << robot.log_prefix
				<< "运行状态查询失败，错误码："
				<< static_cast<int>(state_result)
				<< std::endl;
			return false;
		}

		if (running_state < 0 || running_state > 2)
		{
			std::cerr << robot.log_prefix
				<< "返回了非法运行状态："
				<< running_state << std::endl;
			return false;
		}

		std::this_thread::sleep_for(
			std::chrono::milliseconds(200));
	}

	std::cout << robot.log_prefix
		<< "机器人检测通过"
		<< std::endl;

	return true;
}
cpp
/**
 * @brief 检查程序配置的两台机器人是否均能返回有效本体数据
 * @param[in] fd 控制器连接句柄
 * @param[in] robots 程序配置的两台机器人
 * @retval true 两台机器人的编号有效、互不相同且均通过本体数据检测
 * @retval false 编号配置错误，或任意一台机器人无法访问
 *
 * @note 调用本函数前必须已经开启多机器人并行模式，否则无法访问机器人 2。
 */
bool verify_two_robots_available(
	SOCKETFD fd,
	const std::array<RobotRuntime, 2>& robots)
{
	if (robots[0].number <= 0 ||
		robots[1].number <= 0)
	{
		std::cerr << "[系统] 机器人编号必须大于0"
			<< std::endl;
		return false;
	}

	if (robots[0].number == robots[1].number)
	{
		std::cerr << "[系统] 两台机器人编号相同："
			<< robots[0].number << std::endl;
		return false;
	}

	for (const RobotRuntime& robot : robots)
	{
		if (!probe_robot_instance(fd, robot))
		{
			std::cerr << robot.log_prefix
				<< "未检测到有效机器人"
				<< std::endl;
			return false;
		}
	}

	std::cout << "[系统] 两台机器人均检测有效"
		<< std::endl;

	return true;
}
cpp
/**
 * @brief 检查两台机器人是否都具有独立且可用的追加队列通道
 * @param[in] fd 控制器连接句柄
 * @param[in,out] robots 程序配置的两台机器人；预检结束时会关闭已开启的队列并清除状态标志
 * @retval true 两台机器人的队列均可开启、确认并正常关闭
 * @retval false 任一机器人的队列通道不可用，或预检清理失败
 *
 * @warning 开启和关闭队列模式会清空队列。
 *          本函数必须在正式追加指令之前调用。
 */
bool verify_dual_queue_channels(
	SOCKETFD fd,
	std::array<RobotRuntime, 2>& robots)
{
	bool success = true;

	/*
	 * 依次开启两台机器人的空队列。
	 */
	for (RobotRuntime& robot : robots)
	{
		std::cout << robot.log_prefix
			<< "正在预检追加队列通道"
			<< std::endl;

		if (!start_queue(fd, robot))
		{
			std::cerr << robot.log_prefix
				<< "追加队列通道预检失败"
				<< std::endl;
			success = false;
			break;
		}
	}

	/*
	 * 无论是否全部成功，都关闭已经尝试开启的队列。
	 */
	for (RobotRuntime& robot : robots)
	{
		if (robot.queue_started)
		{
			if (!end_queue(fd, robot))
			{
				std::cerr << robot.log_prefix
					<< "预检后关闭队列失败"
					<< std::endl;
				success = false;
			}
		}
	}

	if (!success)
	{
		std::cerr << "[系统] 双机器人队列通道不可用，"
			<< "禁止继续追加或执行运动指令"
			<< std::endl;
		return false;
	}

	std::cout << "[系统] 两台机器人的追加队列通道预检通过"
		<< std::endl;

	return true;
}

11、封装双机器人队列完整流程 ​

run_demo 首先创建两份 RobotRuntime，并为日志设置不同前缀。随后执行系统级并行模式开启、检测是否存在两台机器人、两台机器人准备、两组队列准备、快速发送执行、联合等待和清理恢复。

cpp
/**
 * @brief 执行双机器人追加队列的完整生命周期
 * @param[in] fd 两台机器人共用的控制器连接句柄
 * @retval true 两台机器人均完成准备、队列执行和清理，且并行模式正常关闭
 * @retval false 任一系统、机器人、队列或清理操作失败
 *
 * @details
 * 流程先开启多机器人并行模式，再依次准备两台机器人和两组队列；队列执行命令按顺序快速发送，
 * 随后在同一轮询循环中监控两台机器人的完成状态，最后恢复模式并关闭并行模式
 */
bool run_demo(SOCKETFD fd)
{
	std::array<RobotRuntime, 2> robots = {
		RobotRuntime{ ROBOT_ONE_NUMBER, "[机器人1] " },
		RobotRuntime{ ROBOT_TWO_NUMBER, "[机器人2] " }
	};

	bool success = true;
	bool parallel_enabled = false;

	// 必须先开启并行模式，控制器才允许访问机器人 2
	parallel_enabled = check_system_result(
		set_robots_parallel(fd, true),
		"开启多机器人并行模式");

	success = parallel_enabled;

	if (success)
	{
		std::cout << "[系统] 多机器人并行模式已开启" << std::endl;
	}

	/*
 * 第一层检测：读取两台机器人的本体数据。
 */
	if (success &&
		!verify_two_robots_available(fd, robots))
	{
		std::cerr << "[系统] 未检测到两个有效机器人，程序退出"
			<< std::endl;

		check_system_result(
			set_robots_parallel(fd, false),
			"关闭多机器人并行模式");

		return false;
	}



	// 并行模式开启后，再分别查询和准备两台机器人
	if (success)
	{
		for (RobotRuntime& robot : robots)
		{
			if (!prepare_robot(fd, robot))
			{
				success = false;
				break;
			}
		}
	}

	/*
 * 第二层检测：验证两台机器人是否都有可用的独立队列通道。
 * 此时尚未追加MoveJ、MoveL或Timer，因此可以安全清空空队列。
 */
	if (success &&
		!verify_dual_queue_channels(fd, robots))
	{
		success = false;
	}

	// 必须先为两台机器人完成队列准备，再发送执行命令
	for (RobotRuntime& robot : robots)
	{
		if (success && !start_queue(fd, robot))
		{
			success = false;
		}
		if (success && !append_relative_movej(fd, robot))
		{
			success = false;
		}
		if (success && !append_relative_moveL(fd, robot))
		{
			success = false;
		}
		if (success && !append_timer_commands(fd, robot))
		{
			success = false;
		}
	}

	if (success)
	{
		std::cout << "[系统] 两台机器人队列准备完成，开始并行执行"
			<< std::endl;
		for (const RobotRuntime& robot : robots)
		{
			if (!execute_queue(fd, robot))
			{
				success = false;
				break;
			}
		}
	}

	if (success)
	{
		success = wait_for_both_queues(fd, robots);
	}

	if (success)
	{
		for (RobotRuntime& robot : robots)
		{
			if (!end_queue(fd, robot))
			{
				success = false;
			}
		}
	}

	if (!success)
	{
		for (RobotRuntime& robot : robots)
		{
			stop_and_close_queue(fd, robot);
		}
	}

	for (const RobotRuntime& robot : robots)
	{
		if (!restore_robot_mode(fd, robot))
		{
			success = false;
		}
	}

	if (parallel_enabled)
	{
		if (!check_system_result(
			set_robots_parallel(fd, false),
			"关闭多机器人并行模式"))
		{
			success = false;
		}
		else
		{
			std::cout << "[系统] 多机器人并行模式已关闭" << std::endl;
		}
	}

	if (success)
	{
		std::cout << "[系统] 追加队列（双机器人）Demo执行成功"
			<< std::endl;
	}
	return success;
}

并行模式必须在准备机器人 2 之前开启。关闭并行模式则应放在两台机器人的队列清理和模式恢复之后，避免提前失去对机器人 2 的访问能力。

12、主程序连接并执行双机器人流程 ​

主函数只建立一次控制器连接。两台机器人所有状态查询和队列操作都通过同一个 fd 完成，并使用机器人编号进行区分。

cpp
#include <array>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <cpp_interface/nrc_interface.h>
#include <cpp_interface/nrc_queue_operate.h>
#include <vector>
#include <cmath>
#include "../demo_utils.h"
/**
 * @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_system_result(disconnect_robot(fd), "断开控制器连接"))
    {
        success = false;
    }
    else
    {
        std::cout << "[系统] 控制器连接已断开" << std::endl;
    }

    return success ? 0 : 1;
}