队列运动接口

头文件：include/tl_queue_operate.h
相关示例代码：examples\cpp\ex_queue_motion.cpp

1. queue_motion_set_status - 打开、关闭队列运动模式

方法：Result queue_motion_set_status(SOCKETFD socketFd, bool status);

功能描述：打开或关闭控制器的队列运动模式。无论打开还是关闭，都会清空远端控制器已存储的队列。打开队列模式（status=true）时控制器会先执行伺服下电再上电；关闭队列模式（status=false）退出后控制器会执行伺服下电，后续如需继续运动需重新执行上电流程。

参数：

socketFd：已连接的句柄。
status：true 打开 / false 关闭。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 打开队列运动模式
    auto ret = tl::queue_motion_set_status(socket_fd, true);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 打开队列运动模式失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 队列运动模式已打开" << std::endl;

2. queue_motion_push_back_moveJ - 队列运动模式的本地队列最后插入一条 move J 运动

方法：Result queue_motion_push_back_moveJ(SOCKETFD socketFd, MoveCmd moveCmd);

功能描述：在队列运动模式的本地队列末尾插入一条 MoveJ（关节空间运动）指令。

参数：

socketFd：已连接的句柄。
moveCmd：运动命令结构体，需设置以下成员：
- coord：坐标系，Coord 枚举（JOINT / BASE / TOOL / USER）
- targetPosValue：目标点坐标，长度 7
- velocity：速度，范围 (1, 100]，单位度/s
- acc：加速度，范围 (1, 100]
- dec：减速度，范围 (1, 100]
- pl：多条轨迹之间平滑参数，范围 [0, 5]

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 插入一条 MoveJ 指令
    tl::MoveCmd cmd;
    cmd.coord        = tl::Coord::JOINT;   // 关节坐标系
    cmd.velocity     = 30;                 // 度/s
    cmd.acc          = 50;
    cmd.dec          = 50;
    cmd.pl           = 0;                  // 平滑参数
    cmd.targetPosValue.resize(14, 0.0);
    cmd.targetPosValue[0] = 30;            // J1 = 30°
    cmd.targetPosValue[1] = 20;            // J2 = 20°

    auto ret = tl::queue_motion_push_back_moveJ(socket_fd, cmd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 插入 MoveJ 指令失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 插入 MoveJ 指令成功" << std::endl;

3. queue_motion_push_back_moveL - 队列运动模式的本地队列最后插入一条 move L 运动

方法：Result queue_motion_push_back_moveL(SOCKETFD socketFd, MoveCmd moveCmd);

功能描述：在队列运动模式的本地队列末尾插入一条 MoveL（笛卡尔直线运动）指令。

参数：

socketFd：已连接的句柄。
moveCmd：运动命令结构体，成员同 2.，velocity 范围 (1, 1000]，单位 mm/s。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 插入一条 MoveL 指令
    tl::MoveCmd cmd;
    cmd.coord        = tl::Coord::BASE;    // 基坐标系
    cmd.velocity     = 100;                // mm/s
    cmd.acc          = 50;
    cmd.dec          = 50;
    cmd.pl           = 0;
    cmd.targetPosValue.resize(14, 0.0);
    cmd.targetPosValue[0] = 300;           // X (mm)
    cmd.targetPosValue[1] = 0;             // Y (mm)
    cmd.targetPosValue[2] = 300;           // Z (mm)
    cmd.targetPosValue[3] = 3.141;         // Rx (rad)
    cmd.targetPosValue[4] = 0;             // Ry (rad)
    cmd.targetPosValue[5] = 0;             // Rz (rad)

    auto ret = tl::queue_motion_push_back_moveL(socket_fd, cmd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 插入 MoveL 指令失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 插入 MoveL 指令成功" << std::endl;

4. queue_motion_push_back_moveC - 队列运动本地队列末尾插入一条 MoveC 运动

方法：Result queue_motion_push_back_moveC(SOCKETFD socketFd, MoveCmd moveCmd);

功能描述：在队列运动模式的本地队列末尾插入一条 MoveC（圆弧运动）指令。

参数：

socketFd：已连接的句柄。
moveCmd：运动命令结构体，成员同 2.，velocity 范围 (1, 1000]，单位 mm/s。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 插入一条 MoveC 指令
    tl::MoveCmd cmd;
    cmd.coord        = tl::Coord::BASE;    // 基坐标系
    cmd.velocity     = 100;                // mm/s
    cmd.acc          = 50;
    cmd.dec          = 50;
    cmd.pl           = 0;
    cmd.targetPosValue.resize(14, 0.0);
    cmd.targetPosValue[0] = 280;           // X (mm)
    cmd.targetPosValue[1] = 0;             // Y (mm)
    cmd.targetPosValue[2] = 380;           // Z (mm)
    cmd.targetPosValue[3] = 3.141;         // Rx (rad)
    cmd.targetPosValue[4] = 0;
    cmd.targetPosValue[5] = 0;

    auto ret = tl::queue_motion_push_back_moveC(socket_fd, cmd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 插入 MoveC 指令失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 插入 MoveC 指令成功" << std::endl;

5. queue_motion_push_back_moveS - 队列运动本地队列末尾插入一条 MoveS 运动

方法：Result queue_motion_push_back_moveS(SOCKETFD socketFd, MoveCmd moveCmd);

功能描述：在队列运动模式的本地队列末尾插入一条 MoveS（样条运动）指令。

参数：

socketFd：已连接的句柄。
moveCmd：运动命令结构体，成员同 2.，velocity 范围 (1, 1000]，单位 mm/s。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 插入一条 MoveS 指令
    tl::MoveCmd cmd;
    cmd.coord        = tl::Coord::BASE;    // 基坐标系
    cmd.velocity     = 100;                // mm/s
    cmd.acc          = 50;
    cmd.dec          = 50;
    cmd.pl           = 0;
    cmd.targetPosValue.resize(14, 0.0);
    cmd.targetPosValue[0] = 300;           // X (mm)
    cmd.targetPosValue[1] = 100;           // Y (mm)
    cmd.targetPosValue[2] = 300;           // Z (mm)
    cmd.targetPosValue[3] = 3.141;         // Rx (rad)

    auto ret = tl::queue_motion_push_back_moveS(socket_fd, cmd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 插入 MoveS 指令失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 插入 MoveS 指令成功" << std::endl;

6. queue_motion_send_to_controller - 将本地队列的前 size 个数据发送到控制器

方法：Result queue_motion_send_to_controller(SOCKETFD socketFd, int size, bool isContinue = false);

功能描述：将本地队列的前 size 个数据发送到控制器。控制器接收到队列后会立即开始运动，调用前请先调用 queue_motion_set_status(true)。

参数：

socketFd：已连接的句柄。
size：要发送的队列大小，范围 [0, 31]；size = 0 时将当前运动队列全部指令发送。
isContinue：是否继续发送，true 继续发送（可继续发送点位，机器人不运动）；false 不继续（机器人接收点位后立刻运动），默认 false。

返回值：

0：成功。
非0：失败（如传入的 size 超过队列长度）。

使用示例：

C++

    // 将本地队列全部指令发送到控制器
    auto ret = tl::queue_motion_send_to_controller(socket_fd, 0, false);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 发送队列到控制器失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 发送队列到控制器成功" << std::endl;

7. queue_motion_get_status - 查询队列模式状态

方法：Result queue_motion_get_status(SOCKETFD socketFd, bool& status);

功能描述：查询控制器当前是否已打开队列运动模式。

参数：

socketFd：已连接的句柄。
status：输出，true 已打开 / false 关闭。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 查询队列模式状态
    bool status = false;
    auto ret = tl::queue_motion_get_status(socket_fd, status);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 查询队列模式状态失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 队列模式状态: " << (status ? "已打开" : "已关闭") << std::endl;

8. queue_motion_size - 查询当前运动队列的长度

方法：Result queue_motion_size(SOCKETFD socketFd, int& size);

功能描述：查询当前本地运动队列的长度。

参数：

socketFd：已连接的句柄。
size：输出，当前运动队列长度。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 查询本地队列长度
    int size = 0;
    auto ret = tl::queue_motion_size(socket_fd, size);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 查询队列长度失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 本地队列长度: " << size << " 条指令" << std::endl;

9. queue_motion_get_queuelen - 查询当前运动队列剩余的指令数量

方法：Result queue_motion_get_queuelen(SOCKETFD socketFd, int& len);

功能描述：查询控制器当前运动队列剩余的指令数量。

参数：

socketFd：已连接的句柄。
len：输出，当前运动队列剩余的指令数量。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 查询剩余队列指令数
    int len = 0;
    auto ret = tl::queue_motion_get_queuelen(socket_fd, len);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 查询剩余队列指令数失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 剩余队列指令数: " << len << std::endl;

10. queue_motion_suspend - 暂停连续运动（轨迹暂停）

方法：Result queue_motion_suspend(SOCKETFD socketFd);

功能描述：暂停连续运动（轨迹暂停）。

参数：

socketFd：已连接的句柄。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 暂停连续运动
    auto ret = tl::queue_motion_suspend(socket_fd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 暂停连续运动失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 已暂停连续运动" << std::endl;

11. queue_motion_restart - 暂停后再次运行运动队列（继续当前轨迹运动）

方法：Result queue_motion_restart(SOCKETFD socketFd);

功能描述：暂停后再次运行运动队列（继续当前轨迹运动）。

参数：

socketFd：已连接的句柄。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 恢复连续运动
    auto ret = tl::queue_motion_restart(socket_fd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 恢复连续运动失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 已恢复连续运动" << std::endl;

12. queue_motion_stop - 停止队列运动

方法：Result queue_motion_stop(SOCKETFD socketFd);

功能描述：停止连续运动。该接口已废弃，建议使用 queue_motion_stop_not_power_off 替代。

参数：

socketFd：已连接的句柄。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 停止队列运动（已废弃，推荐使用 queue_motion_stop_not_power_off）
    auto ret = tl::queue_motion_stop(socket_fd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 停止队列运动失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 已停止队列运动" << std::endl;

13. queue_motion_stop_not_power_off - 停止连续运动保持上电状态（轨迹缓停）

方法：Result queue_motion_stop_not_power_off(SOCKETFD socketFd);

功能描述：停止连续运动并保持上电状态（轨迹缓停）。

参数：

socketFd：已连接的句柄。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 停止连续运动（保持上电）
    auto ret = tl::queue_motion_stop_not_power_off(socket_fd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 停止连续运动失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 已停止连续运动（保持上电）" << std::endl;

14. queue_motion_clear_Data - 清空本地队列数据

方法：Result queue_motion_clear_Data(SOCKETFD socketFd);

功能描述：清空缓存的运动队列数据。仅清除上位机侧缓存的队列数据，不清除已经下发给控制器的队列；控制器已接收的队列会继续执行，如需停止需调用 queue_motion_stop。

参数：

socketFd：已连接的句柄。

返回值：

0：成功。
非0：失败。

使用示例：

C++

    // 清空本地队列数据
    auto ret = tl::queue_motion_clear_Data(socket_fd);
    if (ret != tl::Result::SUCCESS) {
        std::cerr << "[ERROR] 清空本地队列数据失败，错误码: " << ret << std::endl;
        return -1;
    }
    std::cout << "[信息] 已清空本地队列数据" << std::endl;