通讯与算法接口

头文件： cpp/interface/nrc_interface.h

1. set_receive_error_or_warnning_message_callback - 设置错误警告回调

方法： typedef void (*ErrorCallback)(int messageType, const char* message, int messageCode); int set_receive_error_or_warnning_message_callback(SOCKETFD socket_fd, ErrorCallback callback);

功能描述： 注册异步错误/警告回调函数。当控制器产生错误或警告时，会异步调用该回调。

参数：

  • socket_fd：已连接的句柄
  • callback_func：回调函数指针，函数类型为void(int, const char*, int)，其中参数：messageType（1=错误，2=警告），message（描述文本），messageCode（错误代码）

返回值：

  • int
    • 0：成功
    • 负值：失败

使用示例：

C++
bool test_register_error_callback() {
    if (socket_fd_6001_ <= 0) return false;
    // 传入控制端口句柄和回调函数指针，向底层通信框架注册主动上报的错误与告警事件监听器
    int ret = set_receive_error_or_warnning_message_callback(socket_fd_6001_, error_callback);
    CHECK_RET(ret, "注册错误回调");
    return ret == SUCCESS;
}

2. get_origin_coord_to_target_coord - 正解算法

方法： int get_origin_coord_to_target_coord(SOCKETFD socket_fd, int origin_coord, const std::vector<double>& origin_pos, int target_coord, std::vector<double>& target_pos);

功能描述： 在不同坐标系之间进行空间坐标转换（纯数学计算，不触发运动）。

参数：

  • socket_fd：已连接的句柄
  • origin_coord：源坐标系（0~3）
  • origin_pos：源坐标数据
  • target_coord：目标坐标系
  • target_pos：输出变换后的坐标

返回值：

  • int
    • 0：成功
    • 负值：失败（如奇异点、不可达）

使用示例：

C++
std::vector<double> test_transform_coordinate(int srcCoord, const std::vector<double>& srcPos, int dstCoord) {
    // 初始化长度为7的返回向量（通常对应直角坐标数据及预留位）
    std::vector<double> out(7, 0.0);
    if (socket_fd_6001_ <= 0) return out;
    // 传入源坐标系、源坐标数据、目标坐标系，调用底层接口进行坐标转换，结果写入out向量
    int ret = get_origin_coord_to_target_coord(socket_fd_6001_, srcCoord, srcPos, dstCoord, out);
    if (ret != SUCCESS) {
        // 变换失败，清空可能存在异常数据的向量并打印错误码
        out.clear();
        std::cout << "坐标变换失败, 错误码=" << ret << std::endl;
        return out;
    }
    // 变换成功，打印提示并返回转换后的坐标数据
    std::cout << "坐标变换成功" << std::endl;
    return out;
}

3. get_quat2rpy - 四元数转欧拉角

方法： int get_quat2rpy(SOCKETFD socket_fd, const std::vector<double>& quat, std::vector<double>& rpy);

功能描述： 四元数[w, x, y, z]转欧拉角[rx, ry, rz]（弧度）。

参数：

  • socket_fd：已连接的句柄
  • quat：输入四元数（4维）
  • rpy：输出欧拉角（3维）

返回值：

  • int
    • 0：成功
    • 负值：失败

使用示例：

C++
std::vector<double> test_quat_to_rpy(const std::vector<double>& quat) {
    // 初始化长度为3的返回向量，用于存储转换后的欧拉角 [Roll, Pitch, Yaw]
    std::vector<double> out(3, 0.0);
    if (socket_fd_6001_ <= 0) return out;
    // 校验输入的四元数数据，确保包含完整的 [W, X, Y, Z] 四个分量
    if (quat.size() < 4) {
        std::cout << "[提示] 四元数输入数据长度不足4位 (需包含 W, X, Y, Z)" << std::endl;
        return out;
    }
    // 传入控制端口句柄和待转换的四元数，调用底层接口进行姿态变换，结果写入out向量
    int ret = get_quat2rpy(socket_fd_6001_, quat, out);
    if (ret != SUCCESS) {
        out.clear();
        std::cout << "[失败] 四元数转欧拉角失败, 错误码 = " << ret << std::endl;
        return out;
    }
    // 转换成功，格式化打印输入与输出的姿态数据
    std::cout << "[成功] 四元数转欧拉角成功\n"
              << "  输入四元数: [" << quat[0] << ", " << quat[1] << ", " << quat[2] << ", " << quat[3] << "]\n"
              << "  输出欧拉角 (RPY): [" << out[0] << ", " << out[1] << ", " << out[2] << "]" << std::endl;
    return out;
}

4. get_rpy2quat - 欧拉角转四元数

方法： int get_rpy2quat(SOCKETFD socket_fd, const std::vector<double>& rpy, std::vector<double>& quat);

功能描述： 欧拉角（弧度）转四元数。

参数：

  • socket_fd：已连接的句柄
  • rpy：输入欧拉角（3维）
  • quat：输出四元数（4维）

返回值：

  • int
    • 0：成功
    • 负值：失败

使用示例：

C++
std::vector<double> test_rpy_to_quat(const std::vector<double>& rpy) {
    std::vector<double> quat_res;
    if (socket_fd_6001_ <= 0) return quat_res;
    // 校验输入的欧拉角数据，确保包含完整的 [Roll, Pitch, Yaw] 三个分量
    if (rpy.size() < 3) {
        std::cout << "[提示] 欧拉角输入数据长度不足3位 (需包含 Roll, Pitch, Yaw)" << std::endl;
        return quat_res;
    }
    // 传入控制端口句柄和待转换的欧拉角，调用底层接口进行姿态变换
    int ret = get_rpy2quat(socket_fd_6001_, rpy, quat_res);
    if (ret != SUCCESS) {
        quat_res.clear();
        std::cout << "[失败] 欧拉角转四元数失败, 错误码 = " << ret << std::endl;
        return quat_res;
    }
    std::cout << "[成功] 欧拉角转四元数成功\n"
              << "  输入欧拉角 (RPY): [" << rpy[0] << ", " << rpy[1] << ", " << rpy[2] << "]\n"
              << "  输出四元数: [";
    for (size_t i = 0; i < quat_res.size(); ++i) {
        std::cout << quat_res[i] << (i == quat_res.size() - 1 ? "" : ", ");
    }
    std::cout << "]" << std::endl;
    return quat_res;
}

5. get_rpy2r - 欧拉角转旋转矩阵

方法： int get_rpy2r(SOCKETFD socket_fd, const std::vector<double>& rpy, std::vector<double>& r);

功能描述： 欧拉角转3x3旋转矩阵（输出9个元素，行主序）。

参数：

  • socket_fd：已连接的句柄
  • rpy：输入欧拉角（3维）
  • r：输出旋转矩阵（9维）

返回值：

  • int
    • 0：成功
    • 负值：失败

使用示例：

C++
std::vector<double> test_rpy_to_matrix(const std::vector<double>& rpy) {
    std::vector<double> r_res;
    if (socket_fd_6001_ <= 0) return r_res;
    if (rpy.size() < 3) {
        std::cout << "[提示] 欧拉角输入数据长度不足3位" << std::endl;
        return r_res;
    }
    int ret = get_rpy2r(socket_fd_6001_, rpy, r_res);
    if (ret != SUCCESS) {
        r_res.clear();
        std::cout << "[失败] 欧拉角转旋转矩阵失败, 错误码 = " << ret << std::endl;
        return r_res;
    }
    std::cout << "[成功] 欧拉角转旋转矩阵成功\n"
              << "  输入欧拉角 (RPY): [" << rpy[0] << ", " << rpy[1] << ", " << rpy[2] << "]\n"
              << "  输出旋转矩阵元素个数: " << r_res.size() << "\n";
    if (r_res.size() == 9) {
        std::cout << "  矩阵排布 (3x3):\n"
                  << "    [ " << r_res[0] << ",\t" << r_res[1] << ",\t" << r_res[2] << " ]\n"
                  << "    [ " << r_res[3] << ",\t" << r_res[4] << ",\t" << r_res[5] << " ]\n"
                  << "    [ " << r_res[6] << ",\t" << r_res[7] << ",\t" << r_res[8] << " ]" << std::endl;
    }
    return r_res;
}

6. get_tr2r - 位姿转旋转矩阵

方法： int get_tr2r(SOCKETFD socket_fd, const std::vector<double>& tr, std::vector<double>& r);

功能描述： 从4x4齐次变换矩阵（16个元素，行主序）中提取3x3旋转矩阵。

参数：

  • socket_fd：已连接的句柄
  • tr：输入齐次变换矩阵（16维）
  • r：输出旋转矩阵（9维）

返回值：

  • int
    • 0：成功
    • 负值：失败

使用示例：

C++
std::vector<double> test_tr_to_rotation(const std::vector<double>& tr_matrix) {
    std::vector<double> r_res;
    if (socket_fd_6001_ <= 0) return r_res;
    if (tr_matrix.size() < 16) {
        std::cout << "[提示] 齐次变换矩阵输入数据长度不足16位" << std::endl;
        return r_res;
    }
    int ret = get_tr2r(socket_fd_6001_, tr_matrix, r_res);
    if (ret != SUCCESS) {
        r_res.clear();
        std::cout << "[失败] 矩阵提取失败, 错误码 = " << ret << std::endl;
        return r_res;
    }
    std::cout << "[成功] 从齐次变换矩阵中提取旋转矩阵成功\n"
              << "  输出旋转矩阵元素个数: " << r_res.size() << "\n";
    if (r_res.size() == 9) {
        std::cout << "  提取出的旋转矩阵 (3x3):\n"
                  << "    [ " << r_res[0] << ",\t" << r_res[1] << ",\t" << r_res[2] << " ]\n"
                  << "    [ " << r_res[3] << ",\t" << r_res[4] << ",\t" << r_res[5] << " ]\n"
                  << "    [ " << r_res[6] << ",\t" << r_res[7] << ",\t" << r_res[8] << " ]" << std::endl;
    }
    return r_res;
}

7. get_r2tr - 旋转矩阵转位姿矩阵

方法： int get_r2tr(SOCKETFD socket_fd, const std::vector<double>& r, std::vector<double>& tr);

功能描述： 将3x3旋转矩阵扩展为4x4齐次变换矩阵（平移部分为零）。

参数：

  • socket_fd：已连接的句柄
  • r：输入旋转矩阵（9维）
  • tr：输出齐次变换矩阵（16维）

返回值：

  • int
    • 0：成功
    • 负值：失败

使用示例：

C++
std::vector<double> test_rotation_to_tr(const std::vector<double>& r_matrix) {
    std::vector<double> tr_res;
    if (socket_fd_6001_ <= 0) return tr_res;
    if (r_matrix.size() < 9) {
        std::cout << "[提示] 旋转矩阵输入数据长度不足9位" << std::endl;
        return tr_res;
    }
    int ret = get_r2tr(socket_fd_6001_, r_matrix, tr_res);
    if (ret != SUCCESS) {
        tr_res.clear();
        std::cout << "[失败] 矩阵转换失败, 错误码 = " << ret << std::endl;
        return tr_res;
    }
    std::cout << "[成功] 旋转矩阵转换为齐次变换矩阵成功\n"
              << "  输出变换矩阵元素个数: " << tr_res.size() << "\n";
    if (tr_res.size() == 16) {
        std::cout << "  生成的齐次变换矩阵 (4x4):\n"
                  << "    [ " << tr_res[0]  << ",\t" << tr_res[1]  << ",\t" << tr_res[2]  << ",\t" << tr_res[3]  << " ]\n"
                  << "    [ " << tr_res[4]  << ",\t" << tr_res[5]  << ",\t" << tr_res[6]  << ",\t" << tr_res[7]  << " ]\n"
                  << "    [ " << tr_res[8]  << ",\t" << tr_res[9]  << ",\t" << tr_res[10] << ",\t" << tr_res[11] << " ]\n"
                  << "    [ " << tr_res[12] << ",\t" << tr_res[13] << ",\t" << tr_res[14] << ",\t" << tr_res[15] << " ]" << std::endl;
    }
    return tr_res;
}

 

我们来帮助您

四川总部：四川省绵阳高新区火炬西街南段10号

联系电话：0816-2120022

邮 箱：TLIBOT@163.com

联系我们

关注天链机器人

公众号
公众号
视频号
视频号
抖音号
抖音号
哔哩哔哩
哔哩哔哩
新浪微博
新浪微博
今日头条
今日头条
小红书
小红书
百度
百度
1688店铺
1688店铺

Copyright © 2026 四川天链机器人股份有限公司 技术支持： 中企动力 · 成都

微博 微信 朋友圈 QQ QQ空间 复制链接

这里是占位文字

Sorry，当前栏目暂无内容!

您可以查看其他栏目或返回 首页

Sorry，The current column has no content!

You can view other columns or return Home