修改DDSInterface实现,修复ModelObject的问题

This commit is contained in:
jinchao 2025-05-21 12:17:50 +08:00
parent efcc6597cb
commit a6818f35bc
26 changed files with 951 additions and 330 deletions

View File

@ -0,0 +1,99 @@
#pragma once
#include <vector>
#include <cstdint>
#include <cstring>
/**
* @brief
* @details QByteArray的功能
*/
class XNByteArray
{
public:
XNByteArray() = default;
explicit XNByteArray(size_t size) : data_(size) {}
/**
* @brief
* @param buffer
* @param size
*/
XNByteArray(const uint8_t *buffer, size_t size) : data_(buffer, buffer + size) {}
/**
* @brief
* @return
*/
uint8_t *data() { return data_.data(); }
/**
* @brief
* @return
*/
const uint8_t *data() const { return data_.data(); }
/**
* @brief
* @return
*/
size_t size() const { return data_.size(); }
/**
* @brief
* @param size
*/
void resize(size_t size) { data_.resize(size); }
/**
* @brief
*/
void clear() { data_.clear(); }
/**
* @brief
* @param buffer
* @param size
*/
void append(const uint8_t *buffer, size_t size)
{
size_t oldSize = data_.size();
data_.resize(oldSize + size);
std::memcpy(data_.data() + oldSize, buffer, size);
}
/**
* @brief XNByteArray
* @param other XNByteArray
*/
void append(const XNByteArray &other) { append(other.data(), other.size()); }
/**
* @brief
* @param index
* @return
*/
uint8_t &operator[](size_t index) { return data_[index]; }
/**
* @brief
* @param index
* @return
*/
const uint8_t &operator[](size_t index) const { return data_[index]; }
/**
* @brief
* @return
*/
bool isEmpty() const { return data_.empty(); }
/**
* @brief
* @param size
*/
void reserve(size_t size) { data_.reserve(size); }
private:
std::vector<uint8_t> data_;
};

View File

@ -37,6 +37,9 @@
# include <sstream> # include <sstream>
# include <dlfcn.h> # include <dlfcn.h>
# include <filesystem> # include <filesystem>
# include <array>
# include <cstring>
# include <iostream>
#endif #endif
#include <fastdds/dds/domain/DomainParticipant.hpp> #include <fastdds/dds/domain/DomainParticipant.hpp>
@ -47,6 +50,7 @@
#include <fastdds/dds/subscriber/Subscriber.hpp> #include <fastdds/dds/subscriber/Subscriber.hpp>
#include <fastdds/dds/topic/TypeSupport.hpp> #include <fastdds/dds/topic/TypeSupport.hpp>
#include <fastdds/dds/subscriber/DataReaderListener.hpp> #include <fastdds/dds/subscriber/DataReaderListener.hpp>
#include <fastcdr/xcdr/optional.hpp>
#define FAST_DDS_MACRO eprosima::fastdds::dds #define FAST_DDS_MACRO eprosima::fastdds::dds
/** /**

View File

@ -1,191 +1,149 @@
#pragma once #pragma once
#include <string> #include "XNObject.h"
#include <vector> #include "XNByteArray.h"
#include <unordered_map> #include "XNTypeTraits.h"
#include <mutex> #include <stdexcept>
#include <sstream>
#include <iomanip>
#include <array>
#include <functional>
#include <cstring>
#include <iostream>
#include <fastcdr/xcdr/optional.hpp>
// 定义UDP包的最大大小 // 定义UDP包的最大大小
constexpr size_t MAX_UDP_PACKET_SIZE = 40000; constexpr size_t MAX_UDP_PACKET_SIZE = 40000;
template <typename T>
struct is_std_array : std::false_type {
};
template <typename T, std::size_t N>
struct is_std_array<std::array<T, N>> : std::true_type {
};
// 变量模板简化使用
template <typename T>
inline constexpr bool is_std_array_v = is_std_array<T>::value;
// 获取类型大小的辅助函数
template <typename T>
constexpr size_t getTypeSize()
{
if constexpr (is_std_array_v<T>) {
// 对于std::array计算所有元素的总大小
return getTypeSize<typename T::value_type>() * std::tuple_size<T>::value;
} else {
return sizeof(T);
}
}
class XNDDSInterface class XNDDSInterface
{ {
public: public:
XNDDSInterface() = default; XNDDSInterface() = default;
virtual ~XNDDSInterface() = default; virtual ~XNDDSInterface() = default;
void getUDPPackage(uint8_t *buffer, size_t bufferSize) public:
{ /**
if (bufferSize < MAX_UDP_PACKET_SIZE) * @brief
return; * @param framework:
*/
virtual void Initialize(XNFrameworkPtr framework) = 0;
size_t currentPos = 0; /**
* @brief UDP包
* @return
*/
XNByteArray getUDPPackage();
// 复制头部 /**
if (headerSize >= 5) { * @brief JSON前端获取指定变量的数据
std::memcpy(buffer, header, 5); * @param varName:
currentPos = 5; * @param nameSize:
} * @param varData:
* @param dataSize:
// 复制数据 */
for (auto func : getByteArrayFunction) { void getData(const char *varName, const size_t nameSize, char *varData, size_t dataSize);
if (currentPos + func.size <= MAX_UDP_PACKET_SIZE) {
func.func(buffer + currentPos, func.size);
currentPos += func.size;
} else {
break; // 超出最大包大小
}
}
// 更新包大小
if (currentPos >= 5) {
buffer[4] = static_cast<uint8_t>(currentPos);
}
}
std::string getData(const std::string &varName)
{
int index1 = -1;
int index2 = -1;
std::string trueVarName = varName;
size_t startPos = varName.find('[');
if (startPos != std::string::npos) {
size_t midPos = varName.find("][", startPos);
size_t endPos = varName.find_last_of(']');
if (midPos != std::string::npos) {
try {
index1 = std::stoi(varName.substr(startPos + 1, midPos - startPos - 1));
index2 = std::stoi(varName.substr(midPos + 2, endPos - midPos - 2));
} catch (...) {
std::cerr << "无法解析数组索引: " << varName << std::endl;
index1 = 0;
index2 = 0;
}
} else if (endPos != std::string::npos) {
try {
index1 = std::stoi(varName.substr(startPos + 1, endPos - startPos - 1));
} catch (...) {
std::cerr << "无法解析数组索引: " << varName << std::endl;
index1 = 0;
}
}
trueVarName = varName.substr(0, startPos);
}
auto it = getDataFunction.find(trueVarName);
if (it == getDataFunction.end()) {
return std::string();
}
std::lock_guard<std::mutex> lock(mutex);
std::string result = it->second();
if (index1 < 0) {
return result;
}
std::vector<std::string> list;
std::stringstream ss(result);
std::string item;
while (std::getline(ss, item, ',')) {
list.push_back(item);
}
if (index1 >= static_cast<int>(list.size())) {
std::cerr << "数组索引超出范围: " << varName << std::endl;
return std::string();
}
if (index2 < 0) {
return list[index1];
}
std::vector<std::string> list2;
std::stringstream ss2(list[index1]);
while (std::getline(ss2, item, ' ')) {
list2.push_back(item);
}
if (index2 >= static_cast<int>(list2.size())) {
std::cerr << "数组索引超出范围: " << varName << std::endl;
return std::string();
}
return list2[index2];
}
protected: protected:
/**
* @brief
* @param data:
* @return
*/
template <typename T> template <typename T>
void getByteArray(eprosima::fastcdr::optional<T> data, uint8_t *buffer, size_t bufferSize) XNByteArray getByteArray(eprosima::fastcdr::optional<T> data)
{ {
if (bufferSize < getTypeSize<T>()) XNByteArray result(getTypeSize<T>());
return;
if constexpr (std::is_arithmetic_v<T>) { if constexpr (std::is_arithmetic_v<T>) {
if (data) { if (data) {
std::memcpy(buffer, &data.value(), sizeof(T)); std::memcpy(result.data(), &data.value(), sizeof(T));
} else { } else {
T zero = 0; T zero = 0;
std::memcpy(buffer, &zero, sizeof(T)); std::memcpy(result.data(), &zero, sizeof(T));
} }
} else if constexpr (is_std_array_v<T>) { } else if constexpr (is_std_array_v<T>) {
if (data) { if (data) {
getByteArrayFromStdArray(data.value(), buffer, bufferSize); return getByteArrayFromStdArray(data.value());
} else { } else {
T zero = {}; T zero = {};
getByteArrayFromStdArray(zero, buffer, bufferSize); return getByteArrayFromStdArray(zero);
} }
} else {
static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
"Type must be arithmetic or std::array");
}
return result;
}
/**
* @brief
* @param data:
* @param byteArray:
*/
template <typename T>
void setByteArray(eprosima::fastcdr::optional<T> &data, const XNByteArray &byteArray)
{
if (byteArray.size() < getTypeSize<T>())
return;
if constexpr (std::is_arithmetic_v<T>) {
T temp;
std::memcpy(&temp, byteArray.data(), sizeof(T));
data = temp;
} else if constexpr (is_std_array_v<T>) {
if (!data) {
data = T{};
}
setByteArrayFromStdArray(data.value(), byteArray);
} else {
static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
"Type must be arithmetic or std::array");
} }
} }
/**
* @brief
* @param data:
* @return
*/
template <typename T, std::size_t N> template <typename T, std::size_t N>
void getByteArrayFromStdArray(std::array<T, N> data, uint8_t *buffer, size_t bufferSize) XNByteArray getByteArrayFromStdArray(const std::array<T, N> &data)
{ {
if (bufferSize < getTypeSize<T>() * N) XNByteArray result(getTypeSize<T>() * N);
for (std::size_t i = 0; i < N; ++i) {
if constexpr (std::is_arithmetic_v<T>) {
std::memcpy(result.data() + i * getTypeSize<T>(), &data[i], getTypeSize<T>());
} else {
XNByteArray subArray = getByteArrayFromStdArray(data[i]);
std::memcpy(result.data() + i * getTypeSize<T>(), subArray.data(),
getTypeSize<T>());
}
}
return result;
}
/**
* @brief
* @param data:
* @param byteArray:
*/
template <typename T, std::size_t N>
void setByteArrayFromStdArray(std::array<T, N> &data, const XNByteArray &byteArray)
{
if (byteArray.size() < getTypeSize<T>() * N)
return; return;
for (std::size_t i = 0; i < N; ++i) { for (std::size_t i = 0; i < N; ++i) {
if constexpr (std::is_arithmetic_v<T>) { if constexpr (std::is_arithmetic_v<T>) {
std::memcpy(buffer + i * getTypeSize<T>(), &data[i], getTypeSize<T>()); std::memcpy(&data[i], byteArray.data() + i * getTypeSize<T>(), getTypeSize<T>());
} else { } else {
getByteArrayFromStdArray(data[i], buffer + i * getTypeSize<T>(), getTypeSize<T>()); XNByteArray subArray(byteArray.data() + i * getTypeSize<T>(), getTypeSize<T>());
setByteArrayFromStdArray(data[i], subArray);
} }
} }
} }
/**
* @brief JSON前端
* @param data:
* @return:
*/
template <typename T> template <typename T>
std::string getString(eprosima::fastcdr::optional<T> data) std::string getString(eprosima::fastcdr::optional<T> data)
{ {
@ -206,6 +164,11 @@ protected:
return std::string(); return std::string();
} }
/**
* @brief JSON前端
* @param data:
* @return:
*/
template <typename T, std::size_t N> template <typename T, std::size_t N>
std::string getStringFromStdArray(std::array<T, N> data) std::string getStringFromStdArray(std::array<T, N> data)
{ {
@ -222,24 +185,35 @@ protected:
return ss.str(); return ss.str();
} }
private:
/**
* @brief
* @param varName:
* @return:
*/
std::string getData(const std::string &varName);
protected: protected:
struct ByteArrayFunc { struct ByteArrayFunc {
std::function<void(uint8_t *, size_t)> func; std::function<XNByteArray(void)> func;
size_t size; size_t size;
}; };
std::unordered_map<std::string, std::function<std::string()>> getDataFunction; std::unordered_map<std::string, std::function<std::string()>> getDataFunction;
std::vector<ByteArrayFunc> getByteArrayFunction; std::vector<ByteArrayFunc> getByteArrayFunction;
std::unordered_map<std::string, std::function<void(XNByteArray)>> setByteArrayFunction;
std::mutex mutex; std::mutex mutex;
uint8_t header[5]{}; // 固定大小的头部 uint8_t header[5]{}; // 固定大小的头部
size_t headerSize = 0; size_t headerSize = 0;
}; };
#define MAP_GET_DATA_FUNC(NAME) \ #define MAP_DATA_FUNC(NAME) \
getDataFunction[#NAME] = [this]() { return getString(data.NAME()); }; \ getDataFunction[#NAME] = [this]() { return getString(data.NAME()); }; \
getByteArrayFunction.push_back( \ getByteArrayFunction.push_back( \
{[this](uint8_t *buffer, size_t size) { getByteArray(data.NAME(), buffer, size); }, \ {[this]() { return getByteArray(data.NAME()); }, getTypeSize<decltype(data.NAME())>()}); \
getTypeSize<decltype(data.NAME())>()}) setByteArrayFunction[#NAME] = [this](XNByteArray byteArray) { \
setByteArray(data.NAME(), byteArray); \
}
#define ASSIGN_VALUE_GET(NAME) \ #define ASSIGN_VALUE_GET(NAME) \
if (data.NAME()) { \ if (data.NAME()) { \

View File

@ -121,6 +121,54 @@ public:
*/ */
void SetVersion(const std::string &sVersion); void SetVersion(const std::string &sVersion);
/**
* @brief
* @return uint32_t:
*/
uint32_t GetRunFreq() const;
/**
* @brief
* @param runFreq: uint32_t类型
*/
void SetRunFreq(uint32_t runFreq);
/**
* @brief
* @return uint32_t:
*/
uint32_t GetRunNode() const;
/**
* @brief
* @param runNode: uint32_t类型
*/
void SetRunNode(uint32_t runNode);
/**
* @brief
* @return uint32_t:
*/
uint32_t GetRunPriority() const;
/**
* @brief
* @param runPriority: uint32_t类型
*/
void SetRunPriority(uint32_t runPriority);
/**
* @brief
* @return double:
*/
double GetSetFreq() const;
/**
* @brief
* @param setFreq: double类型
*/
void SetSetFreq(double setFreq);
/** /**
* @brief * @brief
* @details * @details
@ -169,12 +217,18 @@ public:
TriggerEvent(eventName, eventData, true, XNEvent::Priority::RealTime); TriggerEvent(eventName, eventData, true, XNEvent::Priority::RealTime);
} }
void SetInitializeType(uint32_t initialType);
void SetThreadID(uint32_t threadID);
uint32_t GetThreadID() const;
/** /**
* @brief * @brief
* @details * @details
* 使AddMyFunction方法注册需要被线程调用的函数 * 使AddMyFunction方法注册需要被线程调用的函数
*/ */
virtual void Initialize(uint32_t initialType, uint32_t threadID); virtual void Initialize();
/** /**
* @brief 仿 * @brief 仿

View File

@ -84,6 +84,11 @@ struct XNModelObjectPrivate : public XNObjectPrivate {
*/ */
double _setFreq; double _setFreq;
/**
* @brief
*/
uint32_t _initialType;
/** /**
* @brief 线ID * @brief 线ID
*/ */

View File

@ -81,6 +81,22 @@ public:
*/ */
virtual bool PrepareForExecute() override; virtual bool PrepareForExecute() override;
/**
* @brief 线
* @param threadID: UINT32类型线ID
* @param freq: double类型线
* @details 线
*/
void SetThreadFreqByID(uint32_t threadID, double freq);
/**
* @brief 线
* @param threadID: UINT32类型线ID
* @return double: 线
* @details 线
*/
double GetThreadFreqByID(uint32_t threadID);
/** /**
* @brief 仿 * @brief 仿
* @param objectId: ID * @param objectId: ID

View File

@ -0,0 +1,28 @@
#pragma once
#include <array>
#include <type_traits>
#include <cstddef>
template <typename T>
struct is_std_array : std::false_type {
};
template <typename T, std::size_t N>
struct is_std_array<std::array<T, N>> : std::true_type {
};
// 变量模板简化使用
template <typename T>
inline constexpr bool is_std_array_v = is_std_array<T>::value;
// 获取类型大小的辅助函数
template <typename T>
constexpr size_t getTypeSize()
{
if constexpr (is_std_array_v<T>) {
// 对于std::array计算所有元素的总大小
return getTypeSize<typename T::value_type>() * std::tuple_size<T>::value;
} else {
return sizeof(T);
}
}

View File

@ -79,6 +79,8 @@
"csignal": "cpp", "csignal": "cpp",
"any": "cpp", "any": "cpp",
"unordered_set": "cpp", "unordered_set": "cpp",
"fstream": "cpp" "fstream": "cpp",
"forward_list": "cpp",
"valarray": "cpp"
} }
} }

View File

@ -21,6 +21,8 @@ find_package(OpenSSL REQUIRED)
add_library(XNCore SHARED add_library(XNCore SHARED
XNCore_global.h XNCore_global.h
XNCore_Function.cpp XNCore_Function.cpp
XNTypeTraits.h
XNByteArray.h
XNObject.h XNObject.h
XNObject.cpp XNObject.cpp
XNObject_p.h XNObject_p.h

99
XNCore/XNByteArray.h Normal file
View File

@ -0,0 +1,99 @@
#pragma once
#include <vector>
#include <cstdint>
#include <cstring>
/**
* @brief
* @details QByteArray的功能
*/
class XNByteArray
{
public:
XNByteArray() = default;
explicit XNByteArray(size_t size) : data_(size) {}
/**
* @brief
* @param buffer
* @param size
*/
XNByteArray(const uint8_t *buffer, size_t size) : data_(buffer, buffer + size) {}
/**
* @brief
* @return
*/
uint8_t *data() { return data_.data(); }
/**
* @brief
* @return
*/
const uint8_t *data() const { return data_.data(); }
/**
* @brief
* @return
*/
size_t size() const { return data_.size(); }
/**
* @brief
* @param size
*/
void resize(size_t size) { data_.resize(size); }
/**
* @brief
*/
void clear() { data_.clear(); }
/**
* @brief
* @param buffer
* @param size
*/
void append(const uint8_t *buffer, size_t size)
{
size_t oldSize = data_.size();
data_.resize(oldSize + size);
std::memcpy(data_.data() + oldSize, buffer, size);
}
/**
* @brief XNByteArray
* @param other XNByteArray
*/
void append(const XNByteArray &other) { append(other.data(), other.size()); }
/**
* @brief
* @param index
* @return
*/
uint8_t &operator[](size_t index) { return data_[index]; }
/**
* @brief
* @param index
* @return
*/
const uint8_t &operator[](size_t index) const { return data_[index]; }
/**
* @brief
* @return
*/
bool isEmpty() const { return data_.empty(); }
/**
* @brief
* @param size
*/
void reserve(size_t size) { data_.reserve(size); }
private:
std::vector<uint8_t> data_;
};

View File

@ -37,6 +37,9 @@
# include <sstream> # include <sstream>
# include <dlfcn.h> # include <dlfcn.h>
# include <filesystem> # include <filesystem>
# include <array>
# include <cstring>
# include <iostream>
#endif #endif
#include <fastdds/dds/domain/DomainParticipant.hpp> #include <fastdds/dds/domain/DomainParticipant.hpp>
@ -47,6 +50,7 @@
#include <fastdds/dds/subscriber/Subscriber.hpp> #include <fastdds/dds/subscriber/Subscriber.hpp>
#include <fastdds/dds/topic/TypeSupport.hpp> #include <fastdds/dds/topic/TypeSupport.hpp>
#include <fastdds/dds/subscriber/DataReaderListener.hpp> #include <fastdds/dds/subscriber/DataReaderListener.hpp>
#include <fastcdr/xcdr/optional.hpp>
#define FAST_DDS_MACRO eprosima::fastdds::dds #define FAST_DDS_MACRO eprosima::fastdds::dds
/** /**

107
XNCore/XNDDSInterface.cpp Normal file
View File

@ -0,0 +1,107 @@
#include "XNDDSInterface.h"
#include "XNObject_p.h"
#include <nlohmann/json.hpp>
using json = nlohmann::json;
void XNDDSInterface::getUDPPackage(uint8_t *buffer, size_t bufferSize)
{
if (bufferSize < MAX_UDP_PACKET_SIZE)
return;
size_t currentPos = 0;
// 复制头部
if (headerSize >= 5) {
std::memcpy(buffer, header, 5);
currentPos = 5;
}
// 复制数据
for (auto func : getByteArrayFunction) {
if (currentPos + func.size <= MAX_UDP_PACKET_SIZE) {
func.func(buffer + currentPos, func.size);
currentPos += func.size;
} else {
break; // 超出最大包大小
}
}
// 更新包大小
if (currentPos >= 5) {
buffer[4] = static_cast<uint8_t>(currentPos);
}
}
std::string XNDDSInterface::getData(const std::string &varName)
{
int index1 = -1;
int index2 = -1;
std::string trueVarName = varName;
size_t startPos = varName.find('[');
if (startPos != std::string::npos) {
size_t midPos = varName.find("][", startPos);
size_t endPos = varName.find_last_of(']');
if (midPos != std::string::npos) {
try {
index1 = std::stoi(varName.substr(startPos + 1, midPos - startPos - 1));
index2 = std::stoi(varName.substr(midPos + 2, endPos - midPos - 2));
} catch (...) {
std::cerr << "无法解析数组索引: " << varName << std::endl;
index1 = 0;
index2 = 0;
}
} else if (endPos != std::string::npos) {
try {
index1 = std::stoi(varName.substr(startPos + 1, endPos - startPos - 1));
} catch (...) {
std::cerr << "无法解析数组索引: " << varName << std::endl;
index1 = 0;
}
}
trueVarName = varName.substr(0, startPos);
}
auto it = getDataFunction.find(trueVarName);
if (it == getDataFunction.end()) {
return std::string();
}
std::lock_guard<std::mutex> lock(mutex);
std::string result = it->second();
if (index1 < 0) {
return result;
}
std::vector<std::string> list;
std::stringstream ss(result);
std::string item;
while (std::getline(ss, item, ',')) {
list.push_back(item);
}
if (index1 >= static_cast<int>(list.size())) {
std::cerr << "数组索引超出范围: " << varName << std::endl;
return std::string();
}
if (index2 < 0) {
return list[index1];
}
std::vector<std::string> list2;
std::stringstream ss2(list[index1]);
while (std::getline(ss2, item, ' ')) {
list2.push_back(item);
}
if (index2 >= static_cast<int>(list2.size())) {
std::cerr << "数组索引超出范围: " << varName << std::endl;
return std::string();
}
return list2[index2];
}

View File

@ -1,191 +1,149 @@
#pragma once #pragma once
#include <string> #include "XNObject.h"
#include <vector> #include "XNByteArray.h"
#include <unordered_map> #include "XNTypeTraits.h"
#include <mutex> #include <stdexcept>
#include <sstream>
#include <iomanip>
#include <array>
#include <functional>
#include <cstring>
#include <iostream>
#include <fastcdr/xcdr/optional.hpp>
// 定义UDP包的最大大小 // 定义UDP包的最大大小
constexpr size_t MAX_UDP_PACKET_SIZE = 40000; constexpr size_t MAX_UDP_PACKET_SIZE = 40000;
template <typename T>
struct is_std_array : std::false_type {
};
template <typename T, std::size_t N>
struct is_std_array<std::array<T, N>> : std::true_type {
};
// 变量模板简化使用
template <typename T>
inline constexpr bool is_std_array_v = is_std_array<T>::value;
// 获取类型大小的辅助函数
template <typename T>
constexpr size_t getTypeSize()
{
if constexpr (is_std_array_v<T>) {
// 对于std::array计算所有元素的总大小
return getTypeSize<typename T::value_type>() * std::tuple_size<T>::value;
} else {
return sizeof(T);
}
}
class XNDDSInterface class XNDDSInterface
{ {
public: public:
XNDDSInterface() = default; XNDDSInterface() = default;
virtual ~XNDDSInterface() = default; virtual ~XNDDSInterface() = default;
void getUDPPackage(uint8_t *buffer, size_t bufferSize) public:
{ /**
if (bufferSize < MAX_UDP_PACKET_SIZE) * @brief
return; * @param framework:
*/
virtual void Initialize(XNFrameworkPtr framework) = 0;
size_t currentPos = 0; /**
* @brief UDP包
* @return
*/
XNByteArray getUDPPackage();
// 复制头部 /**
if (headerSize >= 5) { * @brief JSON前端获取指定变量的数据
std::memcpy(buffer, header, 5); * @param varName:
currentPos = 5; * @param nameSize:
} * @param varData:
* @param dataSize:
// 复制数据 */
for (auto func : getByteArrayFunction) { void getData(const char *varName, const size_t nameSize, char *varData, size_t dataSize);
if (currentPos + func.size <= MAX_UDP_PACKET_SIZE) {
func.func(buffer + currentPos, func.size);
currentPos += func.size;
} else {
break; // 超出最大包大小
}
}
// 更新包大小
if (currentPos >= 5) {
buffer[4] = static_cast<uint8_t>(currentPos);
}
}
std::string getData(const std::string &varName)
{
int index1 = -1;
int index2 = -1;
std::string trueVarName = varName;
size_t startPos = varName.find('[');
if (startPos != std::string::npos) {
size_t midPos = varName.find("][", startPos);
size_t endPos = varName.find_last_of(']');
if (midPos != std::string::npos) {
try {
index1 = std::stoi(varName.substr(startPos + 1, midPos - startPos - 1));
index2 = std::stoi(varName.substr(midPos + 2, endPos - midPos - 2));
} catch (...) {
std::cerr << "无法解析数组索引: " << varName << std::endl;
index1 = 0;
index2 = 0;
}
} else if (endPos != std::string::npos) {
try {
index1 = std::stoi(varName.substr(startPos + 1, endPos - startPos - 1));
} catch (...) {
std::cerr << "无法解析数组索引: " << varName << std::endl;
index1 = 0;
}
}
trueVarName = varName.substr(0, startPos);
}
auto it = getDataFunction.find(trueVarName);
if (it == getDataFunction.end()) {
return std::string();
}
std::lock_guard<std::mutex> lock(mutex);
std::string result = it->second();
if (index1 < 0) {
return result;
}
std::vector<std::string> list;
std::stringstream ss(result);
std::string item;
while (std::getline(ss, item, ',')) {
list.push_back(item);
}
if (index1 >= static_cast<int>(list.size())) {
std::cerr << "数组索引超出范围: " << varName << std::endl;
return std::string();
}
if (index2 < 0) {
return list[index1];
}
std::vector<std::string> list2;
std::stringstream ss2(list[index1]);
while (std::getline(ss2, item, ' ')) {
list2.push_back(item);
}
if (index2 >= static_cast<int>(list2.size())) {
std::cerr << "数组索引超出范围: " << varName << std::endl;
return std::string();
}
return list2[index2];
}
protected: protected:
/**
* @brief
* @param data:
* @return
*/
template <typename T> template <typename T>
void getByteArray(eprosima::fastcdr::optional<T> data, uint8_t *buffer, size_t bufferSize) XNByteArray getByteArray(eprosima::fastcdr::optional<T> data)
{ {
if (bufferSize < getTypeSize<T>()) XNByteArray result(getTypeSize<T>());
return;
if constexpr (std::is_arithmetic_v<T>) { if constexpr (std::is_arithmetic_v<T>) {
if (data) { if (data) {
std::memcpy(buffer, &data.value(), sizeof(T)); std::memcpy(result.data(), &data.value(), sizeof(T));
} else { } else {
T zero = 0; T zero = 0;
std::memcpy(buffer, &zero, sizeof(T)); std::memcpy(result.data(), &zero, sizeof(T));
} }
} else if constexpr (is_std_array_v<T>) { } else if constexpr (is_std_array_v<T>) {
if (data) { if (data) {
getByteArrayFromStdArray(data.value(), buffer, bufferSize); return getByteArrayFromStdArray(data.value());
} else { } else {
T zero = {}; T zero = {};
getByteArrayFromStdArray(zero, buffer, bufferSize); return getByteArrayFromStdArray(zero);
} }
} else {
static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
"Type must be arithmetic or std::array");
}
return result;
}
/**
* @brief
* @param data:
* @param byteArray:
*/
template <typename T>
void setByteArray(eprosima::fastcdr::optional<T> &data, const XNByteArray &byteArray)
{
if (byteArray.size() < getTypeSize<T>())
return;
if constexpr (std::is_arithmetic_v<T>) {
T temp;
std::memcpy(&temp, byteArray.data(), sizeof(T));
data = temp;
} else if constexpr (is_std_array_v<T>) {
if (!data) {
data = T{};
}
setByteArrayFromStdArray(data.value(), byteArray);
} else {
static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
"Type must be arithmetic or std::array");
} }
} }
/**
* @brief
* @param data:
* @return
*/
template <typename T, std::size_t N> template <typename T, std::size_t N>
void getByteArrayFromStdArray(std::array<T, N> data, uint8_t *buffer, size_t bufferSize) XNByteArray getByteArrayFromStdArray(const std::array<T, N> &data)
{ {
if (bufferSize < getTypeSize<T>() * N) XNByteArray result(getTypeSize<T>() * N);
for (std::size_t i = 0; i < N; ++i) {
if constexpr (std::is_arithmetic_v<T>) {
std::memcpy(result.data() + i * getTypeSize<T>(), &data[i], getTypeSize<T>());
} else {
XNByteArray subArray = getByteArrayFromStdArray(data[i]);
std::memcpy(result.data() + i * getTypeSize<T>(), subArray.data(),
getTypeSize<T>());
}
}
return result;
}
/**
* @brief
* @param data:
* @param byteArray:
*/
template <typename T, std::size_t N>
void setByteArrayFromStdArray(std::array<T, N> &data, const XNByteArray &byteArray)
{
if (byteArray.size() < getTypeSize<T>() * N)
return; return;
for (std::size_t i = 0; i < N; ++i) { for (std::size_t i = 0; i < N; ++i) {
if constexpr (std::is_arithmetic_v<T>) { if constexpr (std::is_arithmetic_v<T>) {
std::memcpy(buffer + i * getTypeSize<T>(), &data[i], getTypeSize<T>()); std::memcpy(&data[i], byteArray.data() + i * getTypeSize<T>(), getTypeSize<T>());
} else { } else {
getByteArrayFromStdArray(data[i], buffer + i * getTypeSize<T>(), getTypeSize<T>()); XNByteArray subArray(byteArray.data() + i * getTypeSize<T>(), getTypeSize<T>());
setByteArrayFromStdArray(data[i], subArray);
} }
} }
} }
/**
* @brief JSON前端
* @param data:
* @return:
*/
template <typename T> template <typename T>
std::string getString(eprosima::fastcdr::optional<T> data) std::string getString(eprosima::fastcdr::optional<T> data)
{ {
@ -206,6 +164,11 @@ protected:
return std::string(); return std::string();
} }
/**
* @brief JSON前端
* @param data:
* @return:
*/
template <typename T, std::size_t N> template <typename T, std::size_t N>
std::string getStringFromStdArray(std::array<T, N> data) std::string getStringFromStdArray(std::array<T, N> data)
{ {
@ -222,24 +185,35 @@ protected:
return ss.str(); return ss.str();
} }
private:
/**
* @brief
* @param varName:
* @return:
*/
std::string getData(const std::string &varName);
protected: protected:
struct ByteArrayFunc { struct ByteArrayFunc {
std::function<void(uint8_t *, size_t)> func; std::function<XNByteArray(void)> func;
size_t size; size_t size;
}; };
std::unordered_map<std::string, std::function<std::string()>> getDataFunction; std::unordered_map<std::string, std::function<std::string()>> getDataFunction;
std::vector<ByteArrayFunc> getByteArrayFunction; std::vector<ByteArrayFunc> getByteArrayFunction;
std::unordered_map<std::string, std::function<void(XNByteArray)>> setByteArrayFunction;
std::mutex mutex; std::mutex mutex;
uint8_t header[5]{}; // 固定大小的头部 uint8_t header[5]{}; // 固定大小的头部
size_t headerSize = 0; size_t headerSize = 0;
}; };
#define MAP_GET_DATA_FUNC(NAME) \ #define MAP_DATA_FUNC(NAME) \
getDataFunction[#NAME] = [this]() { return getString(data.NAME()); }; \ getDataFunction[#NAME] = [this]() { return getString(data.NAME()); }; \
getByteArrayFunction.push_back( \ getByteArrayFunction.push_back( \
{[this](uint8_t *buffer, size_t size) { getByteArray(data.NAME(), buffer, size); }, \ {[this]() { return getByteArray(data.NAME()); }, getTypeSize<decltype(data.NAME())>()}); \
getTypeSize<decltype(data.NAME())>()}) setByteArrayFunction[#NAME] = [this](XNByteArray byteArray) { \
setByteArray(data.NAME(), byteArray); \
}
#define ASSIGN_VALUE_GET(NAME) \ #define ASSIGN_VALUE_GET(NAME) \
if (data.NAME()) { \ if (data.NAME()) { \

View File

@ -38,6 +38,9 @@ XNModelManager::XNModelManager(PrivateType *p) : XNBaseFrameObject(p)
bool XNModelManager::PrepareForExecute() bool XNModelManager::PrepareForExecute()
{ {
T_D(); T_D();
for (auto &model : d->ModelMap) {
model.second->PrepareForExecute();
}
d->_status = XNFrameObjectStatus::Ready; d->_status = XNFrameObjectStatus::Ready;
LOG_INFO("XNModelManager is prepared!"); LOG_INFO("XNModelManager is prepared!");
return true; return true;
@ -72,7 +75,8 @@ void XNModelManager::LoadModel(const std::string &modelPath, const std::string &
model->SetUniqueId(modelID); model->SetUniqueId(modelID);
model->SetObjectName(className); model->SetObjectName(className);
model->SetFramework(GetFramework()); model->SetFramework(GetFramework());
model->SetInitializeType(initialType);
model->SetThreadID(threadID);
// 使用std::filesystem处理路径 // 使用std::filesystem处理路径
std::filesystem::path configPath = std::filesystem::path configPath =
std::filesystem::path(modelPath).parent_path() / (className + ".mcfg"); std::filesystem::path(modelPath).parent_path() / (className + ".mcfg");
@ -82,7 +86,7 @@ void XNModelManager::LoadModel(const std::string &modelPath, const std::string &
d->ModelMap[modelID] = model; d->ModelMap[modelID] = model;
// 初始化模型 // 初始化模型
model->Initialize(initialType, threadID); model->Initialize();
// 注册到线程管理器 // 注册到线程管理器
if (threadID != 0) { if (threadID != 0) {
@ -90,7 +94,12 @@ void XNModelManager::LoadModel(const std::string &modelPath, const std::string &
if (framework) { if (framework) {
framework->GetThreadManager()->RegisterFunction( framework->GetThreadManager()->RegisterFunction(
modelID, std::bind(&XNModelObject::StepUpdate, model.get()), threadID, modelID, std::bind(&XNModelObject::StepUpdate, model.get()), threadID,
0, 0, 0); model->GetRunFreq(), model->GetRunNode(), model->GetRunPriority());
// 设置模型设置频率
double threadFreq =
framework->GetThreadManager()->GetThreadFreqByID(threadID);
double modelSetFreq = threadFreq / (double)(1 << model->GetRunFreq());
model->SetSetFreq(modelSetFreq);
} }
} }
} else { } else {

View File

@ -126,12 +126,78 @@ void XNModelObject::SetVersion(const std::string &sVersion)
d->_sVersion = sVersion; d->_sVersion = sVersion;
} }
void XNModelObject::SetInitializeType(uint32_t initialType)
{
T_D();
d->_initialType = initialType;
}
void XNModelObject::SetThreadID(uint32_t threadID)
{
T_D();
d->_threadID = threadID;
}
uint32_t XNModelObject::GetThreadID() const
{
T_D();
return d->_threadID;
}
uint32_t XNModelObject::GetRunFreq() const
{
T_D();
return d->_runFreq;
}
void XNModelObject::SetRunFreq(uint32_t runFreq)
{
T_D();
d->_runFreq = runFreq;
}
uint32_t XNModelObject::GetRunNode() const
{
T_D();
return d->_runNode;
}
void XNModelObject::SetRunNode(uint32_t runNode)
{
T_D();
d->_runNode = runNode;
}
uint32_t XNModelObject::GetRunPriority() const
{
T_D();
return d->_runPriority;
}
void XNModelObject::SetRunPriority(uint32_t runPriority)
{
T_D();
d->_runPriority = runPriority;
}
double XNModelObject::GetSetFreq() const
{
T_D();
return d->_setFreq;
}
void XNModelObject::SetSetFreq(double setFreq)
{
T_D();
d->_setFreq = setFreq;
}
// 初始化函数 // 初始化函数
void XNModelObject::Initialize(uint32_t initialType, uint32_t threadID) void XNModelObject::Initialize()
{ {
// 先尝试调取动态库 // 先尝试调取动态库
T_D(); T_D();
if (initialType == 0) { if (d->_initialType == 0) {
// 读取配置文件,设置循环执行函数 // 读取配置文件,设置循环执行函数
std::ifstream file(GetXmlPath()); std::ifstream file(GetXmlPath());
if (!file.is_open()) { if (!file.is_open()) {
@ -179,7 +245,6 @@ void XNModelObject::Initialize(uint32_t initialType, uint32_t threadID)
// 使用标准C++字符串处理 // 使用标准C++字符串处理
d->_runFreq = std::stoi(funcNode.substr(0, tmp)); d->_runFreq = std::stoi(funcNode.substr(0, tmp));
d->_setFreq = d->_setFreq / std::pow(2.0, d->_runFreq);
d->_runNode = std::stoi(funcNode.substr(tmp + 1)); d->_runNode = std::stoi(funcNode.substr(tmp + 1));
// 注册周期性函数 // 注册周期性函数
@ -188,7 +253,7 @@ void XNModelObject::Initialize(uint32_t initialType, uint32_t threadID)
auto threadManager = framework->GetThreadManager(); auto threadManager = framework->GetThreadManager();
if (threadManager) { if (threadManager) {
threadManager->RegisterFunction( threadManager->RegisterFunction(
GetUniqueId(), std::bind(&XNModelObject::StepUpdate, this), threadID, GetUniqueId(), std::bind(&XNModelObject::StepUpdate, this), d->_threadID,
d->_runFreq, d->_runNode, d->_runPriority); d->_runFreq, d->_runNode, d->_runPriority);
} }
} }
@ -284,9 +349,8 @@ void XNModelObject::RegisterDDSParticipant()
XN_PUBLISHTOPIC(XNSim::XNSimStatus::XNModelStatus); XN_PUBLISHTOPIC(XNSim::XNSimStatus::XNModelStatus);
} }
int XNModelObject::RegisterEventHandler(const std::string &eventName, int XNModelObject::RegisterEventHandler(const std::string &eventName, XNEventCallback callback,
std::function<void(const std::any &)> callback, bool async, bool async, XNEvent::Priority priority)
XNEvent::Priority priority)
{ {
// 获取事件管理器 // 获取事件管理器
auto framework = GetFramework(); auto framework = GetFramework();

View File

@ -121,6 +121,54 @@ public:
*/ */
void SetVersion(const std::string &sVersion); void SetVersion(const std::string &sVersion);
/**
* @brief
* @return uint32_t:
*/
uint32_t GetRunFreq() const;
/**
* @brief
* @param runFreq: uint32_t类型
*/
void SetRunFreq(uint32_t runFreq);
/**
* @brief
* @return uint32_t:
*/
uint32_t GetRunNode() const;
/**
* @brief
* @param runNode: uint32_t类型
*/
void SetRunNode(uint32_t runNode);
/**
* @brief
* @return uint32_t:
*/
uint32_t GetRunPriority() const;
/**
* @brief
* @param runPriority: uint32_t类型
*/
void SetRunPriority(uint32_t runPriority);
/**
* @brief
* @return double:
*/
double GetSetFreq() const;
/**
* @brief
* @param setFreq: double类型
*/
void SetSetFreq(double setFreq);
/** /**
* @brief * @brief
* @details * @details
@ -169,12 +217,18 @@ public:
TriggerEvent(eventName, eventData, true, XNEvent::Priority::RealTime); TriggerEvent(eventName, eventData, true, XNEvent::Priority::RealTime);
} }
void SetInitializeType(uint32_t initialType);
void SetThreadID(uint32_t threadID);
uint32_t GetThreadID() const;
/** /**
* @brief * @brief
* @details * @details
* 使AddMyFunction方法注册需要被线程调用的函数 * 使AddMyFunction方法注册需要被线程调用的函数
*/ */
virtual void Initialize(uint32_t initialType, uint32_t threadID); virtual void Initialize();
/** /**
* @brief 仿 * @brief 仿

View File

@ -84,6 +84,11 @@ struct XNModelObjectPrivate : public XNObjectPrivate {
*/ */
double _setFreq; double _setFreq;
/**
* @brief
*/
uint32_t _initialType;
/** /**
* @brief 线ID * @brief 线ID
*/ */

View File

@ -29,6 +29,9 @@ bool XNServiceManager::Initialize()
bool XNServiceManager::PrepareForExecute() bool XNServiceManager::PrepareForExecute()
{ {
T_D(); T_D();
for (auto &service : d->ServiceList) {
service.second->PrepareForExecute();
}
d->_status = XNFrameObjectStatus::Ready; d->_status = XNFrameObjectStatus::Ready;
LOG_INFO("XNServiceManager is prepared!"); LOG_INFO("XNServiceManager is prepared!");
return true; return true;

View File

@ -271,7 +271,8 @@ const double &XNThread::GetRunFrequecy()
void XNThread::SetRunFrequecy(const double &dRunFrequecy) void XNThread::SetRunFrequecy(const double &dRunFrequecy)
{ {
T_D(); T_D();
d->_setFreq = dRunFrequecy; d->_setFreq = dRunFrequecy;
d->pinfo.period_ns = 1.0E9 / dRunFrequecy;
} }
// 获取线程运行优先级 // 获取线程运行优先级

View File

@ -225,3 +225,15 @@ uint32_t XNThreadManager::AllocateThreadID()
d->threadList[threadID] = nullptr; d->threadList[threadID] = nullptr;
return threadID; return threadID;
} }
void XNThreadManager::SetThreadFreqByID(uint32_t threadID, double freq)
{
T_D();
d->threadList[threadID]->SetRunFrequecy(freq);
}
double XNThreadManager::GetThreadFreqByID(uint32_t threadID)
{
T_D();
return d->threadList[threadID]->GetRunFrequecy();
}

View File

@ -81,6 +81,22 @@ public:
*/ */
virtual bool PrepareForExecute() override; virtual bool PrepareForExecute() override;
/**
* @brief 线
* @param threadID: UINT32类型线ID
* @param freq: double类型线
* @details 线
*/
void SetThreadFreqByID(uint32_t threadID, double freq);
/**
* @brief 线
* @param threadID: UINT32类型线ID
* @return double: 线
* @details 线
*/
double GetThreadFreqByID(uint32_t threadID);
/** /**
* @brief 仿 * @brief 仿
* @param objectId: ID * @param objectId: ID

28
XNCore/XNTypeTraits.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include <array>
#include <type_traits>
#include <cstddef>
template <typename T>
struct is_std_array : std::false_type {
};
template <typename T, std::size_t N>
struct is_std_array<std::array<T, N>> : std::true_type {
};
// 变量模板简化使用
template <typename T>
inline constexpr bool is_std_array_v = is_std_array<T>::value;
// 获取类型大小的辅助函数
template <typename T>
constexpr size_t getTypeSize()
{
if constexpr (is_std_array_v<T>) {
// 对于std::array计算所有元素的总大小
return getTypeSize<typename T::value_type>() * std::tuple_size<T>::value;
} else {
return sizeof(T);
}
}

View File

@ -7,6 +7,73 @@
"*.ipp": "cpp", "*.ipp": "cpp",
"*.tcc": "cpp", "*.tcc": "cpp",
"optional": "cpp", "optional": "cpp",
"type_traits": "cpp" "type_traits": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"any": "cpp",
"array": "cpp",
"atomic": "cpp",
"bit": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"codecvt": "cpp",
"compare": "cpp",
"complex": "cpp",
"concepts": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"list": "cpp",
"map": "cpp",
"set": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"random": "cpp",
"ratio": "cpp",
"regex": "cpp",
"string": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"utility": "cpp",
"fstream": "cpp",
"future": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"ostream": "cpp",
"ranges": "cpp",
"span": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"stop_token": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp"
} }
} }

View File

@ -18,7 +18,7 @@ protected:
XNAerodynamics(PrivateType *p); XNAerodynamics(PrivateType *p);
public: public:
virtual void Initialize(uint32_t initialType, uint32_t threadID) override; virtual void Initialize() override;
virtual void PrepareForExecute() override; virtual void PrepareForExecute() override;
public: public:

View File

@ -3,13 +3,12 @@
#include <XNCore/XNModelObject.h> #include <XNCore/XNModelObject.h>
#include "../XNGroundHandlingInterface/XNGroundHandlingInterface.hpp" #include "../XNGroundHandlingInterface/XNGroundHandlingInterface.hpp"
class XNGroundHandlingPrivate; struct XNGroundHandlingPrivate;
class XNGROUNDHANDLING_EXPORT XNGroundHandling : public XNModelObject class XNGROUNDHANDLING_EXPORT XNGroundHandling : public XNModelObject
{ {
Q_OBJECT XN_METATYPE(XNGroundHandling, XNModelObject)
Q_DISABLE_COPY(XNGroundHandling) XN_DECLARE_PRIVATE(XNGroundHandling)
Q_DECLARE_PRIVATE(XNGroundHandling)
XN_DECLARE_DDS() XN_DECLARE_DDS()
public: public:
explicit XNGroundHandling(QObject *parent = nullptr); explicit XNGroundHandling(QObject *parent = nullptr);
@ -18,9 +17,9 @@ public:
protected: protected:
XNGroundHandling(XNGroundHandlingPrivate &dd, QObject *parent = nullptr); XNGroundHandling(XNGroundHandlingPrivate &dd, QObject *parent = nullptr);
public slots: public:
virtual void OnInitialize() override; virtual void Initialize() override;
virtual void OnPrepareForExecute() override; virtual void PrepareForExecute() override;
public: public:
virtual void StepUpdate() override; virtual void StepUpdate() override;

View File

@ -1,20 +1,15 @@
#pragma once #pragma once
#include <XNCore/XNModelObject_p.h> #include <XNCore/XNModelObject_p.h>
#include <DataModels/libSACSCGroundHandling_V2.0.27.1H/std_04_dll.h> #include <DataModels/libSACSCGroundHandling_V2.0.27.1H/std_04_dll.h>
#include <QMutex>
typedef void (*FunctionType)(ComacDataStructure_S *); typedef void (*FunctionType)(ComacDataStructure_S *);
class XNGroundHandlingPrivate : public XNModelObjectPrivate struct XNGroundHandlingPrivate : public XNModelObjectPrivate {
{
public:
Q_DECLARE_PUBLIC(XNGroundHandling)
XNGroundHandlingPrivate(XNGroundHandling *q) : XNModelObjectPrivate(q) {}
FunctionType _fun = nullptr; FunctionType _fun = nullptr;
ComacDataStructure_S _data; ComacDataStructure_S _data;
QMutex _mutex; std::string _entryPointName = "_Z29SACSCGroundHandlingEntryPointP20ComacDataStructure_S";
std::mutex _mutex;
}; };