423 lines
16 KiB
C
423 lines
16 KiB
C
|
#pragma once
|
|||
|
|
|||
|
#include "XNGlobalDefine/XNDefine.h"
|
|||
|
|
|||
|
namespace XNSim {
|
|||
|
// 定义UDP包的最大大小
|
|||
|
constexpr XN_SIZE MAX_UDP_PACKET_SIZE = 40000;
|
|||
|
|
|||
|
using XNInterfaceToDataMap = std::unordered_map<XN_STRING, XN_STRING>;
|
|||
|
using XNInterfaceList = std::vector<XN_STRING>;
|
|||
|
using XNGetDataFunction = std::function<XN_STRING()>;
|
|||
|
using XNSetDataFunction = std::function<void(XN_STRING)>;
|
|||
|
using XNGetByteArrayFunction = std::function<XNByteArray(void)>;
|
|||
|
using XNSetByteArrayFunction = std::function<void(XNByteArray, XN_UINT32 &)>;
|
|||
|
using XNGetDataFunctionMap = std::unordered_map<XN_STRING, XNGetDataFunction>;
|
|||
|
using XNSetDataFunctionMap = std::unordered_map<XN_STRING, XNSetDataFunction>;
|
|||
|
using XNGetByteArrayFunctionMap = std::vector<XNGetByteArrayFunction>;
|
|||
|
using XNSetByteArrayFunctionMap = std::vector<XNSetByteArrayFunction>;
|
|||
|
|
|||
|
class XNDDSInterface {
|
|||
|
public:
|
|||
|
XNDDSInterface() = default;
|
|||
|
virtual ~XNDDSInterface() = default;
|
|||
|
|
|||
|
public:
|
|||
|
/**
|
|||
|
* @brief 初始化
|
|||
|
* @param framework: 框架
|
|||
|
*/
|
|||
|
virtual void Initialize(XNFrameworkPtr framework, XN_UINT32 modelID,
|
|||
|
XN_UINT32 DDS_type) = 0;
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 获取该接口的UDP包
|
|||
|
* @return 字节数组
|
|||
|
*/
|
|||
|
XNByteArray getUDPPackage();
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 通过UDP包设置数据
|
|||
|
* @param package: UDP包
|
|||
|
*/
|
|||
|
void setDataByUDPPackage(const XNByteArray &package);
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 批量获取指定变量的数据
|
|||
|
* @param varNames: 变量名列表
|
|||
|
* @return: 变量名到数据的映射
|
|||
|
*/
|
|||
|
XNInterfaceToDataMap getStringData(const XNInterfaceList &varNames);
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 批量设置指定变量的数据
|
|||
|
* @param data: 变量名到数据的映射
|
|||
|
*/
|
|||
|
void setDataByString(const XNInterfaceToDataMap &data);
|
|||
|
|
|||
|
protected:
|
|||
|
/**
|
|||
|
* @brief 获取指定变量的字节数据,用于网络通信
|
|||
|
* @param data: 数据
|
|||
|
* @return 字节数组
|
|||
|
*/
|
|||
|
template <typename T> XNByteArray getByteArray(const XNDDSOptional<T> &data) {
|
|||
|
XNByteArray result(getTypeSize<T>());
|
|||
|
|
|||
|
if constexpr (std::is_arithmetic_v<T>) {
|
|||
|
if (data) {
|
|||
|
std::memcpy(result.data(), &data.value(), sizeof(T));
|
|||
|
} else {
|
|||
|
T zero = 0;
|
|||
|
std::memcpy(result.data(), &zero, sizeof(T));
|
|||
|
}
|
|||
|
} else if constexpr (is_array_v<T>) {
|
|||
|
if (data) {
|
|||
|
return getByteArrayFromStdArray(data.value());
|
|||
|
} else {
|
|||
|
T zero = {};
|
|||
|
return getByteArrayFromStdArray(zero);
|
|||
|
}
|
|||
|
} else {
|
|||
|
static_assert(
|
|||
|
std::is_arithmetic_v<T> || is_array_v<T>,
|
|||
|
"T 必须是算术类型或std::array类型,详见XNDDSInterface.cpp:line 78");
|
|||
|
}
|
|||
|
|
|||
|
return result;
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 设置指定变量的字节数据,用于网络通信
|
|||
|
* @param data: 数据
|
|||
|
* @param byteArray: 字节数组
|
|||
|
*/
|
|||
|
template <typename T>
|
|||
|
void setByteArray(XNDDSOptional<T> &data, const XNByteArray &byteArray,
|
|||
|
XN_UINT32 &pos) {
|
|||
|
XN_UINT32 thisSize = getTypeSize<T>();
|
|||
|
XNByteArray thisArray(thisSize);
|
|||
|
if (pos + thisSize > byteArray.size()) {
|
|||
|
return;
|
|||
|
}
|
|||
|
std::memcpy(thisArray.data(), byteArray.data() + pos, thisSize);
|
|||
|
pos += thisSize;
|
|||
|
|
|||
|
if constexpr (std::is_arithmetic_v<T>) {
|
|||
|
T temp;
|
|||
|
std::memcpy(&temp, thisArray.data(), sizeof(T));
|
|||
|
data = temp;
|
|||
|
} else if constexpr (is_array_v<T>) {
|
|||
|
if (!data) {
|
|||
|
data = T{};
|
|||
|
}
|
|||
|
setByteArrayFromStdArray(data.value(), thisArray);
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<T> || is_array_v<T>,
|
|||
|
"T 必须是算术类型或std::array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 获取指定数组变量的字节数据,用于网络通信
|
|||
|
* @param data: 数据
|
|||
|
* @return 字节数组
|
|||
|
*/
|
|||
|
template <typename T, XN_SIZE N>
|
|||
|
XNByteArray getByteArrayFromStdArray(const std::array<T, N> &data) {
|
|||
|
XNByteArray result(getTypeSize<T>() * N);
|
|||
|
|
|||
|
for (XN_SIZE i = 0; i < N; ++i) {
|
|||
|
if constexpr (std::is_arithmetic_v<T>) {
|
|||
|
std::memcpy(result.data() + i * getTypeSize<T>(), &data[i],
|
|||
|
getTypeSize<T>());
|
|||
|
} else if constexpr (is_array_v<T>) {
|
|||
|
XNByteArray subArray = getByteArrayFromStdArray(data[i]);
|
|||
|
std::memcpy(result.data() + i * getTypeSize<T>(), subArray.data(),
|
|||
|
getTypeSize<T>());
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<T> || is_array_v<T>,
|
|||
|
"T 必须是算术类型或std::array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
return result;
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 设置指定数组变量的字节数据,用于网络通信
|
|||
|
* @param data: 数据
|
|||
|
* @param byteArray: 字节数组
|
|||
|
*/
|
|||
|
template <typename T, XN_SIZE N>
|
|||
|
void setByteArrayFromStdArray(std::array<T, N> &data,
|
|||
|
const XNByteArray &byteArray) {
|
|||
|
if (byteArray.size() < getTypeSize<T>() * N)
|
|||
|
return;
|
|||
|
|
|||
|
for (XN_SIZE i = 0; i < N; ++i) {
|
|||
|
if constexpr (std::is_arithmetic_v<T>) {
|
|||
|
std::memcpy(&data[i], byteArray.data() + i * getTypeSize<T>(),
|
|||
|
getTypeSize<T>());
|
|||
|
} else if constexpr (is_array_v<T>) {
|
|||
|
XNByteArray subArray(byteArray.data() + i * getTypeSize<T>(),
|
|||
|
getTypeSize<T>());
|
|||
|
setByteArrayFromStdArray(data[i], subArray);
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<T> || is_array_v<T>,
|
|||
|
"T 必须是算术类型或std::array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 获取指定变量的字符串数据,用于JSON前端
|
|||
|
* @param data: 数据
|
|||
|
* @return: 数据(字符串格式)
|
|||
|
*/
|
|||
|
template <typename T> std::string getString(const XNDDSOptional<T> &data) {
|
|||
|
if constexpr (std::is_arithmetic_v<T>) {
|
|||
|
if (data) {
|
|||
|
return std::to_string(data.value());
|
|||
|
} else {
|
|||
|
return "Unknown";
|
|||
|
}
|
|||
|
} else if constexpr (is_array_v<T>) {
|
|||
|
if (data) {
|
|||
|
return getStringFromStdArray(data.value());
|
|||
|
} else {
|
|||
|
T zero = {};
|
|||
|
return getStringFromStdArray(zero);
|
|||
|
}
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<T> || is_array_v<T>,
|
|||
|
"T 必须是算术类型或std::array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
return std::string();
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 通过字符串数据设置指定变量,用于JSON前端
|
|||
|
* @param data: 数据
|
|||
|
* @param value: 字符串数据
|
|||
|
*/
|
|||
|
template <typename T>
|
|||
|
void setDataFromString(XNDDSOptional<T> &data, const std::string &value) {
|
|||
|
if constexpr (std::is_arithmetic_v<T>) {
|
|||
|
if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
|
|||
|
data = std::stod(value);
|
|||
|
} else {
|
|||
|
data = std::stoll(value);
|
|||
|
}
|
|||
|
} else if constexpr (is_array_v<T>) {
|
|||
|
// 解析输入字符串
|
|||
|
std::stringstream ss(value);
|
|||
|
std::string item;
|
|||
|
std::vector<std::string> items;
|
|||
|
while (std::getline(ss, item, ',')) {
|
|||
|
items.push_back(item);
|
|||
|
}
|
|||
|
T temp;
|
|||
|
setStdArrayFromString(temp, items, 0);
|
|||
|
data = temp;
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<T> || is_array_v<T>,
|
|||
|
"T 必须是算术类型或std::array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 获取指定数组变量的字符串数据,用于JSON前端
|
|||
|
* @param data: 数据
|
|||
|
* @return: 数据(字符串格式)
|
|||
|
*/
|
|||
|
template <typename T, XN_SIZE N>
|
|||
|
std::string getStringFromStdArray(const std::array<T, N> &data) {
|
|||
|
std::stringstream ss;
|
|||
|
for (XN_SIZE i = 0; i < N; ++i) {
|
|||
|
if (i > 0)
|
|||
|
ss << ",";
|
|||
|
if constexpr (std::is_arithmetic_v<T>) {
|
|||
|
ss << data[i];
|
|||
|
} else if constexpr (is_array_v<T>) {
|
|||
|
ss << getStringFromStdArray(data[i]);
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<T> || is_array_v<T>,
|
|||
|
"T 必须是算术类型或std::array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
return ss.str();
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* @brief 通过字符串数据设置指定数组变量,用于JSON前端
|
|||
|
* @param data: 数据
|
|||
|
* @param value: 字符串数据,格式为"数字,数字,..."
|
|||
|
* @param start_pos: 当前数组在输入字符串中的起始位置
|
|||
|
* @return 处理完当前数组后,下一个数组的起始位置
|
|||
|
* @throw std::runtime_error: 当输入数据格式不正确时抛出异常
|
|||
|
*/
|
|||
|
template <typename T, XN_SIZE N>
|
|||
|
size_t setStdArrayFromString(std::array<T, N> &data,
|
|||
|
const std::vector<std::string> &value,
|
|||
|
size_t start_pos = 0) {
|
|||
|
// 递归处理每个元素
|
|||
|
for (XN_SIZE i = 0; i < N; ++i) {
|
|||
|
try {
|
|||
|
if constexpr (std::is_arithmetic_v<T>) {
|
|||
|
// 对于基本类型,直接转换
|
|||
|
if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
|
|||
|
data[i] = static_cast<T>(std::stod(value[start_pos + i]));
|
|||
|
} else {
|
|||
|
data[i] = static_cast<T>(std::stoll(value[start_pos + i]));
|
|||
|
}
|
|||
|
} else if constexpr (is_array_v<T>) {
|
|||
|
// 对于嵌套数组,递归处理
|
|||
|
start_pos = setStdArrayFromString(data[i], value, start_pos);
|
|||
|
} else {
|
|||
|
static_assert(
|
|||
|
std::is_arithmetic_v<T> || is_array_v<T>,
|
|||
|
"T 必须是算术类型或std::array类型,详见XNDDSInterface.cpp:line "
|
|||
|
"275");
|
|||
|
}
|
|||
|
} catch (const std::exception &e) {
|
|||
|
throw std::runtime_error("无法解析第 " + std::to_string(i) +
|
|||
|
" 个元素: " + e.what());
|
|||
|
}
|
|||
|
}
|
|||
|
return start_pos + N;
|
|||
|
}
|
|||
|
|
|||
|
template <typename T1, typename T2>
|
|||
|
void assign_value_get(const XNDDSOptional<T1> &data, T2 &model_data) {
|
|||
|
if (data) {
|
|||
|
auto temp = data.value();
|
|||
|
if constexpr (std::is_arithmetic_v<T1>) {
|
|||
|
static_assert(std::is_arithmetic_v<T2>,
|
|||
|
"模板参数T2必须是算术类型,详见" + XN_STRING(__FILE__) +
|
|||
|
":" + XN_STRING(__LINE__));
|
|||
|
static_assert(std::is_convertible_v<T1, T2>,
|
|||
|
"模板参数T1必须可以转换为T2类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
model_data = temp;
|
|||
|
} else if constexpr (is_array_v<T1>) {
|
|||
|
XN_SIZE arraySize = array_size_v<T1>;
|
|||
|
for (XN_SIZE i = 0; i < arraySize; ++i) {
|
|||
|
auto temp2 = temp[i];
|
|||
|
using array_type = typename T1::value_type;
|
|||
|
if constexpr (std::is_arithmetic_v<array_type>) {
|
|||
|
model_data[i] = temp2;
|
|||
|
} else if constexpr (is_array_v<array_type>) {
|
|||
|
XN_SIZE arraySize2 = array_size_v<array_type>;
|
|||
|
using sub_array_type = typename array_type::value_type;
|
|||
|
if constexpr (std::is_arithmetic_v<sub_array_type>) {
|
|||
|
for (XN_SIZE j = 0; j < arraySize2; ++j) {
|
|||
|
model_data[i][j] = temp2[j];
|
|||
|
}
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<sub_array_type>,
|
|||
|
"模板参数T1是std::"
|
|||
|
"array类型时,它的数组嵌套不能超过两层,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" +
|
|||
|
XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<array_type> ||
|
|||
|
is_array_v<array_type>,
|
|||
|
"模板参数T1是std::array类型时,它的value_"
|
|||
|
"type必须是算术类型或std::"
|
|||
|
"array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<T1> || is_array_v<T1>,
|
|||
|
"模板参数T1必须是算术类型或std::"
|
|||
|
"array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
template <typename T1, typename T2>
|
|||
|
void assign_value_set(XNDDSOptional<T1> &data, const T2 &model_data) {
|
|||
|
if constexpr (std::is_arithmetic_v<T1>) {
|
|||
|
static_assert(std::is_arithmetic_v<T2>, "模板参数T2必须是算术类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" +
|
|||
|
XN_STRING(__LINE__));
|
|||
|
static_assert(std::is_convertible_v<T2, T1>,
|
|||
|
"模板参数T2必须可以转换为T1类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
data = model_data;
|
|||
|
} else if constexpr (is_array_v<T1>) {
|
|||
|
T1 temp;
|
|||
|
XN_SIZE arraySize = array_size_v<T1>;
|
|||
|
using array_type = typename T1::value_type;
|
|||
|
for (XN_SIZE i = 0; i < arraySize; ++i) {
|
|||
|
if constexpr (std::is_arithmetic_v<array_type>) {
|
|||
|
temp[i] = model_data[i];
|
|||
|
} else if constexpr (is_array_v<array_type>) {
|
|||
|
XN_SIZE arraySize2 = array_size_v<array_type>;
|
|||
|
using sub_array_type = typename array_type::value_type;
|
|||
|
if constexpr (std::is_arithmetic_v<sub_array_type>) {
|
|||
|
for (XN_SIZE j = 0; j < arraySize2; ++j) {
|
|||
|
temp[i][j] = model_data[i][j];
|
|||
|
}
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<sub_array_type>,
|
|||
|
"模板参数T1是std::"
|
|||
|
"array类型时,它的数组嵌套不能超过两层,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<array_type> ||
|
|||
|
is_array_v<array_type>,
|
|||
|
"模板参数T1是std::array类型时,它的value_"
|
|||
|
"type必须是算术类型或std::"
|
|||
|
"array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
data = temp;
|
|||
|
} else {
|
|||
|
static_assert(std::is_arithmetic_v<T1> || is_array_v<T1>,
|
|||
|
"模板参数T1必须是算术类型或std::"
|
|||
|
"array类型,详见" +
|
|||
|
XN_STRING(__FILE__) + ":" + XN_STRING(__LINE__));
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
virtual void clearOutData() {}
|
|||
|
virtual void sendOutData() {}
|
|||
|
|
|||
|
protected:
|
|||
|
XNGetDataFunctionMap getDataFunction;
|
|||
|
XNSetDataFunctionMap setDataFunction;
|
|||
|
XNGetByteArrayFunctionMap getByteArrayFunction;
|
|||
|
XNSetByteArrayFunctionMap setByteArrayFunction;
|
|||
|
XN_MUTEX dataMutex;
|
|||
|
XN_MUTEX outDataMutex;
|
|||
|
XN_UINT8 header[8]{0}; // 固定大小的头部
|
|||
|
XN_SIZE headerSize = 8;
|
|||
|
XNDDSDataWriterPtr dataWriter;
|
|||
|
};
|
|||
|
} // namespace XNSim
|
|||
|
|
|||
|
#define MAP_DATA_FUNC(NAME) \
|
|||
|
getDataFunction[#NAME] = [this]() { return getString(data.NAME()); }; \
|
|||
|
setDataFunction[#NAME] = [this](const XN_STRING &value) { \
|
|||
|
setDataFromString(out_data.NAME(), value); \
|
|||
|
}; \
|
|||
|
getByteArrayFunction.push_back( \
|
|||
|
[this]() { return getByteArray(data.NAME()); }); \
|
|||
|
setByteArrayFunction.push_back( \
|
|||
|
[this](const XNByteArray &byteArray, XN_UINT32 &pos) { \
|
|||
|
setByteArray(out_data.NAME(), byteArray, pos); \
|
|||
|
});
|