XNSim/XNCore/XNDDSManager.h

#pragma once
#include "XNBaseFrameObject.h"

#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
#include <fastdds/dds/publisher/DataWriter.hpp>
#include <fastdds/dds/publisher/Publisher.hpp>
#include <fastdds/dds/subscriber/DataReader.hpp>
#include <fastdds/dds/subscriber/Subscriber.hpp>
#include <fastdds/dds/topic/TypeSupport.hpp>
#include <fastdds/dds/subscriber/DataReaderListener.hpp>

struct PublisherInfo {
	FAST_DDS_MACRO::Publisher *publisher;
	FAST_DDS_MACRO::DataWriter *dataWriter;
};

struct SubscriberInfo {
	FAST_DDS_MACRO::Subscriber *subscriber;
	FAST_DDS_MACRO::DataReader *dataReader;
};

struct TopicInfo {
	FAST_DDS_MACRO::Topic *topic;
	QMap<quint32, PublisherInfo> publishers_;
	QMap<quint32, SubscriberInfo> subscribers_;
};

template <typename T>
class DataReaderListenerImpl : public FAST_DDS_MACRO::DataReaderListener
{
public:
	DataReaderListenerImpl(std::function<void(const T &)> callback) : callback_(callback) {}

	void on_data_available(FAST_DDS_MACRO::DataReader *reader) override
	{
		FAST_DDS_MACRO::SampleInfo info;
		if (reader->take_next_sample(&data_, &info) == FAST_DDS_MACRO::RETCODE_OK
			&& info.valid_data) {
			callback_(data_);
		}
	}

private:
	T data_;
	std::function<void(const T &)> callback_;
};

class XNDDSManagerPrivate;

class XNDDSManager : public XNBaseFrameObject
{
	Q_OBJECT
	Q_DECLARE_PRIVATE(XNDDSManager)
	Q_DISABLE_COPY(XNDDSManager)
public:
	explicit XNDDSManager(QObject *parent = nullptr);
	~XNDDSManager();

protected:
	XNDDSManager(XNDDSManagerPrivate &dd, QObject *parent = nullptr);

public slots:
	virtual void OnInitialize() override;

	virtual void OnPrepareForExecute() override;

	void SetDomainID(quint32 domainID);

public:
	template <typename T>
	FAST_DDS_MACRO::DataWriter *RegisterPublisher(const QString &topicName, quint32 publisherID)
	{
		std::lock_guard<std::mutex> lock(mutex_);
		if (!topics_.contains(topicName)) {
			topics_[topicName] = TopicInfo();
			TopicInfo &tmp	   = topics_[topicName];
			FAST_DDS_MACRO::TypeSupport typeSupport(new T());
			typeSupport.register_type(participant_);
			tmp.topic =
				participant_->create_topic(topicName.toStdString(), typeSupport.get_type_name(),
										   FAST_DDS_MACRO::TOPIC_QOS_DEFAULT);
			if (tmp.topic == nullptr) {
				LOG_ERROR("0x2130 Create Topic %1 Failed!", topicName);
				topics_.remove(topicName);
				return nullptr;
			}
		}
		TopicInfo &tmp				 = topics_[topicName];
		tmp.publishers_[publisherID] = PublisherInfo();
		tmp.publishers_[publisherID].publisher =
			participant_->create_publisher(FAST_DDS_MACRO::PUBLISHER_QOS_DEFAULT, nullptr);
		if (tmp.publishers_[publisherID].publisher == nullptr) {
			LOG_ERROR("0x2131 Create Publisher %1 for Topic %2 Failed!", publisherID, topicName);
			return nullptr;
		}

		// 设置数据写入器QoS策略
		FAST_DDS_MACRO::DataWriterQos dataWriterQos;
		// 设置数据写入器的历史记录策略, 只保留最新的一个数据
		// dataWriterQos.history().kind  = FAST_DDS_MACRO::KEEP_LAST_HISTORY_QOS;
		// dataWriterQos.history().depth = 1;
		// 设置数据写入器的可靠性策略, 使用最佳努力可靠性
		// dataWriterQos.reliability().kind = FAST_DDS_MACRO::BEST_EFFORT_RELIABILITY_QOS;
		// 设置数据写入器的持久性策略, 使用瞬态本地持久性
		dataWriterQos.durability().kind = FAST_DDS_MACRO::VOLATILE_DURABILITY_QOS;
		// 设置数据写入器的生命周期策略, 设置为5秒
		dataWriterQos.lifespan().duration = FAST_DDS_MACRO::Duration_t(5, 0);

		tmp.publishers_[publisherID].dataWriter =
			tmp.publishers_[publisherID].publisher->create_datawriter(tmp.topic, dataWriterQos,
																	  nullptr);
		if (tmp.publishers_[publisherID].dataWriter == nullptr) {
			LOG_ERROR("0x2132 Create DataWriter %1 for Topic %2 Failed!", publisherID, topicName);
			return nullptr;
		}
		LOG_INFO("0x2133 Create Publisher %1 for Topic %2 Success!", publisherID, topicName);
		return tmp.publishers_[publisherID].dataWriter;
	}

	template <typename T>
	void RegisterSubscriber(const QString &topicName, quint32 subscriberID,
							std::function<void(const typename T::type &)> fun)
	{
		Q_D(XNDDSManager);
		std::lock_guard<std::mutex> lock(mutex_);
		if (!topics_.contains(topicName)) {
			topics_[topicName] = TopicInfo();
			TopicInfo &tmp	   = topics_[topicName];
			FAST_DDS_MACRO::TypeSupport typeSupport(new T());
			typeSupport.register_type(participant_);
			tmp.topic =
				participant_->create_topic(topicName.toStdString(), typeSupport.get_type_name(),
										   FAST_DDS_MACRO::TOPIC_QOS_DEFAULT);
			if (tmp.topic == nullptr) {
				LOG_ERROR("0x2130 Create Topic %1 Failed!", topicName);
				topics_.remove(topicName);
				return;
			}
		}
		TopicInfo &tmp				   = topics_[topicName];
		tmp.subscribers_[subscriberID] = SubscriberInfo();
		tmp.subscribers_[subscriberID].subscriber =
			participant_->create_subscriber(FAST_DDS_MACRO::SUBSCRIBER_QOS_DEFAULT, nullptr);
		if (tmp.subscribers_[subscriberID].subscriber == nullptr) {
			LOG_ERROR("0x2135 Create Subscriber %1 for Topic %2 Failed!", subscriberID, topicName);
		}
		FAST_DDS_MACRO::DataReaderQos dataReaderQos;
		dataReaderQos.durability().kind = FAST_DDS_MACRO::VOLATILE_DURABILITY_QOS;
		FAST_DDS_MACRO::DataReaderListener *listener =
			new DataReaderListenerImpl<typename T::type>(fun);
		tmp.subscribers_[subscriberID].dataReader =
			tmp.subscribers_[subscriberID].subscriber->create_datareader(tmp.topic, dataReaderQos,
																		 listener);
		if (tmp.subscribers_[subscriberID].dataReader == nullptr) {
			LOG_ERROR("0x2136 Create DataReader %1 for Topic %2 Failed!", subscriberID, topicName);
		}
		LOG_INFO("0x2137 Create Subscriber %1 for Topic %2 Success!", subscriberID, topicName);
	}

private:
	FAST_DDS_MACRO::DomainParticipant *participant_;
	QMap<QString, TopicInfo> topics_;
	std::mutex mutex_;
};