init

unitree_SDK/include/unitree/common/dds/dds_callback.hpp

@@ -0,0 +1,35 @@
+#ifndef __UT_DDS_CALLBACK_HPP__
+#define __UT_DDS_CALLBACK_HPP__
+
+#include <unitree/common/decl.hpp>
+
+using DdsMessageHandler = std::function<void(const void*)>;
+
+namespace unitree
+{
+namespace common
+{
+class DdsReaderCallback
+{
+public:
+    DdsReaderCallback();
+    DdsReaderCallback(const DdsMessageHandler& handler);
+    DdsReaderCallback(const DdsReaderCallback& cb);
+    DdsReaderCallback& operator=(const DdsReaderCallback& cb);
+
+    ~DdsReaderCallback();
+
+public:
+    bool HasMessageHandler() const;
+    void OnDataAvailable(const void* message);
+
+private:
+    DdsMessageHandler mMessageHandler;
+};
+
+using DdsReaderCallbackPtr = std::shared_ptr<DdsReaderCallback>;
+
+}
+}
+
+#endif//__UT_DDS_CALLBACK_HPP__

unitree_SDK/include/unitree/common/dds/dds_easy_model.hpp

@@ -0,0 +1,148 @@
+#ifndef __DDS_EASY_MODEL_HPP__
+#define __DDS_EASY_MODEL_HPP__
+
+#include <unitree/common/dds/dds_topic_channel.hpp>
+#include <unitree/common/dds/dds_parameter.hpp>
+#include <unitree/common/dds/dds_qos_realize.hpp>
+#include <unitree/common/string_tool.hpp>
+
+#define UT_DDS_PARAMETER_CONFIG_FILENAME "dds_parameter.json"
+
+namespace unitree
+{
+namespace common
+{
+class DdsEasyModel
+{
+public:
+    explicit DdsEasyModel();
+    ~DdsEasyModel();
+
+    void Init(uint32_t domainId);
+    void Init(const std::string& ddsParameterFileName = "");
+    void Init(const JsonMap& param);
+
+    template<typename MSG>
+    void SetTopic(const std::string& topic)
+    {
+        DdsTopicChannelPtr<MSG> channel = GetChannel<MSG>(topic);
+        if (!channel)
+        {
+            channel = DdsTopicChannelPtr<MSG>(new DdsTopicChannel<MSG>());
+            mChannelMap[topic] = std::static_pointer_cast<DdsTopicChannelAbstract>(channel);
+
+            DdsTopicQos topicQos;
+            GetTopicQos(topic, topicQos);
+            channel->SetTopic(mParticipant, topic, topicQos);
+        }
+
+        DdsWriterPtr<MSG> writer = channel->GetWriter();
+        if (!writer)
+        {
+            DdsWriterQos writerQos;
+            GetWriterQos(topic, writerQos);
+            channel->SetWriter(GetPublisher(topic), writerQos);
+        }
+        else
+        {
+            UT_THROW(CommonException, std::string("topic reader is already exist. topic:") + topic);
+        }
+    }
+
+    template<typename MSG>
+    void SetTopic(const std::string& topic, const DdsMessageHandler& handler, int32_t queuelen = 0)
+    {
+        DdsReaderCallback cb(handler);
+        SetTopic<MSG>(topic, cb, queuelen);
+    }
+
+    template<typename MSG>
+    void SetTopic(const std::string& topic, const DdsReaderCallback& rcb, int32_t queuelen = 0)
+    {
+        DdsTopicChannelPtr<MSG> channel = GetChannel<MSG>(topic);
+        if (!channel)
+        {
+            channel = DdsTopicChannelPtr<MSG>(new DdsTopicChannel<MSG>());
+            mChannelMap[topic] = std::static_pointer_cast<DdsTopicChannelAbstract>(channel);
+
+            DdsTopicQos topicQos;
+            GetTopicQos(topic, topicQos);
+            channel->SetTopic(mParticipant, topic, topicQos);
+        }
+
+        DdsReaderPtr<MSG> reader = channel->GetReader();
+        if (!reader)
+        {
+            DdsReaderQos readerQos;
+            GetReaderQos(topic, readerQos);
+            channel->SetReader(GetSubscriber(topic), readerQos, rcb, queuelen);
+        }
+        else
+        {
+            UT_THROW(CommonException, std::string("topic reader is already exist. topic:") + topic);
+        }
+    }
+
+    template<typename MSG>
+    bool WriteMessage(const std::string topic, const MSG& message, int64_t waitMicrosec = 0)
+    {
+        DdsTopicChannelPtr<MSG> channel = GetChannel<MSG>(topic);
+        if (channel == NULL)
+        {
+            return false;
+        }
+
+        return channel->Write(message, waitMicrosec);
+    }
+
+    bool WriteMessage(const std::string topic, const void* message, int64_t waitMicrosec = 0);
+
+    int64_t GetLastDataAvailableTime(const std::string topic);
+
+private:
+    void GetTopicQos(const std::string& topic, DdsTopicQos& qos);
+    void GetWriterQos(const std::string& topic, DdsWriterQos& qos);
+    void GetReaderQos(const std::string& topic, DdsReaderQos& qos);
+
+    DdsTopicChannelAbstractPtr GetChannel(const std::string& topic);
+
+    template<typename MSG>
+    DdsTopicChannelPtr<MSG> GetChannel(const std::string& topic)
+    {
+        DdsTopicChannelPtr<MSG> channel;
+
+        DdsTopicChannelAbstractPtr channelAbstract = GetChannel(topic);
+        if (channelAbstract)
+        {
+            channel = std::static_pointer_cast<DdsTopicChannel<MSG>>(channelAbstract);
+        }
+
+        return channel;
+    }
+
+    DdsSubscriberPtr GetSubscriber(const std::string& topic);
+    DdsSubscriberPtr GetSubscriberDefault();
+
+    DdsPublisherPtr GetPublisher(const std::string& topic);
+    DdsPublisherPtr GetPublisherDefault();
+
+private:
+    DdsParameter mDdsParameter;
+
+    DdsParticipantPtr mParticipant;
+    std::vector<DdsPublisherPtr> mPublisherList;
+    std::vector<DdsSubscriberPtr> mSubscriberList;
+    DdsPublisherPtr mPublisherDefault;
+    DdsSubscriberPtr mSubscriberDefault;
+
+    std::map<std::string,DdsTopicChannelAbstractPtr> mChannelMap;
+
+    Logger *mLogger;
+};
+
+using DdsEasyModelPtr = std::shared_ptr<DdsEasyModel>;
+
+}
+}
+
+#endif//__DDS_EASY_MODEL_HPP__

unitree_SDK/include/unitree/common/dds/dds_entity.hpp

@@ -0,0 +1,411 @@
+#ifndef __UT_DDS_ENTITY_HPP__
+#define __UT_DDS_ENTITY_HPP__
+
+#include <dds/dds.hpp>
+#include <unitree/common/log/log.hpp>
+#include <unitree/common/block_queue.hpp>
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <unitree/common/time/sleep.hpp>
+#include <unitree/common/dds/dds_exception.hpp>
+#include <unitree/common/dds/dds_callback.hpp>
+#include <unitree/common/dds/dds_qos.hpp>
+#include <unitree/common/dds/dds_traits.hpp>
+
+#define __UT_DDS_NULL__ ::dds::core::null
+
+/*
+ * dds wait sub/pub matched default time slice.
+ * default 10000 us
+ */
+#define __UT_DDS_WAIT_MATCHED_TIME_SLICE 10000
+#define __UT_DDS_WAIT_MATCHED_TIME_MAX   1000000
+
+using namespace org::eclipse::cyclonedds;
+
+namespace unitree
+{
+namespace common
+{
+class DdsLogger
+{
+public:
+    DdsLogger();
+    virtual ~DdsLogger();
+
+protected:
+    Logger* mLogger;
+};
+
+/*
+ * @brief: DdsParticipant
+ */
+class DdsParticipant : public DdsLogger
+{
+public:
+    using NATIVE_TYPE = ::dds::domain::DomainParticipant;
+
+    explicit DdsParticipant(uint32_t domainId, const DdsParticipantQos& qos, const std::string& config = "");
+    ~DdsParticipant();
+
+    const NATIVE_TYPE& GetNative() const;
+
+private:
+    NATIVE_TYPE mNative;
+};
+
+using DdsParticipantPtr = std::shared_ptr<DdsParticipant>;
+
+
+/*
+ * @brief: DdsPublisher
+ */
+class DdsPublisher : public DdsLogger
+{
+public:
+    using NATIVE_TYPE = ::dds::pub::Publisher;
+
+    explicit DdsPublisher(const DdsParticipantPtr& participant, const DdsPublisherQos& qos);
+    ~DdsPublisher();
+
+    const NATIVE_TYPE& GetNative() const;
+
+private:
+    NATIVE_TYPE mNative;
+};
+
+using DdsPublisherPtr = std::shared_ptr<DdsPublisher>;
+
+
+/*
+ * @brief: DdsSubscriber
+ */
+class DdsSubscriber : public DdsLogger
+{
+public:
+    using NATIVE_TYPE = ::dds::sub::Subscriber;
+
+    explicit DdsSubscriber(const DdsParticipantPtr& participant, const DdsSubscriberQos& qos);
+    ~DdsSubscriber();
+
+    const NATIVE_TYPE& GetNative() const;
+
+private:
+    NATIVE_TYPE mNative;
+};
+
+using DdsSubscriberPtr = std::shared_ptr<DdsSubscriber>;
+
+
+/*
+ * @brief: DdsTopic
+ */
+template<typename MSG>
+class DdsTopic : public DdsLogger
+{
+public:
+    using NATIVE_TYPE = ::dds::topic::Topic<MSG>;
+
+    explicit DdsTopic(const DdsParticipantPtr& participant, const std::string& name, const DdsTopicQos& qos) :
+        mNative(__UT_DDS_NULL__)
+    {
+        UT_DDS_EXCEPTION_TRY
+
+        auto topicQos = participant->GetNative().default_topic_qos();
+        qos.CopyToNativeQos(topicQos);
+
+        mNative = NATIVE_TYPE(participant->GetNative(), name, topicQos);
+
+        UT_DDS_EXCEPTION_CATCH(mLogger, true)
+    }
+
+    ~DdsTopic()
+    {
+        mNative = __UT_DDS_NULL__;
+    }
+
+    const NATIVE_TYPE& GetNative() const
+    {
+        return mNative;
+    }
+
+private:
+    NATIVE_TYPE mNative;
+};
+
+template<typename MSG>
+using DdsTopicPtr = std::shared_ptr<DdsTopic<MSG>>;
+
+
+/*
+ * @brief: DdsWriter
+ */
+template<typename MSG>
+class DdsWriter : public DdsLogger
+{
+public:
+    using NATIVE_TYPE = ::dds::pub::DataWriter<MSG>;
+
+    explicit DdsWriter(const DdsPublisherPtr publisher, const DdsTopicPtr<MSG>& topic, const DdsWriterQos& qos) :
+        mNative(__UT_DDS_NULL__)
+    {
+        UT_DDS_EXCEPTION_TRY
+
+        auto writerQos = publisher->GetNative().default_datawriter_qos();
+        qos.CopyToNativeQos(writerQos);
+
+        mNative = NATIVE_TYPE(publisher->GetNative(), topic->GetNative(), writerQos);
+
+        UT_DDS_EXCEPTION_CATCH(mLogger, true)
+    }
+
+    ~DdsWriter()
+    {
+        mNative = __UT_DDS_NULL__;
+    }
+
+    const NATIVE_TYPE& GetNative() const
+    {
+        return mNative;
+    }
+
+    bool Write(const MSG& message, int64_t waitMicrosec)
+    {
+        if (waitMicrosec > 0)
+        {
+            WaitReader(waitMicrosec);
+        }
+
+        UT_DDS_EXCEPTION_TRY
+        {
+            mNative.write(message);
+            return true;
+        }
+        UT_DDS_EXCEPTION_CATCH(mLogger, false)
+
+        return false;
+    }
+
+private:
+    void WaitReader(int64_t waitMicrosec)
+    {
+        if (waitMicrosec < __UT_DDS_WAIT_MATCHED_TIME_SLICE)
+        {
+            return;
+        }
+
+        int64_t waitTime = (waitMicrosec / 2);
+        if (waitTime > __UT_DDS_WAIT_MATCHED_TIME_MAX)
+        {
+            waitTime = __UT_DDS_WAIT_MATCHED_TIME_MAX;
+        }
+
+        while (waitTime > 0 && mNative.publication_matched_status().current_count() == 0)
+        {
+            MicroSleep(__UT_DDS_WAIT_MATCHED_TIME_SLICE);
+            waitTime -=__UT_DDS_WAIT_MATCHED_TIME_SLICE;
+        }
+    }
+
+private:
+    NATIVE_TYPE mNative;
+};
+
+template<typename MSG>
+using DdsWriterPtr = std::shared_ptr<DdsWriter<MSG>>;
+
+
+/*
+ * @brief: DdsReaderListener
+ */
+template<typename MSG>
+class DdsReaderListener : public ::dds::sub::NoOpDataReaderListener<MSG>, DdsLogger
+{
+public:
+    using NATIVE_TYPE = ::dds::sub::DataReaderListener<MSG>;
+    using MSG_PTR = std::shared_ptr<MSG>;
+
+    explicit DdsReaderListener() :
+        mHasQueue(false), mQuit(false), mMask(::dds::core::status::StatusMask::none()), mLastDataAvailableTime(0)
+    {}
+
+    ~DdsReaderListener()
+    {
+        if (mHasQueue)
+        {
+            mQuit = true;
+            mDataQueuePtr->Interrupt(false);
+            mDataQueueThreadPtr->Wait();
+        }
+    }
+
+    void SetCallback(const DdsReaderCallback& cb)
+    {
+        if (cb.HasMessageHandler())
+        {
+            mMask |= ::dds::core::status::StatusMask::data_available();
+        }
+
+        mCallbackPtr.reset(new DdsReaderCallback(cb));
+    }
+
+    void SetQueue(int32_t len)
+    {
+        if (len <= 0)
+        {
+            return;
+        }
+
+        mHasQueue = true;
+        mDataQueuePtr.reset(new BlockQueue<MSG_PTR>(len));
+
+        auto queueThreadFunc = [this]() {
+            while (true)
+            {
+                if (mCallbackPtr && mCallbackPtr->HasMessageHandler())
+                {
+                    break;
+                }
+                else
+                {
+                    MicroSleep(__UT_DDS_WAIT_MATCHED_TIME_SLICE);
+                }
+            }
+            while (!mQuit)
+            {
+                MSG_PTR dataPtr;
+                if (mDataQueuePtr->Get(dataPtr))
+                {
+                    if (dataPtr)
+                    {
+                        mCallbackPtr->OnDataAvailable(dataPtr.get());
+                    }
+                }
+            }
+            return 0;
+        };
+
+        mDataQueueThreadPtr = CreateThreadEx("rlsnr", UT_CPU_ID_NONE, queueThreadFunc);
+    }
+
+    int64_t GetLastDataAvailableTime() const
+    {
+        return mLastDataAvailableTime;
+    }
+
+    NATIVE_TYPE* GetNative() const
+    {
+        return (NATIVE_TYPE*)this;
+    }
+
+    const ::dds::core::status::StatusMask& GetStatusMask() const
+    {
+        return mMask;
+    }
+
+private:
+    void on_data_available(::dds::sub::DataReader<MSG>& reader)
+    {
+        ::dds::sub::LoanedSamples<MSG> samples;
+        samples = reader.take();
+
+        if (samples.length() <= 0)
+        {
+            return;
+        }
+
+        typename ::dds::sub::LoanedSamples<MSG>::const_iterator iter;
+        for (iter=samples.begin(); iter<samples.end(); ++iter)
+        {
+            const MSG& m = iter->data();
+            if (iter->info().valid())
+            {
+                mLastDataAvailableTime = GetCurrentMonotonicTimeNanosecond();
+
+                if (mHasQueue)
+                {
+                    if (!mDataQueuePtr->Put(MSG_PTR(new MSG(m)), true))
+                    {
+                        LOG_WARNING(mLogger, "earliest mesage was evicted. type:", DdsGetTypeName(MSG));
+                    }
+                }
+                else
+                {
+                    mCallbackPtr->OnDataAvailable((const void*)&m);
+                }
+            }
+        }
+    }
+
+private:
+    bool mHasQueue;
+    volatile bool mQuit;
+
+    ::dds::core::status::StatusMask mMask;
+    int64_t mLastDataAvailableTime;
+
+    DdsReaderCallbackPtr mCallbackPtr;
+    BlockQueuePtr<MSG_PTR> mDataQueuePtr;
+    ThreadPtr mDataQueueThreadPtr;
+};
+
+template<typename MSG>
+using DdsReaderListenerPtr = std::shared_ptr<DdsReaderListener<MSG>>;
+
+
+/*
+ * @brief: DdsReader
+ */
+template<typename MSG>
+class DdsReader : public DdsLogger
+{
+public:
+    using NATIVE_TYPE = ::dds::sub::DataReader<MSG>;
+
+    explicit DdsReader(const DdsSubscriberPtr& subscriber, const DdsTopicPtr<MSG>& topic, const DdsReaderQos& qos) :
+        mNative(__UT_DDS_NULL__)
+    {
+        UT_DDS_EXCEPTION_TRY
+
+        auto readerQos = subscriber->GetNative().default_datareader_qos();
+        qos.CopyToNativeQos(readerQos);
+
+        mNative = NATIVE_TYPE(subscriber->GetNative(), topic->GetNative(), readerQos);
+
+        UT_DDS_EXCEPTION_CATCH(mLogger, true)
+    }
+
+    ~DdsReader()
+    {
+        mNative = __UT_DDS_NULL__;
+    }
+
+    const NATIVE_TYPE& GetNative() const
+    {
+        return mNative;
+    }
+
+    void SetListener(const DdsReaderCallback& cb, int32_t qlen)
+    {
+        mListener.SetCallback(cb);
+        mListener.SetQueue(qlen);
+        mNative.listener(mListener.GetNative(), mListener.GetStatusMask());
+    }
+
+    int64_t GetLastDataAvailableTime() const
+    {
+        return mListener.GetLastDataAvailableTime();
+    }
+
+private:
+    NATIVE_TYPE mNative;
+    DdsReaderListener<MSG> mListener;
+};
+
+template<typename MSG>
+using DdsReaderPtr = std::shared_ptr<DdsReader<MSG>>;
+
+}
+}
+
+#endif//__UT_DDS_ENTITY_HPP__

unitree_SDK/include/unitree/common/dds/dds_error.hpp

@@ -0,0 +1,17 @@
+#ifndef __UT_DDS_ERROR_HPP__
+#define __UT_DDS_ERROR_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree
+{
+namespace common
+{
+/*
+ * dds error.
+ */
+UT_DECL_ERR(UT_ERR_DDS, 2001, "dds error")
+
+}
+}
+#endif//__UT_DDS_ERROR_HPP__

unitree_SDK/include/unitree/common/dds/dds_exception.hpp

@@ -0,0 +1,58 @@
+#ifndef __UT_DDS_EXCEPTION_HPP__
+#define __UT_DDS_EXCEPTION_HPP__
+
+#include <unitree/common/exception.hpp>
+#include <unitree/common/dds/dds_error.hpp>
+
+#define __UT_DDS_EXCEPTION_MESSAGE(e, d)        \
+    std::string("Catch dds::core exception. Class:") + __UT_STR(d) + ", Message:" + e.what();
+
+#define __UT_DDS_EXCEPTION_CATCH(except, l, t)  \
+catch (const except & e)                        \
+{                                               \
+    if (l || t)                                 \
+    {                                           \
+        std::string __t9b78e5r = __UT_DDS_EXCEPTION_MESSAGE(e, except);  \
+        if (l)                                  \
+        {                                       \
+            LOG_ERROR(l, __t9b78e5r);           \
+        }                                       \
+        if (t)                                  \
+        {                                       \
+            UT_THROW(DdsException, __t9b78e5r); \
+        }                                       \
+    }                                           \
+}
+
+#define UT_DDS_EXCEPTION_TRY                    \
+try                                             \
+{
+
+#define UT_DDS_EXCEPTION_CATCH(l, t)            \
+}                                               \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::Error, l, t)                    \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::InvalidArgumentError, l, t)     \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::TimeoutError, l, t)             \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::UnsupportedError, l, t)         \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::AlreadyClosedError, l, t)       \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::IllegalOperationError, l, t)    \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::NotEnabledError, l, t)          \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::PreconditionNotMetError, l, t)  \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::ImmutablePolicyError, l, t)     \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::InconsistentPolicyError, l, t)  \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::OutOfResourcesError, l, t)      \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::InvalidDowncastError, l, t)     \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::NullReferenceError, l, t)       \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::InvalidDataError, l, t)         \
+__UT_DDS_EXCEPTION_CATCH(::dds::core::Exception, l, t)                \
+__UT_DDS_EXCEPTION_CATCH(std::exception, l, t)
+
+namespace unitree
+{
+namespace common
+{
+UT_DECL_EXCEPTION(DdsException, UT_ERR_DDS, UT_DESC_ERR(UT_ERR_DDS))
+}
+}
+
+#endif//__UT_DDS_EXCEPTION_HPP__

unitree_SDK/include/unitree/common/dds/dds_factory_model.hpp

@@ -0,0 +1,62 @@
+#ifndef __UT_DDS_FACTORY_MODEL_HPP__
+#define __UT_DDS_FACTORY_MODEL_HPP__
+
+#include <unitree/common/dds/dds_parameter.hpp>
+#include <unitree/common/dds/dds_topic_channel.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class DdsFactoryModel
+{
+public:
+    explicit DdsFactoryModel();
+    ~DdsFactoryModel();
+
+    void Init(uint32_t domainId, const std::string& ddsConfig = "");
+    void Init(const std::string& ddsParameterFileName = "");
+    void Init(const JsonMap& param);
+
+    template<typename MSG>
+    DdsTopicChannelPtr<MSG> CreateTopicChannel(const std::string& topic)
+    {
+        DdsTopicChannelPtr<MSG> channel = DdsTopicChannelPtr<MSG>(new DdsTopicChannel<MSG>());
+        channel->SetTopic(mParticipant, topic, mTopicQos);
+        return channel;
+    }
+
+    template<typename MSG>
+    void SetWriter(DdsTopicChannelPtr<MSG>& channelPtr)
+    {
+        channelPtr->SetWriter(mPublisher, mWriterQos);
+    }
+
+    template<typename MSG>
+    void SetReader(DdsTopicChannelPtr<MSG>& channelPtr, const std::function<void(const void*)>& handler, int32_t queuelen = 0)
+    {
+        DdsReaderCallback cb(handler);
+        channelPtr->SetReader(mSubscriber, mReaderQos, cb, queuelen);
+    }
+
+private:
+    DdsParticipantPtr mParticipant;
+    DdsPublisherPtr mPublisher;
+    DdsSubscriberPtr mSubscriber;
+
+    DdsParticipantQos mParticipantQos;
+    DdsTopicQos mTopicQos;
+    DdsPublisherQos mPublisherQos;
+    DdsSubscriberQos mSubscriberQos;
+    DdsWriterQos mWriterQos;
+    DdsReaderQos mReaderQos;
+
+    Logger *mLogger;
+};
+
+using DdsFactoryModelPtr = std::shared_ptr<DdsFactoryModel>;
+
+}
+}
+
+#endif//__UT_DDS_FACTORY_MODEL_HPP__

unitree_SDK/include/unitree/common/dds/dds_native.hpp

@@ -0,0 +1,37 @@
+#ifndef __UT_DDS_NATIVE_HPP__
+#define __UT_DDS_NATIVE_HPP__
+
+namespace unitree
+{
+namespace common
+{
+template<typename NATIVE>
+class DdsNative
+{
+public:
+    using NativeType = NATIVE;
+
+    explicit DdsNative()
+    {}
+
+    virtual ~DdsNative()
+    {}
+
+    void SetNative(const NATIVE& native)
+    {
+        mNative = native;
+    }
+
+    const NATIVE& GetNative() const
+    {
+        return mNative;
+    }
+
+protected:
+    NATIVE mNative;
+};
+
+}
+}
+
+#endif//

unitree_SDK/include/unitree/common/dds/dds_parameter.hpp

@@ -0,0 +1,214 @@
+#ifndef __UT_DDS_PARAMETER_HPP__
+#define __UT_DDS_PARAMETER_HPP__
+
+#include <unitree/common/decl.hpp>
+#include <unitree/common/dds/dds_qos_parameter.hpp>
+
+#define UT_DDS_PARAM_KEY_PARTICIPANT        "Participant"
+#define UT_DDS_PARAM_KEY_DOMAINID           "DomainId"
+#define UT_DDS_PARAM_KEY_CONFIG             "Config"
+#define UT_DDS_PARAM_KEY_NAME               "Name"
+#define UT_DDS_PARAM_KEY_TOPIC              "Topic"
+#define UT_DDS_PARAM_KEY_TOPICNAME          "TopicName"
+#define UT_DDS_PARAM_KEY_PUBLISHER          "Publisher"
+#define UT_DDS_PARAM_KEY_SUBSCRIBER         "Subscriber"
+#define UT_DDS_PARAM_KEY_WRITER             "Writer"
+#define UT_DDS_PARAM_KEY_READER             "Reader"
+#define UT_DDS_PARAM_KEY_QOS                "Qos"
+
+namespace unitree
+{
+namespace common
+{
+class DdsQosParameterHolder
+{
+public:
+    DdsQosParameterHolder();
+
+    virtual ~DdsQosParameterHolder();
+
+    void SetQos(const DdsQosParameter& qos);
+    const DdsQosParameter& GetQos() const;
+
+protected:
+    DdsQosParameter mQos;
+};
+
+class DdsParticipantParameter : public DdsQosParameterHolder
+{
+public:
+    DdsParticipantParameter();
+    DdsParticipantParameter(uint32_t domainId, const std::string& config = "");
+
+    ~DdsParticipantParameter();
+
+    void SetDomainId(int32_t domainId);
+    uint32_t GetDomainId() const;
+
+    void SetConfig(const std::string& config);
+    const std::string& GetConfig() const;
+
+private:
+    uint32_t mDomainId;
+    std::string mConfig;
+};
+
+class DdsTopicParameter : public DdsQosParameterHolder
+{
+public:
+    DdsTopicParameter();
+    DdsTopicParameter(const std::string& name);
+
+    ~DdsTopicParameter();
+
+    void SetName(const std::string& name);
+    const std::string& GetName() const;
+
+private:
+    std::string mName;
+};
+
+class DdsTopicParameterHolder
+{
+public:
+    DdsTopicParameterHolder();
+    DdsTopicParameterHolder(const std::string& topicName);
+
+    virtual ~DdsTopicParameterHolder();
+
+    void SetTopicName(const std::string& topicName);
+    const std::string& GetTopicName() const;
+
+private:
+    std::string mTopicName;
+};
+
+class DdsWriterParameter : public DdsTopicParameterHolder, public DdsQosParameterHolder
+{
+public:
+    DdsWriterParameter();
+    DdsWriterParameter(const std::string& topicName);
+
+    ~DdsWriterParameter();
+};
+
+class DdsWriterParameterHolder
+{
+public:
+    DdsWriterParameterHolder();
+
+    virtual ~DdsWriterParameterHolder();
+
+    void SetWriter(const DdsWriterParameter& writer);
+    const DdsWriterParameter& GetWriter() const;
+
+private:
+    DdsWriterParameter mWriter;
+};
+
+class DdsReaderParameter : public DdsTopicParameterHolder, public DdsQosParameterHolder
+{
+public:
+    DdsReaderParameter();
+    DdsReaderParameter(const std::string& topicName);
+
+    virtual ~DdsReaderParameter();
+};
+
+class DdsReaderParameterHolder
+{
+public:
+    DdsReaderParameterHolder();
+
+    virtual ~DdsReaderParameterHolder();
+
+    void SetReader(const DdsReaderParameter& reader);
+    const DdsReaderParameter& GetReader() const;
+
+private:
+    DdsReaderParameter mReader;
+};
+
+class DdsPublisherParameter : public DdsQosParameterHolder
+{
+public:
+    DdsPublisherParameter();
+
+    ~DdsPublisherParameter();
+
+    void AppendWriter(const DdsWriterParameter& writer);
+    void SetWriter(const std::vector<DdsWriterParameter>& writer);
+    const std::vector<DdsWriterParameter>& GetWriter() const;
+
+private:
+    std::vector<DdsWriterParameter> mWriter;
+};
+
+class DdsSubscriberParameter : public DdsQosParameterHolder
+{
+public:
+    DdsSubscriberParameter();
+    ~DdsSubscriberParameter();
+
+    void AppendReader(const DdsReaderParameter& reader);
+    void SetReader(const std::vector<DdsReaderParameter>& reader);
+    const std::vector<DdsReaderParameter>& GetReader() const;
+
+private:
+    std::vector<DdsReaderParameter> mReader;
+};
+
+class DdsParameter
+{
+public:
+    DdsParameter();
+    DdsParameter(const JsonMap& param);
+
+    ~DdsParameter();
+
+    void Init(const JsonMap& param);
+
+    uint32_t GetDomainId();
+    const std::string& GetConfig() const;
+
+    const DdsParticipantParameter& GetParticipant();
+
+    void AppendTopic(const DdsTopicParameter& topic);
+    const std::map<std::string,DdsTopicParameter>& GetTopic() const;
+
+    void AppendPublisher(const DdsPublisherParameter& publisher);
+    void SetPublisher(const std::vector<DdsPublisherParameter>& publisher);
+    const std::vector<DdsPublisherParameter>& GetPublisher() const;
+
+    void AppendSubscriber(const DdsSubscriberParameter& subscriber);
+    void SetSubscriber(const std::vector<DdsSubscriberParameter>& subscriber);
+    const std::vector<DdsSubscriberParameter>& GetSubscriber() const;
+
+    const DdsQosParameter& GetParticipantQos() const;
+    const DdsQosParameter& GetTopicQos() const;
+    const DdsQosParameter& GetPublisherQos() const;
+    const DdsQosParameter& GetSubscriberQos() const;
+    const DdsQosParameter& GetWriterQos() const;
+    const DdsQosParameter& GetReaderQos() const;
+
+private:
+    uint32_t mDomainId;
+    std::string mConfig;
+
+    DdsQosParameter mParticipantQos;
+    DdsQosParameter mTopicQos;
+    DdsQosParameter mPublisherQos;
+    DdsQosParameter mSubscriberQos;
+    DdsQosParameter mWriterQos;
+    DdsQosParameter mReaderQos;
+
+    DdsParticipantParameter mParticipant;
+    std::map<std::string,DdsTopicParameter> mTopic;
+    std::vector<DdsPublisherParameter> mPublisher;
+    std::vector<DdsSubscriberParameter> mSubscriber;
+
+};
+
+}
+}
+#endif//__UT_DDS_PARAMETER_HPP__

unitree_SDK/include/unitree/common/dds/dds_qos.hpp

@@ -0,0 +1,71 @@
+#ifndef __UT_DDS_QOS_HPP__
+#define __UT_DDS_QOS_HPP__
+
+#include <unitree/common/dds/dds_qos_policy.hpp>
+
+using namespace org::eclipse::cyclonedds;
+
+namespace unitree
+{
+namespace common
+{
+#define UT_DECL_DDS_QOS(QosType, QosNative)         \
+class QosType: public QosNative                     \
+{                                                   \
+public:                                             \
+    using NativeQosType = QosNative::NativeType;    \
+    explicit QosType()                              \
+    {                                               \
+        InitPolicyDefault();                        \
+    }                                               \
+    template<typename POLICY>                       \
+    void SetPolicy(const POLICY& policy)            \
+    {                                               \
+        mNative.policy(policy.GetNative());         \
+        mPolicyNameSet.insert(policy.GetName());    \
+    }                                               \
+    bool HasPolicy(const std::string& name) const   \
+    {                                               \
+        return mPolicyNameSet.find(name) != mPolicyNameSet.end();   \
+    }                                               \
+    void CopyToNativeQos(NativeQosType& qos) const; \
+private:                                            \
+    void InitPolicyDefault();                       \
+private:                                            \
+    std::set<std::string> mPolicyNameSet;           \
+};
+
+/*
+ * DdsParticipantQos
+ */
+UT_DECL_DDS_QOS(DdsParticipantQos, DdsNative<::dds::domain::qos::DomainParticipantQos>)
+
+/*
+ * DdsTopicQos
+ */
+UT_DECL_DDS_QOS(DdsTopicQos, DdsNative<::dds::topic::qos::TopicQos>)
+
+/*
+ * DdsPublisherQos
+ */
+UT_DECL_DDS_QOS(DdsPublisherQos, DdsNative<::dds::pub::qos::PublisherQos>)
+
+/*
+ * DdsSubscriberQos
+ */
+UT_DECL_DDS_QOS(DdsSubscriberQos, DdsNative<::dds::sub::qos::SubscriberQos>)
+
+/*
+ * DdsWriterQos
+ */
+UT_DECL_DDS_QOS(DdsWriterQos, DdsNative<::dds::pub::qos::DataWriterQos>)
+
+/*
+ * DdsReaderQos
+ */
+UT_DECL_DDS_QOS(DdsReaderQos, DdsNative<::dds::sub::qos::DataReaderQos>)
+
+}
+}
+
+#endif//__UT_DDS_QOS_HPP__

unitree_SDK/include/unitree/common/dds/dds_qos_parameter.hpp

@@ -0,0 +1,81 @@
+#ifndef __UT_DDS_QOS_PARAMETER_HPP__
+#define __UT_DDS_QOS_PARAMETER_HPP__
+
+#include <unitree/common/dds/dds_qos_policy_parameter.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class DdsQosParameter
+{
+public:
+    DdsQosParameter();
+    ~DdsQosParameter();
+
+    void Init(const JsonMap& data);
+
+#define __GET_POLICY(POLICY)                                    \
+    const DdsQos##POLICY##PolicyParameter& Get##POLICY() const  \
+    {                                                           \
+        return m##POLICY;                                       \
+    }
+
+    __GET_POLICY(Deadline)
+    __GET_POLICY(DestinationOrder)
+    __GET_POLICY(Durability)
+    __GET_POLICY(DurabilityService)
+    __GET_POLICY(EntityFactory)
+    __GET_POLICY(GroupData)
+    __GET_POLICY(History)
+    __GET_POLICY(LatencyBudget)
+    __GET_POLICY(Lifespan)
+    __GET_POLICY(Liveliness)
+    __GET_POLICY(Ownership)
+    __GET_POLICY(OwnershipStrength)
+    __GET_POLICY(Partition)
+    __GET_POLICY(Presentation)
+    __GET_POLICY(ReaderDataLifecycle)
+    __GET_POLICY(Reliability)
+    __GET_POLICY(ResourceLimits)
+    __GET_POLICY(TimeBasedFilter)
+    __GET_POLICY(TopicData)
+    __GET_POLICY(TransportPriority)
+    __GET_POLICY(WriterDataLifecycle)
+    __GET_POLICY(UserData)
+
+#undef __GET_POLICY
+
+    bool Default() const;
+
+private:
+    bool mDefault;
+
+    DdsQosDeadlinePolicyParameter mDeadline;
+    DdsQosDestinationOrderPolicyParameter mDestinationOrder;
+    DdsQosDurabilityPolicyParameter mDurability;
+    DdsQosDurabilityServicePolicyParameter mDurabilityService;
+    DdsQosEntityFactoryPolicyParameter mEntityFactory;
+    DdsQosGroupDataPolicyParameter mGroupData;
+    DdsQosHistoryPolicyParameter mHistory;
+    DdsQosLatencyBudgetPolicyParameter mLatencyBudget;
+    DdsQosLifespanPolicyParameter mLifespan;
+    DdsQosLivelinessPolicyParameter mLiveliness;
+    DdsQosOwnershipPolicyParameter mOwnership;
+    DdsQosOwnershipStrengthPolicyParameter mOwnershipStrength;
+    DdsQosPartitionPolicyParameter mPartition;
+    DdsQosPresentationPolicyParameter mPresentation;
+    DdsQosReaderDataLifecyclePolicyParameter mReaderDataLifecycle;
+    DdsQosReliabilityPolicyParameter mReliability;
+    DdsQosResourceLimitsPolicyParameter mResourceLimits;
+    DdsQosTimeBasedFilterPolicyParameter mTimeBasedFilter;
+    DdsQosTopicDataPolicyParameter mTopicData;
+    DdsQosTransportPriorityPolicyParameter mTransportPriority;
+    DdsQosWriterDataLifecyclePolicyParameter mWriterDataLifecycle;
+    DdsQosUserDataPolicyParameter mUserData;
+};
+
+}
+}
+
+#endif//__UT_DDS_QOS_PARAMETER_HPP__

unitree_SDK/include/unitree/common/dds/dds_qos_policy.hpp

@@ -0,0 +1,195 @@
+#ifndef __UT_DDS_QOS_POLICY_HPP__
+#define __UT_DDS_QOS_POLICY_HPP__
+
+#include <dds/dds.hpp>
+#include <unitree/common/dds/dds_native.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class DdsQosPolicyName
+{
+public:
+    explicit DdsQosPolicyName(const std::string& name) :
+        mName(name)
+    {}
+
+    virtual ~DdsQosPolicyName()
+    {}
+
+    const std::string& GetName() const
+    {
+        return mName;
+    }
+
+protected:
+    std::string mName;
+};
+
+class DdsDuration : public DdsNative<::dds::core::Duration>
+{
+public:
+    explicit DdsDuration(int64_t nanoSecond);
+    ~DdsDuration();
+};
+
+class DdsQosDeadlinePolicy : public DdsNative<::dds::core::policy::Deadline>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosDeadlinePolicy(int64_t period);
+    ~DdsQosDeadlinePolicy();
+};
+
+class DdsQosDestinationOrderPolicy : public DdsNative<::dds::core::policy::DestinationOrder>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosDestinationOrderPolicy(int32_t kind);
+    ~DdsQosDestinationOrderPolicy();
+};
+
+class DdsQosDurabilityPolicy : public DdsNative<::dds::core::policy::Durability>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosDurabilityPolicy(int32_t kind);
+    ~DdsQosDurabilityPolicy();
+};
+
+class DdsQosDurabilityServicePolicy : public DdsNative<::dds::core::policy::DurabilityService>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosDurabilityServicePolicy(int64_t cleanupDelay, int32_t historyKind, int32_t historyDepth,
+        int32_t maxSamples, int32_t maxInstances, int32_t maxSamplesPerInstance);
+    ~DdsQosDurabilityServicePolicy();
+};
+
+class DdsQosEntityFactoryPolicy : public DdsNative<::dds::core::policy::EntityFactory>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosEntityFactoryPolicy(bool autoEnable);
+    ~DdsQosEntityFactoryPolicy();
+};
+
+class DdsQosGroupDataPolicy : public DdsNative<::dds::core::policy::GroupData>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosGroupDataPolicy(const std::vector<uint8_t>& value);
+    ~DdsQosGroupDataPolicy();
+};
+
+class DdsQosHistoryPolicy : public DdsNative<::dds::core::policy::History>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosHistoryPolicy(int32_t kind, int32_t depth);
+    ~DdsQosHistoryPolicy();
+};
+
+class DdsQosLatencyBudgetPolicy : public DdsNative<::dds::core::policy::LatencyBudget>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosLatencyBudgetPolicy(int64_t duration);
+    ~DdsQosLatencyBudgetPolicy();
+};
+
+class DdsQosLifespanPolicy : public DdsNative<::dds::core::policy::Lifespan>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosLifespanPolicy(int64_t duration);
+    ~DdsQosLifespanPolicy();
+};
+
+class DdsQosLivelinessPolicy : public DdsNative<::dds::core::policy::Liveliness>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosLivelinessPolicy(int32_t kind, int64_t leaseDuration);
+    ~DdsQosLivelinessPolicy();
+};
+
+class DdsQosOwnershipPolicy : public DdsNative<::dds::core::policy::Ownership>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosOwnershipPolicy(int32_t kind);
+    ~DdsQosOwnershipPolicy();
+};
+
+class DdsQosOwnershipStrengthPolicy : public DdsNative<::dds::core::policy::OwnershipStrength>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosOwnershipStrengthPolicy(int32_t strength);
+    ~DdsQosOwnershipStrengthPolicy();
+};
+
+class DdsQosPartitionPolicy : public DdsNative<::dds::core::policy::Partition>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosPartitionPolicy(const std::string& name);
+    ~DdsQosPartitionPolicy();
+};
+
+class DdsQosPresentationPolicy : public DdsNative<::dds::core::policy::Presentation>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosPresentationPolicy(int32_t accessScopeKind, bool coherentAccess, bool orderedAccess);
+    ~DdsQosPresentationPolicy();
+};
+
+class DdsQosReaderDataLifecyclePolicy : public DdsNative<::dds::core::policy::ReaderDataLifecycle>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosReaderDataLifecyclePolicy(int64_t autopurgeNowriterSamplesDelay, int64_t autopurgeDisposedSamplesDelay);
+    ~DdsQosReaderDataLifecyclePolicy();
+};
+
+class DdsQosReliabilityPolicy : public DdsNative<::dds::core::policy::Reliability>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosReliabilityPolicy(int32_t kind, int64_t maxBlockingTime);
+    ~DdsQosReliabilityPolicy();
+};
+
+class DdsQosResourceLimitsPolicy : public DdsNative<::dds::core::policy::ResourceLimits>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosResourceLimitsPolicy(int32_t maxSamples, int32_t maxInstances, int32_t maxSamplesPerInstance);
+    ~DdsQosResourceLimitsPolicy();
+};
+
+class DdsQosTimeBasedFilterPolicy : public DdsNative<::dds::core::policy::TimeBasedFilter>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosTimeBasedFilterPolicy(int64_t minSep);
+    ~DdsQosTimeBasedFilterPolicy();
+};
+
+class DdsQosTopicDataPolicy : public DdsNative<::dds::core::policy::TopicData>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosTopicDataPolicy(const std::vector<uint8_t>& value);
+    ~DdsQosTopicDataPolicy();
+};
+
+class DdsQosTransportPriorityPolicy : public DdsNative<::dds::core::policy::TransportPriority>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosTransportPriorityPolicy(int32_t value);
+    ~DdsQosTransportPriorityPolicy();
+};
+
+class DdsQosWriterDataLifecyclePolicy : public DdsNative<::dds::core::policy::WriterDataLifecycle>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosWriterDataLifecyclePolicy(bool autodisposeUnregisteredInstances);
+    ~DdsQosWriterDataLifecyclePolicy();
+};
+
+class DdsQosUserDataPolicy : public DdsNative<::dds::core::policy::UserData>, public DdsQosPolicyName
+{
+public:
+    explicit DdsQosUserDataPolicy(const std::vector<uint8_t>& value);
+    ~DdsQosUserDataPolicy();
+};
+
+}
+}
+
+#endif//__UT_DDS_QOS_POLICY_HPP__

unitree_SDK/include/unitree/common/dds/dds_qos_policy_parameter.hpp

@@ -0,0 +1,607 @@
+#ifndef __UT_DDS_QOS_POLICY_PARAMETER_HPP__
+#define __UT_DDS_QOS_POLICY_PARAMETER_HPP__
+
+#include <unitree/common/json/json.hpp>
+
+namespace unitree
+{
+namespace common
+{
+/*
+ *----------------------------
+ * Supported Qos policy:
+ * 1. Deadline
+ *      IDL:
+ *      struct DeadlineQosPolicy {
+ *           Duration_t period;
+ *      };
+ *
+ * 2. DestinationOrder
+ *      IDL:
+ *      enum DestinationOrderQosPolicyKind {
+ *          BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS,
+ *          BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS
+ *      };
+ *      struct DestinationOrderQosPolicy {
+ *          DestinationOrderQosPolicyKind kind;
+ *      };
+ *
+ * 3. Durability
+ *      IDL:
+ *      enum DurabilityQosPolicyKind {
+ *          VOLATILE_DURABILITY_QOS, // Least Durability
+ *          TRANSIENT_LOCAL_DURABILITY_QOS,
+ *          TRANSIENT_DURABILITY_QOS,
+ *          PERSISTENT_DURABILITY_QOS // Greatest Durability
+ *      };
+ *      struct DurabilityQosPolicy {
+ *          DurabilityQosPolicyKind kind;
+ *      };
+ *
+ * 4. DurabilityService
+ *      IDL:
+ *      struct DurabilityServiceQosPolicy {
+ *          Duration_t service_cleanup_delay;
+ *          HistoryQosPolicyKind history_kind;
+ *          long history_depth;
+ *          long max_samples;
+ *          long max_instances;
+ *          long max_samples_per_instance;
+ *      };
+ *
+ * 5. EntityFactory
+ *      IDL:
+ *      struct EntityFactoryQosPolicy {
+ *          boolean autoenable_created_entities;
+ *      };
+ *
+ * 6. GroupData
+ *      IDL:
+ *      struct GroupDataQosPolicy {
+ *          sequence<octet> value;
+ *      };
+ *
+ * 7. History
+ *      IDL:
+ *      enum HistoryQosPolicyKind {
+ *          KEEP_LAST_HISTORY_QOS,
+ *          KEEP_ALL_HISTORY_QOS
+ *      };
+ *      struct HistoryQosPolicy {
+ *          HistoryQosPolicyKind kind;
+ *          long depth;
+ *      };
+ * 8. LatencyBudget
+ *      IDL:
+ *      struct LatencyBudgetQosPolicy {
+ *          Duration_t duration;
+ *      };
+ *
+ * 9. Lifespan
+ *      IDL:
+ *      struct LifespanQosPolicy {
+ *          Duration_t duration;
+ *      }
+ *
+ * 10.Liveliness
+ *      IDL:
+ *      enum LivelinessQosPolicyKind {
+ *          AUTOMATIC_LIVELINESS_QOS,
+ *          MANUAL_BY_PARTICIPANT_LIVELINESS_QOS,
+ *          MANUAL_BY_TOPIC_LIVELINESS_QOS
+ *      };
+ *      struct LivelinessQosPolicy {
+ *          LivelinessQosPolicyKind kind;
+ *          Duration_t lease_duration;
+ *      };
+ *
+ * 11.Ownership
+ *      IDL:
+ *      enum OwnershipQosPolicyKind {
+ *          SHARED_OWNERSHIP_QOS,
+ *          EXCLUSIVE_OWNERSHIP_QOS
+ *      };
+ *      struct OwnershipQosPolicy {
+ *          OwnershipQosPolicyKind kind;
+ *      };
+ *
+ * 12.OwnershipStrength
+ *      IDL:
+ *      struct OwnershipStrengthQosPolicy {
+ *          long value;
+ *      };
+ *
+ * 13.Partition
+ *      IDL:
+ *      struct PartitionQosPolicy {
+ *          StringSeq name;
+ *      };
+ *
+ * 14.Presentation
+ *      IDL:
+ *      enum PresentationQosPolicyAccessScopeKind {
+ *          INSTANCE_PRESENTATION_QOS,
+ *          TOPIC_PRESENTATION_QOS,
+ *          GROUP_PRESENTATION_QOS
+ *      };
+ *      struct PresentationQosPolicy {
+ *          PresentationQosPolicyAccessScopeKind access_scope;
+ *          boolean coherent_access;
+ *          boolean ordered_access;
+ *      };
+ *
+ * 15.ReaderDataLifecycle
+ *      IDL:
+ *      struct ReaderDataLifecycleQosPolicy {
+ *          Duration_t autopurge_nowriter_samples_delay;
+ *          Duration_t autopurge_disposed_samples_delay;
+ *      };
+ *
+ * 16.Reliability
+ *      IDL:
+ *      enum ReliabilityQosPolicyKind {
+ *          BEST_EFFORT_RELIABILITY_QOS,
+ *          RELIABLE_RELIABILITY_QOS
+ *      };
+ *      struct ReliabilityQosPolicy {
+ *          ReliabilityQosPolicyKind kind;
+ *          Duration_t max_blocking_time;
+ *      };
+ * 17.ResourceLimits
+ *      IDL:
+ *      struct ResourceLimitsQosPolicy {
+ *          long max_samples;
+ *          long max_instances;
+ *          long max_samples_per_instance;
+ *      };
+ *
+ * 18.TimeBasedFilter
+ *      IDL:
+ *      struct TimeBasedFilterQosPolicy {
+ *          Duration_t minimum_separation;
+ *      };
+ *
+ * 19.TopicData
+ *      IDL:
+ *      struct TopicDataQosPolicy {
+ *          sequence<octet> value;
+ *      };
+ *
+ * 20.TransportPriority
+ *      IDL:
+ *      struct TransportPriorityQosPolicy {
+ *          long value;
+ *      };
+ *
+ * 21.UserData
+ *      IDL:
+ *      struct UserDataQosPolicy {
+ *          sequence<octet> value;
+ *      };
+ *
+ * 22.WriterDataLifecycle
+ *      IDL:
+ *      struct WriterDataLifecycleQosPolicy {
+ *          boolean autodispose_unregistered_instances;
+ *      };
+ *
+ *-------------------------------------------------------------------------------------------------
+ * QoS policis corresponding to entity: 
+ * DomainParticipant: [2]  { EntityFactory, UserData }
+ * Topic:             [13] { Deadline, DestinationOrder, Durability, DurabilityService, History,
+ *                           LatencyBudget, Lifespan, Liveliness, Ownership, Reliability,
+ *                           ResourceLimits, TopicData, TransportPriority }
+ * Publisher:         [4]  { EntityFactory, GroupData, Partition, Presentation }
+ * Subscriber:        [4]  { EntityFactory, GroupData, Partition, Presentation }
+ * DataWriter:        [14] { Deadline, DestinationOrder, Durability, History, LatencyBudget, 
+ *                           Lifespan, Liveliness, Ownership, OwnershipStrength, Reliability,
+ *                           ResourceLimits, TransportPriority, UserData, WriterDataLifecycle }
+ * DataReader:        [12] { Deadline, DestinationOrder, Durability, History, LatencyBudget
+ *                           Liveliness, Ownership, Reliability, ReaderDataLifecycle, ResourceLimits,
+ *                           TimeBasedFilter, UserData }
+ *-------------------------------------------------------------------------------------------------
+ */
+
+/*
+ * Qos policy name
+ */
+#define UT_DDS_QOS_POLICY_NAME_DEADLINE                 "Deadline"
+#define UT_DDS_QOS_POLICY_NAME_DESTINATION_ORDER        "DestinationOrder"
+#define UT_DDS_QOS_POLICY_NAME_DURABILITY               "Durability"
+#define UT_DDS_QOS_POLICY_NAME_DURABILITY_SERVICE       "DurabilityService"
+#define UT_DDS_QOS_POLICY_NAME_ENTITY_FACTORY           "EntityFactory"
+#define UT_DDS_QOS_POLICY_NAME_GROUP_DATA               "GroupData"
+#define UT_DDS_QOS_POLICY_NAME_HISTORY                  "History"
+#define UT_DDS_QOS_POLICY_NAME_LATENCY_BUDGET           "LatencyBudget"
+#define UT_DDS_QOS_POLICY_NAME_LIFESPAN                 "Lifespan"
+#define UT_DDS_QOS_POLICY_NAME_LIVELINESS               "Liveliness"
+#define UT_DDS_QOS_POLICY_NAME_OWNERSHIP                "Ownership"
+#define UT_DDS_QOS_POLICY_NAME_OWNERSHIP_STRENGTH       "OwnershipStrength"
+#define UT_DDS_QOS_POLICY_NAME_PARTITION                "Partition"
+#define UT_DDS_QOS_POLICY_NAME_PRESENTATION             "Presentation"
+#define UT_DDS_QOS_POLICY_NAME_READER_DATA_LIFECYCLE    "ReaderDataLifecycle"
+#define UT_DDS_QOS_POLICY_NAME_RELIABILITY              "Reliability"
+#define UT_DDS_QOS_POLICY_NAME_RESOURCE_LIMITS          "ResourceLimits"
+#define UT_DDS_QOS_POLICY_NAME_TIME_BASED_FILTER        "TimeBasedFilter"
+#define UT_DDS_QOS_POLICY_NAME_TOPIC_DATA               "TopicData"
+#define UT_DDS_QOS_POLICY_NAME_TRANSPORT_PRIORITY       "TransportPriority"
+#define UT_DDS_QOS_POLICY_NAME_WRITER_DATA_LIFECYCLE    "WriterDataLifecycle"
+#define UT_DDS_QOS_POLICY_NAME_USER_DATA                "UserData"
+
+/*
+ * Qos Policy Member Name
+ */
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_KIND              "kind"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_PEROID            "peroid"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_CLEANUP_DELAY     "service_cleanup_delay"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_HISTORY_KIND      "history_kind"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_HISTORY_DEPTH     "history_depth"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_MAX_SAMPLES       "max_samples"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_MAX_INSTANCES     "max_instances"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_MAX_SAMPLES_PER_INSTANCE  "max_samples_per_instance"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_AUTO_ENABLE       "autoenable_created_entities"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_VALUE             "value"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_DEPTH             "depth"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_DURATION          "duration"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_LEASE_DURATION    "lease_duration"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_NAME              "name"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_ACCESS_SCOPE      "access_scope"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_COHERENT_ACCESS   "coherent_access"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_ORDERED_ACCESS    "ordered_access"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_MAX_BLOCKING_TIME "max_blocking_time"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_AUTODISPOSE_UNREGISETED_INSTANCES "autodispose_unregistered_instances"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_AUTOPURGE_NOWRITER_SAMPLES_DELAY  "autopurge_nowriter_samples_delay"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_AUTOPURGE_DISPOSED_SAMPLES_DELAY  "autopurge_disposed_samples_delay"
+#define UT_DDS_QOS_POLICY_MEMBER_NAME_MIN_SEP           "minimum_separation"
+
+class DdsQosPolicyParameter
+{
+public:
+    DdsQosPolicyParameter();
+    virtual ~DdsQosPolicyParameter();
+
+    virtual void Init(const JsonMap& data) = 0;
+
+    void Update();
+    bool Default() const;
+
+protected:
+    bool mDefault;
+};
+
+class DdsQosDeadlinePolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosDeadlinePolicyParameter();
+    ~DdsQosDeadlinePolicyParameter();
+
+    void Init(const JsonMap& data);
+    int64_t GetPeriod() const;
+
+private:
+    int64_t mPeriod;
+};
+
+class DdsQosDestinationOrderPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosDestinationOrderPolicyParameter();
+    ~DdsQosDestinationOrderPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetKind() const;
+
+private:
+    int32_t mKind;
+};
+
+class DdsQosDurabilityPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosDurabilityPolicyParameter();
+    ~DdsQosDurabilityPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetKind() const;
+
+private:
+    int32_t mKind;
+};
+
+class DdsQosDurabilityServicePolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosDurabilityServicePolicyParameter();
+    ~DdsQosDurabilityServicePolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int64_t GetCleanupDelay() const;
+    int32_t GetHistoryKind() const;
+    int32_t GetHistoryDepth() const;
+    int32_t GetMaxSamples() const;
+    int32_t GetMaxInstances() const;
+    int32_t GetMaxSamplesPerInstance() const;
+
+private:
+    int64_t mCleanupDelay;
+    int32_t mHistoryKind;
+    int32_t mHistoryDepth;
+    int32_t mMaxSamples;
+    int32_t mMaxInstances;
+    int32_t mMaxSamplesPerInstance;
+};
+
+class DdsQosEntityFactoryPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosEntityFactoryPolicyParameter();
+    ~DdsQosEntityFactoryPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int64_t GetAutoEnable() const;
+
+private:
+    bool mAutoEnable;
+};
+
+class DdsQosGroupDataPolicyParameter: public DdsQosPolicyParameter
+{
+public:
+    DdsQosGroupDataPolicyParameter();
+    ~DdsQosGroupDataPolicyParameter();
+
+    void Init(const JsonMap& data);
+    const std::vector<uint8_t>& GetValue() const;
+
+private:
+    std::vector<uint8_t> mValue;
+};
+
+class DdsQosHistoryPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosHistoryPolicyParameter();
+    ~DdsQosHistoryPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetKind() const;
+    int32_t GetDepth() const;
+
+private:
+    int32_t mKind;
+    int32_t mDepth;
+};
+
+class DdsQosLatencyBudgetPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosLatencyBudgetPolicyParameter();
+    ~DdsQosLatencyBudgetPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int64_t GetDuration() const;
+
+private:
+    int64_t mDuration;
+};
+
+class DdsQosLifespanPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosLifespanPolicyParameter();
+    ~DdsQosLifespanPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int64_t GetDuration() const;
+
+private:
+    int64_t mDuration;
+};
+
+class DdsQosLivelinessPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosLivelinessPolicyParameter();
+    ~DdsQosLivelinessPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetKind() const;
+    int64_t GetLeaseDuration() const;
+
+private:
+    int32_t mKind;
+    int64_t mLeaseDuration;
+};
+
+class DdsQosOwnershipPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosOwnershipPolicyParameter();
+    ~DdsQosOwnershipPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetKind() const;
+
+private:
+    int32_t mKind;
+};
+
+class DdsQosOwnershipStrengthPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosOwnershipStrengthPolicyParameter();
+    ~DdsQosOwnershipStrengthPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetValue() const;
+
+private:
+    int32_t mValue;
+};
+
+class DdsQosPartitionPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosPartitionPolicyParameter();
+    ~DdsQosPartitionPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    const std::string& GetName() const;
+
+private:
+    std::string mName;
+};
+
+class DdsQosPresentationPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosPresentationPolicyParameter();
+    ~DdsQosPresentationPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetAccessScopeKind() const;
+    bool GetCoherentAccess() const;
+    bool GetOrderedAccess() const;
+
+private:
+    int32_t mAccessScopeKind;
+    bool mCoherentAccess;
+    bool mOrderedAccess;
+};
+
+class DdsQosReaderDataLifecyclePolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosReaderDataLifecyclePolicyParameter();
+    ~DdsQosReaderDataLifecyclePolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int64_t GetAutopurgeNowriterSamplesDelay() const;
+    int64_t GetAutopurgeDisposedSamplesDelay() const;
+
+private:
+    int64_t mAutopurgeNowriterSamplesDelay;
+    int64_t mAutopurgeDisposedSamplesDelay;
+};
+
+class DdsQosReliabilityPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosReliabilityPolicyParameter();
+    ~DdsQosReliabilityPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetKind() const;
+    int64_t GetMaxBlockingTime() const;
+
+private:
+    int32_t mKind;
+    int64_t mMaxBlockingTime;
+};
+
+class DdsQosResourceLimitsPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosResourceLimitsPolicyParameter();
+    ~DdsQosResourceLimitsPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetMaxSamples() const;
+    int32_t GetMaxInstances() const;
+    int32_t GetMaxSamplesPerInstance() const;
+
+private:
+    int32_t mMaxSamples;
+    int32_t mMaxInstances;
+    int32_t mMaxSamplesPerInstance;
+};
+
+class DdsQosTimeBasedFilterPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosTimeBasedFilterPolicyParameter();
+    ~DdsQosTimeBasedFilterPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int64_t GetMinSep() const;
+
+private:
+    int64_t mMinSep;
+};
+
+class DdsQosTopicDataPolicyParameter: public DdsQosPolicyParameter
+{
+public:
+    DdsQosTopicDataPolicyParameter();
+    ~DdsQosTopicDataPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    const std::vector<uint8_t>& GetValue() const;
+
+private:
+    std::vector<uint8_t> mValue;
+};
+
+class DdsQosTransportPriorityPolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosTransportPriorityPolicyParameter();
+    ~DdsQosTransportPriorityPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    int32_t GetValue() const;
+
+private:
+    int32_t mValue;
+};
+
+class DdsQosWriterDataLifecyclePolicyParameter : public DdsQosPolicyParameter
+{
+public:
+    DdsQosWriterDataLifecyclePolicyParameter();
+    ~DdsQosWriterDataLifecyclePolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    bool GetAutodisposeUnregisteredInstances() const;
+
+private:
+    bool mAutodisposeUnregisteredInstances;
+};
+
+class DdsQosUserDataPolicyParameter: public DdsQosPolicyParameter
+{
+public:
+    DdsQosUserDataPolicyParameter();
+    ~DdsQosUserDataPolicyParameter();
+
+    void Init(const JsonMap& data);
+
+    const std::vector<uint8_t>& GetValue() const;
+
+private:
+    std::vector<uint8_t> mValue;
+};
+
+}
+}
+
+#endif//__UT_DDS_QOS_POLICY_PARAMETER_HPP__

unitree_SDK/include/unitree/common/dds/dds_qos_realize.hpp

@@ -0,0 +1,26 @@
+#ifndef __UT_DDS_QOS_REALIZE_HPP__
+#define __UT_DDS_QOS_REALIZE_HPP__
+
+#include <unitree/common/dds/dds_qos_parameter.hpp>
+#include <unitree/common/dds/dds_qos.hpp>
+
+namespace unitree
+{
+namespace common
+{
+void Realize(const DdsQosParameter& parameter, DdsParticipantQos& qos);
+
+void Realize(const DdsQosParameter& parameter, DdsTopicQos& qos);
+
+void Realize(const DdsQosParameter& parameter, DdsPublisherQos& qos);
+
+void Realize(const DdsQosParameter& parameter, DdsSubscriberQos& qos);
+
+void Realize(const DdsQosParameter& parameter, DdsWriterQos& qos);
+
+void Realize(const DdsQosParameter& parameter, DdsReaderQos& qos);
+
+}
+}
+
+#endif//__UT_DDS_QOS_REALIZE_HPP__

unitree_SDK/include/unitree/common/dds/dds_topic_channel.hpp

@@ -0,0 +1,96 @@
+#ifndef __UT_DDS_TOPIC_CHANNEL_HPP__
+#define __UT_DDS_TOPIC_CHANNEL_HPP__
+
+#include <unitree/common/dds/dds_entity.hpp>
+
+namespace unitree
+{
+namespace common
+{
+/*
+ * @brief: DdsTopicChannelAbstract
+ */
+class DdsTopicChannelAbstract
+{
+public:
+    virtual bool Write(const void* message, int64_t waitMicrosec) = 0;
+    virtual int64_t GetLastDataAvailableTime() const = 0;
+};
+
+using DdsTopicChannelAbstractPtr = std::shared_ptr<DdsTopicChannelAbstract>;
+
+#define UT_DDS_WAIT_MATCHED_TIME_MICRO_SEC 100000
+
+/*
+ * @brief: DdsTopicChannel
+ */
+template<typename MSG>
+class DdsTopicChannel : public DdsTopicChannelAbstract
+{
+public:
+    explicit DdsTopicChannel()
+    {}
+
+    ~DdsTopicChannel()
+    {}
+
+    void SetTopic(const DdsParticipantPtr& participant, const std::string& name, const DdsTopicQos& qos)
+    {
+        mTopic = DdsTopicPtr<MSG>(new DdsTopic<MSG>(participant, name, qos));
+    }
+
+    void SetWriter(const DdsPublisherPtr& publisher, const DdsWriterQos& qos)
+    {
+        mWriter = DdsWriterPtr<MSG>(new DdsWriter<MSG>(publisher, mTopic, qos));
+        MicroSleep(UT_DDS_WAIT_MATCHED_TIME_MICRO_SEC);
+    }
+
+    void SetReader(const DdsSubscriberPtr& subscriber, const DdsReaderQos& qos, const DdsReaderCallback& cb, int32_t queuelen)
+    {
+        mReader = DdsReaderPtr<MSG>(new DdsReader<MSG>(subscriber, mTopic, qos));
+        mReader->SetListener(cb, queuelen);
+    }
+
+    DdsWriterPtr<MSG> GetWriter() const
+    {
+        return mWriter;
+    }
+
+    DdsReaderPtr<MSG> GetReader() const
+    {
+        return mReader;
+    }
+
+    bool Write(const void* message, int64_t waitMicrosec)
+    {
+        return Write(*(const MSG*)message, waitMicrosec);
+    }
+
+    bool Write(const MSG& message, int64_t waitMicrosec)
+    {
+        return mWriter->Write(message, waitMicrosec);
+    }
+
+    int64_t GetLastDataAvailableTime() const
+    {
+        if (mReader)
+        {
+            return mReader->GetLastDataAvailableTime();
+        }
+
+        return 0;
+    }
+
+private:
+    DdsTopicPtr<MSG> mTopic;
+    DdsWriterPtr<MSG> mWriter;
+    DdsReaderPtr<MSG> mReader;
+};
+
+template<typename MSG>
+using DdsTopicChannelPtr = std::shared_ptr<DdsTopicChannel<MSG>>;
+
+}
+}
+
+#endif//__UT_DDS_TOPIC_CHANNEL_HPP__

unitree_SDK/include/unitree/common/dds/dds_traits.hpp

@@ -0,0 +1,18 @@
+#ifndef __UT_DDS_TRAINTS_HPP__
+#define __UT_DDS_TRAINTS_HPP__
+
+namespace unitree
+{
+namespace common
+{
+#include <org/eclipse/cyclonedds/topic/TopicTraits.hpp>
+
+#define DdsGetTypeName(TYPE) \
+    org::eclipse::cyclonedds::topic::TopicTraits<TYPE>::getTypeName()
+
+#define DdsIsKeyless(TYPE) \
+    org::eclipse::cyclonedds::topic::TopicTraits<TYPE>::isKeyless()
+
+}
+}
+#endif//__UT_DDS_TRAINTS_HPP__