init

unitree_SDK/include/unitree/common/any.hpp

@@ -0,0 +1,390 @@
+#ifndef __UT_ANY_HPP__
+#define __UT_ANY_HPP__
+
+#include <unitree/common/exception.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class Any
+{
+public:
+    Any()
+        : mContent(0)
+    {}
+
+    template<typename ValueType>
+    Any(const ValueType& value)
+        : mContent(new Holder<ValueType>(value))
+    {}
+
+    Any(const char* s)
+        : Any(std::string(s))
+    {}
+
+    Any(const char* s, size_t len)
+        : Any(std::string(s, len))
+    {}
+
+    Any(const Any& other)
+        : mContent(other.mContent ? other.mContent->Clone() : 0)
+    {}
+
+    ~Any()
+    {
+        delete mContent;
+        mContent = 0;
+    }
+
+    Any& Swap(Any& other)
+    {
+        std::swap(mContent, other.mContent);
+        return *this;
+    }
+
+    bool Empty() const
+    {
+        return mContent == 0;
+    }
+
+    const std::type_info& GetTypeInfo() const
+    {
+        return mContent ? mContent->GetTypeInfo() : typeid(void);
+    }
+
+    template<typename ValueType>
+    Any& operator=(const ValueType& other)
+    {
+        Any(other).Swap(*this);
+        return *this;
+    }
+
+    Any& operator=(Any other)
+    {
+        other.Swap(*this);
+        return *this;
+    }
+
+public:
+    class PlaceHolder
+    {
+    public:
+        virtual ~PlaceHolder()
+        {}
+
+    public:
+        virtual const std::type_info& GetTypeInfo() const = 0;
+        virtual PlaceHolder* Clone() const = 0;
+    };
+
+    template<typename ValueType>
+    class Holder : public PlaceHolder
+    {
+    public:
+        explicit Holder(const ValueType& value)
+            : mValue(value)
+        {}
+
+        virtual const std::type_info& GetTypeInfo() const
+        {
+            return typeid(ValueType);
+        }
+
+        virtual PlaceHolder* Clone() const
+        {
+            return new Holder(mValue);
+        }
+
+    public:
+        ValueType mValue;
+    };
+
+public:
+    PlaceHolder* mContent;
+};
+
+/*
+ * static const Any
+ */
+static const Any UT_EMPTY_ANY = Any();
+
+static inline bool IsBool(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(bool);
+}
+
+static inline bool IsString(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(std::string);
+}
+
+static inline bool IsInt8(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(int8_t);
+}
+
+static inline bool IsUint8(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(uint8_t);
+}
+
+static inline bool IsInt16(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(int16_t);
+}
+
+static inline bool IsUint16(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(uint16_t);
+}
+
+static inline bool IsInt(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(int32_t);
+}
+
+static inline bool IsUint(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(uint32_t);
+}
+
+static inline bool IsInt64(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(int64_t);
+}
+
+static inline bool IsUint64(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(uint64_t);
+}
+
+static inline bool IsFloat(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(float);
+}
+
+static inline bool IsDouble(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(double);
+}
+
+static inline bool IsLongDouble(const Any& any)
+{
+    return any.GetTypeInfo() == typeid(long double);
+}
+
+static inline bool IsInteger(const Any& any)
+{
+    return IsInt(any) || IsUint(any) || IsInt64(any) || IsUint64(any)
+        || IsInt16(any) || IsUint16(any) || IsInt8(any) || IsUint8(any);
+}
+
+static inline bool IsNumber(const Any& any)
+{
+    return IsBool(any) || IsInteger(any) || IsFloat(any) || IsDouble(any)
+        || IsLongDouble(any);
+}
+
+static inline bool IsBoolType(const std::type_info& t)
+{
+    return t == typeid(bool);
+}
+
+static inline bool IsInt8Type(const std::type_info& t)
+{
+    return t == typeid(int8_t);
+}
+
+static inline bool IsUint8Type(const std::type_info& t)
+{
+    return t == typeid(uint8_t);
+}
+
+static inline bool IsInt16Type(const std::type_info& t)
+{
+    return t == typeid(int16_t);
+}
+
+static inline bool IsUint16Type(const std::type_info& t)
+{
+    return t == typeid(uint16_t);
+}
+
+static inline bool IsIntType(const std::type_info& t)
+{
+    return t == typeid(int32_t);
+}
+
+static inline bool IsUintType(const std::type_info& t)
+{
+    return t == typeid(uint32_t);
+}
+
+static inline bool IsInt64Type(const std::type_info& t)
+{
+    return t == typeid(int64_t);
+}
+
+static inline bool IsUint64Type(const std::type_info& t)
+{
+    return t == typeid(uint64_t);
+}
+
+static inline bool IsIntegerType(const std::type_info& t)
+{
+    return IsIntType(t) || IsUintType(t) || IsInt64Type(t) || IsUint64Type(t) ||
+           IsInt8Type(t) || IsUint8Type(t) || IsInt16Type(t) || IsUint16Type(t);
+}
+
+static inline bool IsFloatType(const std::type_info& t)
+{
+    return t == typeid(float);
+}
+
+static inline bool IsDoubleType(const std::type_info& t)
+{
+    return t == typeid(double);
+}
+
+static inline bool IsLongDoubleType(const std::type_info& t)
+{
+    return t == typeid(long double);
+}
+
+static inline bool IsNumberType(const std::type_info& t)
+{
+    return IsBoolType(t) || IsIntegerType(t) || IsFloatType(t)
+        || IsDoubleType(t) || IsLongDoubleType(t);
+}
+
+static inline bool IsTypeEqual(const std::type_info& t1, const std::type_info& t2)
+{
+    return t1 == t2;
+}
+
+template<typename ValueType>
+const ValueType& AnyCast(const Any* operand)
+{
+    const std::type_info& t1 = typeid(ValueType);
+    const std::type_info& t2 = operand->GetTypeInfo();
+
+    if (IsTypeEqual(t1, t2))
+    {
+        return ((Any::Holder<ValueType>*)(operand->mContent))->mValue;
+    }
+
+    UT_THROW(BadCastException, std::string("AnyCast error. target type is ")
+        + t1.name() + ", but source type is " + t2.name());
+}
+
+template<typename ValueType>
+const ValueType& AnyCast(const Any& operand)
+{
+    return AnyCast<ValueType>(&operand);
+}
+
+template<typename ValueType>
+ValueType AnyNumberCast(const Any* operand)
+{
+    const std::type_info& t1 = typeid(ValueType);
+    const std::type_info& t2 = operand->GetTypeInfo();
+
+    if (IsNumberType(t1) && IsNumberType(t2))
+    {
+        if (IsTypeEqual(t1, t2))
+        {
+            return ((Any::Holder<ValueType>*)(operand->mContent))->mValue;
+        }
+        else if (IsFloatType(t2))
+        {
+            return (ValueType)((Any::Holder<float>*)(operand->mContent))->mValue;
+        }
+        else if (IsDoubleType(t2))
+        {
+            return (ValueType)((Any::Holder<double>*)(operand->mContent))->mValue;
+        }
+        else if (IsLongDoubleType(t2))
+        {
+            return (ValueType)((Any::Holder<long double>*)(operand->mContent))->mValue;
+        }
+        else if (IsInt8Type(t2))
+        {
+            return (ValueType)((Any::Holder<int8_t>*)(operand->mContent))->mValue;
+        }
+        else if (IsUint8Type(t2))
+        {
+            return (ValueType)((Any::Holder<uint8_t>*)(operand->mContent))->mValue;
+        }
+        else if (IsInt16Type(t2))
+        {
+            return (ValueType)((Any::Holder<int16_t>*)(operand->mContent))->mValue;
+        }
+        else if (IsUint16Type(t2))
+        {
+            return (ValueType)((Any::Holder<uint16_t>*)(operand->mContent))->mValue;
+        }
+        else if (IsIntType(t2))
+        {
+            return (ValueType)((Any::Holder<int32_t>*)(operand->mContent))->mValue;
+        }
+        else if (IsUintType(t2))
+        {
+            return (ValueType)((Any::Holder<uint32_t>*)(operand->mContent))->mValue;
+        }
+        else if (IsInt64Type(t2))
+        {
+            return (ValueType)((Any::Holder<int64_t>*)(operand->mContent))->mValue;
+        }
+        else if (IsUint64Type(t2))
+        {
+            return (ValueType)((Any::Holder<uint64_t>*)(operand->mContent))->mValue;
+        }
+        else
+        {
+            UT_THROW(BadCastException, std::string("AnyNumberCast error. unknown number type:", t2.name()));
+        }
+    }
+
+    UT_THROW(BadCastException, std::string("AnyNumberCast error. not number type"));
+}
+
+template<typename ValueType>
+ValueType AnyNumberCast(const Any& operand)
+{
+    return AnyNumberCast<ValueType>(&operand);
+}
+
+static inline const std::string& ToString(const Any& operand)
+{
+    if (operand.Empty())
+    {
+        return UT_EMPTY_STR;
+    }
+
+    return AnyCast<std::string>(operand);
+}
+
+static inline void StringTo(const std::string& s, Any& value)
+{
+    value = s;
+}
+
+static inline void StringTo(const char* s, Any& value)
+{
+    value = std::string(s);
+}
+
+static inline void StringTo(const char* s, size_t len, Any& value)
+{
+    value = std::string(s, len);
+}
+
+static inline void StringTo(const char* s, size_t pos, size_t len, Any& value)
+{
+    value = std::string(s, pos, len);
+}
+
+}
+}
+#endif//__UT_ANY_HPP__

unitree_SDK/include/unitree/common/assert.hpp

@@ -0,0 +1,149 @@
+#ifndef __UT_ASSERT_HPP__
+#define __UT_ASSERT_HPP__
+
+#include <unitree/common/decl.hpp>
+
+/*
+ * Declare assert output
+ */
+#define UT_ASSERT_OUT(debug, file, func, line, r)           \
+    if (debug)                                              \
+    {                                                       \
+        std::cout << "[" << ::time(NULL)                    \
+            << "] [" << ::syscall(SYS_gettid)               \
+            << "] UT_ASSERT DEBUG at __FILE__:" << file     \
+            << ", __FUNCTION__:" << func                    \
+            << ", __LINE__:" << line                        \
+            << ", r:" << r                                  \
+            << ", errno:" << errno                          \
+            << std::endl;                                   \
+    }                                                       \
+    else                                                    \
+    {                                                       \
+        std::cout << "[" << ::time(NULL)                    \
+            << "] [" << ::syscall(SYS_gettid)               \
+            << "] UT_ASSERT ABORT at __FILE__:" << file     \
+            << ", __FUNCTION__:" << func                    \
+            << ", __LINE__:" << line                        \
+            << ", r:" << r                                  \
+            << ", errno:" << errno                          \
+            << std::endl;                                   \
+    }
+
+#define UT_ASSERT_ABORT(debug, file, func, line, r)         \
+    if (debug)                                              \
+    {                                                       \
+        UT_ASSERT_OUT(1, file, func, line, r);              \
+    }                                                       \
+    else                                                    \
+    {                                                       \
+        UT_ASSERT_OUT(0, file, func, line, r);              \
+        abort();                                            \
+    }
+
+/*
+ * Declare assert return value
+ */
+#define UT_ASSERT_EQ(x, r)                                  \
+    unitree::common::AssertEqual(x, r, 0, __FILE__,                 \
+        __PRETTY_FUNCTION__, __LINE__)
+
+#define UT_ASSERT_EQ_DEBUG(x, r)                            \
+    unitree::common::AssertEqual(x, r, 1, __FILE__,                 \
+        __PRETTY_FUNCTION__, __LINE__)
+
+#define UT_ASSERT_NOT_EQ(x, r)                              \
+    unitree::common::AssertNotEqual(x, r, 0, __FILE__,              \
+        __PRETTY_FUNCTION__, __LINE__)
+
+#define UT_ASSERT_NOT_EQ_DEBUG(x, r)                        \
+    unitree::common::AssertNotEqual(x, r, 1, __FILE__,              \
+        __PRETTY_FUNCTION__, __LINE__)
+
+/*
+ * Declare assert return value and errno
+ */
+#define UT_ASSERT_ENO_EQ(x, r, eno)                         \
+    unitree::common::AssertEqual(x, r, eno, 0, __FILE__,            \
+        __PRETTY_FUNCTION__, __LINE__)
+
+#define UT_ASSERT_ENO_EQ_DEBUG(x, r, eno)                   \
+    unitree::common::AssertEqual(x, r, eno, 1, __FILE__,            \
+        __PRETTY_FUNCTION__, __LINE__)
+
+#define UT_ASSERT_ENO_EQ_EX(x, r, eno)                      \
+    unitree::common::AssertEqualEx(x, r, eno, 0, __FILE__,          \
+        __PRETTY_FUNCTION__, __LINE__)
+
+#define UT_ASSERT_ENO_EQ_EX_DEBUG(x, r, eno)                \
+    unitree::common::AssertEqualEx(x, r, eno, 1, __FILE__,          \
+        __PRETTY_FUNCTION__, __LINE__)
+
+/*
+ * Declare assert wrapper
+ */
+#define UT_ASSERT_0(x)                  \
+    UT_ASSERT_EQ(x, 0)
+
+#define UT_ASSERT_DEBUG_0(x)            \
+    UT_ASSERT_EQ_DEBUG(x, 0)
+
+#define UT_ASSERT_ENO_0(x, eno)         \
+    UT_ASSERT_ENO_EQ(x, 0, eno)
+
+#define UT_ASSERT_ENO_DEBUG_0(x, eno)   \
+    UT_ASSERT_ENO_EQ_DEBUG(x, 0, eno)
+
+
+//Declare assert function
+namespace unitree
+{
+namespace common
+{
+inline int AssertEqual(int r, int expectRet, bool debug,
+    const char* file, const char* func, int line)
+{
+    if (UT_UNLIKELY(r != expectRet))
+    {
+        UT_ASSERT_ABORT(debug, file, func, line, r);
+    }
+
+    return r;
+}
+
+inline int AssertNotEqual(int r, int expectRet, bool debug,
+    const char* file, const char* func, int line)
+{
+    if (UT_UNLIKELY(r == expectRet))
+    {
+        UT_ASSERT_ABORT(debug, file, func, line, r);
+    }
+
+    return r;
+}
+
+inline int AssertEqual(int r, int expectRet, int expectErrno, bool debug,
+    const char* file, const char* func, int line)
+{
+    if (UT_UNLIKELY(r != expectRet) && UT_UNLIKELY(errno != expectErrno))
+    {
+        UT_ASSERT_ABORT(debug, file, func, line, r);
+    }
+
+    return r;
+}
+
+inline int AssertEqualEx(int r, int expectRet, int expectErrno, bool debug,
+    const char* file, const char* func, int line)
+{
+    if (UT_UNLIKELY(r != 0) && UT_UNLIKELY(r != expectRet) && 
+        UT_UNLIKELY(errno != expectErrno))
+    {
+        UT_ASSERT_ABORT(debug, file, func, line, r);
+    }
+
+    return r; 
+}
+}
+}
+#endif//__UT_ASSERT_HPP__

unitree_SDK/include/unitree/common/block_queue.hpp

@@ -0,0 +1,137 @@
+#ifndef __UT_BLOCK_QUEUE_HPP__
+#define __UT_BLOCK_QUEUE_HPP__
+
+#include <unitree/common/exception.hpp>
+#include <unitree/common/lock/lock.hpp>
+
+namespace unitree
+{
+namespace common
+{
+template<typename T>
+class BlockQueue
+{
+public:
+    BlockQueue(uint64_t maxSize = UT_QUEUE_MAX_LEN) :
+        mMaxSize(maxSize), mCurSize(0)
+    {
+        if (mMaxSize == 0)
+        {
+            mMaxSize = UT_QUEUE_MAX_LEN;
+        }
+    }
+
+    bool Put(const T& t, bool replace = false, bool putfront = false)
+    {
+        /*
+         * if queue is full or full-replaced occured return false
+         */
+        bool noneReplaced = true;
+
+        LockGuard<MutexCond> guard(mMutexCond);
+        if (mCurSize >= mMaxSize)
+        {
+            if (!replace)
+            {
+                return false;
+            }
+
+            noneReplaced = false;
+
+            mQueue.pop_front();
+            mCurSize --;
+        }
+
+        if (putfront)
+        {
+            mQueue.emplace_front(t);
+        }
+        else
+        {
+            mQueue.emplace_back(t);
+        }
+
+        mCurSize ++;
+        mMutexCond.Notify();
+
+        return noneReplaced;
+    }
+
+    bool Get(T& t, uint64_t microsec = 0)
+    {
+        LockGuard<MutexCond> guard(mMutexCond);
+        return GetTimeout(t, microsec);
+    }
+
+    T Get(uint64_t microsec = 0)
+    {
+        LockGuard<MutexCond> guard(mMutexCond);
+        T t;
+        if (GetTimeout(t, microsec))
+        {
+            return std::move(t);
+        }
+
+        UT_THROW(TimeoutException, "block queue get timeout or interrupted");
+    }
+
+    bool Empty()
+    {
+        return mCurSize == 0;
+    }
+
+    uint64_t Size()
+    {
+        return mCurSize;
+    }
+
+    void Interrupt(bool all = false)
+    {
+        LockGuard<MutexCond> guard(mMutexCond);
+        if (all)
+        {
+            mMutexCond.NotifyAll();
+        }
+        else
+        {
+            mMutexCond.Notify();
+        }
+    }
+
+private:
+    bool GetTimeout(T& t, uint64_t microsec = 0)
+    {
+        if (mQueue.empty())
+        {
+            if (!mMutexCond.Wait(microsec))
+            {
+                return false;
+            }
+        
+            if (mQueue.empty())
+            {
+                return false;
+            }
+        }
+
+        t = mQueue.front();
+        mQueue.pop_front();
+
+        mCurSize--;
+
+        return true;
+    }
+
+private:
+    uint64_t mMaxSize;
+    uint64_t mCurSize;
+    std::list<T> mQueue;
+    MutexCond mMutexCond;
+};
+
+template <typename T>
+using BlockQueuePtr = std::shared_ptr<BlockQueue<T>>;
+
+}
+}
+#endif//__UT_BLOCK_QUEUE_HPP__

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__

unitree_SDK/include/unitree/common/decl.hpp

@@ -0,0 +1,92 @@
+#ifndef __UT_DECL_HPP__
+#define __UT_DECL_HPP__
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+#include <exception>
+#include <execinfo.h>
+#include <sched.h>
+#include <signal.h>
+#include <string.h>
+#include <strings.h>
+#include <arpa/inet.h>
+#include <ifaddrs.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/statvfs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <sys/sysinfo.h>
+#include <sys/syscall.h>
+#include <sys/resource.h>
+#include <sys/timerfd.h>
+#include <net/if.h>
+#include <netinet/in.h>
+#include <netdb.h>
+#include <time.h>
+#include <poll.h>
+#include <pthread.h>
+#include <pwd.h>
+#include <limits.h>
+#include <fcntl.h>
+#include <dirent.h>
+#include <utime.h>
+#include <atomic>
+#include <string>
+#include <iostream>
+#include <sstream>
+#include <typeinfo>
+#include <list>
+#include <vector>
+#include <set>
+#include <map>
+#include <unordered_map>
+#include <functional>
+#include <iomanip>
+#include <memory>
+#include <regex>
+
+#ifdef __GLIBC__
+#define UT_UNLIKELY(x)  __builtin_expect(!!(x), 0)
+#define UT_LIKELY(x)    __builtin_expect(!!(x), 1)
+#else
+#define UT_UNLIKELY(x)  (x)
+#define UT_LIKELY(x)    (x)
+#endif//__GLIBC__
+
+#define __UT_CAT(x, y)  x##y
+#define UT_CAT(x, y)    __UT_CAT(x, y)
+
+#define __UT_STR(x)     #x
+#define UT_STR(x)       __UT_STR(x)
+
+#define UT_QUEUE_MAX_LEN        INT_MAX
+#define UT_PATH_MAX_LEN         1024
+#define UT_THREAD_NAME_MAX_LEN  16
+
+#define UT_DECL_ERR(name, code, desc)   \
+    const int32_t name = code; const std::string name##_DESC = desc;
+
+#define UT_DESC_ERR(name) name##_DESC
+
+#ifndef SYS_gettid
+#define SYS_gettid __NR_gettid
+#endif//SYS_gettid
+
+#define UT_SAFE_DEL(x)  \
+    if ((x) != NULL) { delete (x); (x) = NULL; }
+
+namespace unitree
+{
+namespace common
+{
+static const std::string UT_EMPTY_STR = "";
+}
+}
+
+#endif//__UT_DECL_HPP__

unitree_SDK/include/unitree/common/error.hpp

@@ -0,0 +1,25 @@
+#ifndef __UT_ERROR_HPP__
+#define __UT_ERROR_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree
+{
+//Declare error
+UT_DECL_ERR(UT_OK,              0,      "Success")
+UT_DECL_ERR(UT_ERR_COMMON,      1001,   "common error")
+UT_DECL_ERR(UT_ERR_BADCAST,     1002,   "Bad cast error")
+UT_DECL_ERR(UT_ERR_FUTURE,      1003,   "Future error")
+UT_DECL_ERR(UT_ERR_FUTURE_FAULT,1004,   "Future fault error")
+UT_DECL_ERR(UT_ERR_JSON,        1005,   "Json data error")
+UT_DECL_ERR(UT_ERR_SYSTEM,      1006,   "System error")
+UT_DECL_ERR(UT_ERR_FILE,        1007,   "File operation error")
+UT_DECL_ERR(UT_ERR_SOCKET,      1008,   "Socket operaton error")
+UT_DECL_ERR(UT_ERR_IO,          1009,   "IO operaton error")
+UT_DECL_ERR(UT_ERR_LOCK,        1010,   "Lock operation error")
+UT_DECL_ERR(UT_ERR_NETWORK,     1011,   "Network error")
+UT_DECL_ERR(UT_ERR_TIMEOUT,     1012,   "Timeout error")
+UT_DECL_ERR(UT_ERR_UNKNOWN,     -1,     "Unknown error")
+}
+
+#endif//__UT_ERROR_HPP__

unitree_SDK/include/unitree/common/exception.hpp

@@ -0,0 +1,203 @@
+#ifndef __UT_EXCEPTION_HPP__
+#define __UT_EXCEPTION_HPP__
+
+#include <unitree/common/error.hpp>
+
+#define UT_MAX_TRACE_ADDR_NUMBER 64
+
+namespace unitree
+{
+namespace common
+{
+class Exception : public std::exception
+{
+public:
+    Exception() throw()
+        : Exception(UT_ERR_UNKNOWN, UT_DESC_ERR(UT_ERR_UNKNOWN))
+    {}
+
+    Exception(int32_t code, const std::string& message) throw()
+        : mCode(code), mMessage(message), mLine(0)
+    {}
+
+    virtual ~Exception()
+    {}
+
+    int32_t GetCode() const
+    {
+        return mCode;
+    }
+
+    const std::string& GetMessage() const
+    {
+        return mMessage;
+    }
+
+    virtual const char* what() const noexcept
+    {
+        return mMessage.c_str();
+    }
+
+    virtual std::string GetClassName() const
+    {
+        return "Exception";
+    }
+
+    void Init(const char* file, const char* func, int32_t line)
+    {
+        mFile = file;
+        mFunc = func;
+        mLine = line;
+    }
+
+    std::string ToString() const
+    {
+        std::ostringstream os;
+        AddDetail(os);
+
+        return os.str();
+    }
+
+    std::string StackTrace() const
+    {
+        std::ostringstream os;
+        AddDetail(os);
+        AddBackTrace(os);
+
+        return os.str();
+    }
+
+protected:
+    void AddDetail(std::ostringstream& os) const
+    {
+        os << "Catch " << GetClassName() << " code:" << mCode
+            << ", message:" << mMessage << std::endl;
+
+        if (!mFile.empty() && !mFunc.empty() && mLine > 0)
+        {
+            os << " at __FILE__:" << mFile << ", __LINE__:"
+                << mLine << ", __FUNCTION__:" << mFunc << std::endl;
+        }
+    }
+
+    void AddBackTrace(std::ostringstream& os) const
+    {
+        os << "Stack:" << std::endl;
+
+        void* addr[UT_MAX_TRACE_ADDR_NUMBER];
+        int32_t number = backtrace(addr, UT_MAX_TRACE_ADDR_NUMBER);
+
+        char **info = backtrace_symbols(addr, number);
+        if(info == NULL)
+        {
+            return;
+        }
+
+        for(int32_t i=0; i<number; i++)
+        {
+            os << info[i] << std::endl;
+        }
+
+        free(info);
+    }
+
+protected:
+    int32_t mCode;
+    std::string mMessage;
+
+    std::string mFile;
+    std::string mFunc;
+    int32_t mLine;
+
+    std::string mWhat;
+};
+
+#define UT_THROW_0(ExceptionType)           \
+    do                                      \
+    {                                       \
+        ExceptionType __temp_except_r38e2d; \
+        __temp_except_r38e2d.Init(__FILE__,__PRETTY_FUNCTION__,__LINE__); \
+        throw(__temp_except_r38e2d);        \
+    } while(0);
+
+#define UT_THROW(ExceptionType, args...)    \
+    do                                      \
+    {                                       \
+        ExceptionType __temp_except_r38e2d(args);       \
+        __temp_except_r38e2d.Init(__FILE__,__PRETTY_FUNCTION__,__LINE__); \
+        throw(__temp_except_r38e2d);        \
+    } while(0);
+
+#define UT_THROW_IF(condition, ExceptionType, args...)  \
+    if (condition)                          \
+    {                                       \
+        UT_THROW(ExceptionType, args);      \
+    }
+
+#define UT_EXCEPTION_TRY                    \
+    try                                     \
+    {
+
+#define __UT_EXCEPTION_CATCH(except, l, t)  \
+    catch (const except& e)                 \
+    {                                       \
+        if (l)                              \
+        {                                   \
+            LOG_ERROR(l, e.what());         \
+        }                                   \
+        if (t)                              \
+        {                                   \
+            throw e;                        \
+        }                                   \
+    }
+
+#define UT_EXCEPTION_CATCH(l, t)            \
+    }                                       \
+    __UT_EXCEPTION_CATCH(unitree::common::Exception, l, t)      \
+    __UT_EXCEPTION_CATCH(std::exception, l, t)
+
+#define UT_DECL_EXCEPTION(ExceptionType, code, desc)    \
+class ExceptionType : public unitree::common::Exception \
+{                                           \
+public:                                     \
+    ExceptionType() throw()                 \
+        : Exception(code, desc)             \
+    {}                                      \
+                                            \
+    ExceptionType(const std::string& message) throw()   \
+        : Exception(code, message)          \
+    {}                                      \
+                                            \
+    std::string GetClassName() const        \
+    {                                       \
+        return #ExceptionType;              \
+    }                                       \
+};
+
+UT_DECL_EXCEPTION(CommonException, UT_ERR_COMMON, UT_DESC_ERR(UT_ERR_COMMON))
+
+UT_DECL_EXCEPTION(SystemException, UT_ERR_SYSTEM, UT_DESC_ERR(UT_ERR_SYSTEM))
+
+UT_DECL_EXCEPTION(NetworkException, UT_ERR_NETWORK, UT_DESC_ERR(UT_ERR_NETWORK))
+
+UT_DECL_EXCEPTION(FileException, UT_ERR_FILE, UT_DESC_ERR(UT_ERR_FILE))
+
+UT_DECL_EXCEPTION(SocketException, UT_ERR_SOCKET, UT_DESC_ERR(UT_ERR_SOCKET))
+
+UT_DECL_EXCEPTION(IOException, UT_ERR_IO, UT_DESC_ERR(UT_ERR_IO))
+
+UT_DECL_EXCEPTION(LockException, UT_ERR_LOCK, UT_DESC_ERR(UT_ERR_LOCK))
+
+UT_DECL_EXCEPTION(TimeoutException, UT_ERR_TIMEOUT, UT_DESC_ERR(UT_ERR_TIMEOUT))
+
+UT_DECL_EXCEPTION(BadCastException, UT_ERR_BADCAST, UT_DESC_ERR(UT_ERR_BADCAST))
+
+UT_DECL_EXCEPTION(JsonException, UT_ERR_JSON, UT_DESC_ERR(UT_ERR_JSON))
+
+UT_DECL_EXCEPTION(FutureException, UT_ERR_FUTURE, UT_DESC_ERR(UT_ERR_FUTURE))
+
+UT_DECL_EXCEPTION(FutureFaultException, UT_ERR_FUTURE_FAULT, UT_DESC_ERR(UT_ERR_FUTURE_FAULT))
+
+}
+}
+#endif//__UT_EXCEPTION_HPP__

unitree_SDK/include/unitree/common/filesystem/directory.hpp

@@ -0,0 +1,68 @@
+#ifndef __UT_DIRECTORY_HPP__
+#define __UT_DIRECTORY_HPP__
+
+#include <unitree/common/filesystem/filesystem.hpp>
+
+namespace unitree
+{
+namespace common
+{
+typedef std::pair<int32_t,std::string> DIRENT_INFO;
+typedef std::pair<std::string,std::string> SYMLNK_INFO;
+
+class Directory
+{
+public:
+    Directory();
+    Directory(const std::string& dirName);
+
+    ~Directory();
+
+    void Open();
+    void Open(const std::string& dirName);
+
+    bool IsOpen();
+
+    void ListFile(std::vector<std::string>& fileNameList, const std::string& regExpress);
+    void ListFile(std::vector<std::string>& fileNameList, bool recurse = true, bool absolute = true);
+
+    void ListDir(std::vector<std::string>& dirNameList, bool recurse = true, bool absolute = true);
+
+    void List(std::list<std::string>& fileNameList, std::list<std::string>& dirNameList,
+        std::list<SYMLNK_INFO>& symlinkList);
+
+    void Cleanup();
+    bool CopyTo(const std::string& dirName, bool deeply = false);
+
+    void Close();
+
+private:
+    void CheckOpen();
+    void OpenInner();
+
+private:
+    DIR *mDIR;
+    std::string mDirName;
+};
+
+typedef std::shared_ptr<Directory> DirectoryPtr;
+
+bool ExistDirectory(const std::string& dirName);
+
+void CreateDirectory(const std::string& dirName, bool recurse = true,
+    uint32_t mode = UT_OPEN_MODE_RWX);
+
+void ListDirectory(const std::string& dirName, std::vector<std::string>& fileNameList,
+    const std::string& regExpress);
+
+void ListDirectory(const std::string& dirName, std::vector<std::string>& fileNameList, bool recurse = true, bool absolute = true);
+
+void ListChildDirectory(const std::string& dirName, std::vector<std::string>& dirNameList, bool recurse = true, bool absolute = true);
+
+void RemoveDirectory(const std::string& dirName, bool recurse = true);
+
+void CopyDirectory(const std::string& dirName, const std::string& destDirName, bool deeply);
+
+}
+}
+#endif//__UT_DIRECTORY_HPP__

unitree_SDK/include/unitree/common/filesystem/file.hpp

@@ -0,0 +1,127 @@
+#ifndef __UT_FILE_HPP__
+#define __UT_FILE_HPP__
+
+#include <unitree/common/filesystem/filesystem.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class File
+{
+public:
+    File();
+    File(const std::string& fileName);
+    File(const std::string& fileName, int32_t flag);
+    File(const std::string& fileName, int32_t flag, uint32_t mode);
+
+    virtual ~File();
+
+    int32_t GetFd() const;
+    bool IsOpen() const;
+
+    void Open();
+    void Open(int32_t flag, uint32_t mode = UT_OPEN_MODE_NONE);
+    void Open(const std::string& fileName, int32_t flag = UT_OPEN_FLAG_R,
+        uint32_t mode = UT_OPEN_MODE_NONE);
+
+    void Append(const char* s, int64_t len);
+    int64_t Write(const char* s, int64_t len);
+    int64_t Write(const std::string& s);
+
+    int64_t Read(char* s, int64_t len);
+    int64_t Read(std::string& s, int64_t len);
+    int64_t ReadAll(std::string& s);
+
+    void SeekBegin();
+    void SeekEnd();
+    void Seek(int64_t offset, int64_t whence);
+
+    void Sync();
+    void Truncate(int64_t len);
+
+    void Close();
+
+    int64_t Size() const;
+
+private:
+    void CheckOpen();
+    void OpenInner();
+
+private:
+    std::string mFileName;
+    int32_t mFd;
+    int32_t mFlag;
+    uint32_t mMode;
+};
+
+typedef std::shared_ptr<File> FilePtr;
+
+class MMReadFile
+{
+public:
+    MMReadFile();
+    MMReadFile(const std::string& fileName);
+
+    virtual ~MMReadFile();
+
+    int32_t GetFd() const;
+    bool IsOpen() const;
+
+    void Open();
+    void Open(const std::string& fileName);
+
+    int64_t Size() const;
+    int64_t Read(char* s, int64_t len);
+    int64_t Read(std::string& s, int64_t len);
+    int64_t ReadAll(std::string& s);
+
+    void Seek(int64_t offset);
+
+    void Close();
+
+public:
+    void* MMRead(int64_t len, int64_t& canreadlen);
+    void MMClose(void* ptr, int64_t len);
+
+private:
+    void Init();
+
+private:
+    std::string mFileName;
+    int32_t mFd;
+    int64_t mOffset;
+    int64_t mSize;
+};
+
+typedef std::shared_ptr<MMReadFile> MMReadFilePtr;
+
+//Functions
+bool ExistFile(const std::string& fileName);
+
+std::string LoadFile(const std::string& fileName);
+std::string LoadFileEx(const std::string& fileName);
+
+void SaveFile(const std::string& fileName, const char* s, int64_t len,
+    uint32_t mode = UT_OPEN_MODE_RW);
+
+void SaveFile(const std::string& fileName, const std::string& s,
+    uint32_t mode = UT_OPEN_MODE_RW);
+
+void AppendFile(const std::string& fileName, const char* s, int64_t len,
+    uint32_t mode = UT_OPEN_MODE_RW);
+
+void AppendFile(const std::string& fileName, const std::string& s,
+    uint32_t mode = UT_OPEN_MODE_RW);
+
+int64_t GetFileSize(const std::string& fileName);
+
+void RemoveFile(const std::string& fileName);
+
+int64_t MMLoadFile(const std::string& fileName, std::string& s);
+
+bool CopyFile(const std::string& fileName, const std::string& destFileName, bool deeply = false);
+}
+}
+
+#endif//__UT_FILE_HPP__

unitree_SDK/include/unitree/common/filesystem/filesystem.hpp

@@ -0,0 +1,284 @@
+#ifndef __UT_FILE_SYSTEM_HPP__
+#define __UT_FILE_SYSTEM_HPP__
+
+#include <unitree/common/decl.hpp>
+
+#define UT_FILE_READ_SIZE           65536 //64K
+#define UT_FILE_READ_BIGGER_SIZE    262144 //256K
+
+#define UT_FD_INVALID   -1
+#define UT_FD_STDIN     STDIN_FILENO
+#define UT_FD_STDOUT    STDOUT_FILENO
+#define UT_FD_STDERR    STDERR_FILENO
+
+#define UT_PATH_DELIM_CHAR  '/'
+#define UT_PATH_DELIM_STR   "/"
+#define UT_PATH_TRIM_DELIM_STR   " \t\r\n"
+
+#define NOT_FLAG(f, flag)   \
+    (((f) & (flag)) != (flag))
+
+#define HAS_FLAG(f, flag)   \
+    (((f) & (flag)) == (flag))
+    
+#define SET_FLAG(f, flag)   \
+    (f) |= (flag)
+
+#define IS_RDONLY_FLAG(f) \
+    ((f & O_ACCMODE) == O_RDONLY)
+
+namespace unitree
+{
+namespace common
+{
+enum
+{
+    UT_OPEN_FLAG_R         = O_RDONLY,
+    UT_OPEN_FLAG_W         = O_WRONLY,
+    UT_OPEN_FLAG_RW        = O_RDWR,
+    UT_OPEN_FLAG_T         = O_TRUNC,
+    UT_OPEN_FLAG_A         = O_APPEND,
+    UT_OPEN_FLAG_S         = O_SYNC,
+    UT_OPEN_FLAG_C         = O_CREAT,
+    UT_OPEN_FLAG_CW        = O_CREAT | O_WRONLY,
+    UT_OPEN_FLAG_CWT       = O_CREAT | O_WRONLY | O_TRUNC,
+    UT_OPEN_FLAG_CA        = O_CREAT | O_RDWR | O_APPEND,
+    UT_OPEN_FLAG_CS        = O_CREAT | O_RDWR | O_SYNC,
+    UT_OPEN_FLAG_CAS       = O_CREAT | O_RDWR | O_APPEND | O_SYNC,
+    UT_OPEN_FLAG_CTS       = O_CREAT | O_RDWR | O_TRUNC | O_SYNC
+};
+
+enum
+{
+    UT_OPEN_MODE_NONE = 0,
+    UT_OPEN_MODE_RW   = S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH,
+    UT_OPEN_MODE_RWX  = S_IRWXU | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH
+};
+
+enum
+{
+    UT_OPEN_MASK_000 = 0,
+    UT_OPEN_MASK_022 = 0022
+};
+
+typedef struct dirent DIRENT;
+
+/*
+ * FileSystemHelper
+ */
+class FileSystemHelper
+{
+public:
+    static FileSystemHelper* Instance()
+    {
+        static FileSystemHelper inst;
+        return &inst;
+    }
+
+    /*
+     * File Functions
+     */
+    int32_t Open(const std::string& fileName, int32_t flag, uint32_t mode = 0);
+
+    int64_t Write(int32_t fd, const char* s, int64_t len);
+    int64_t Write(int32_t fd, const std::string& s);
+
+    int64_t Read(int32_t fd, char* s, int64_t len);
+    int64_t Read(int32_t fd, std::string& s, int64_t len);
+    int64_t ReadAll(int32_t fd, std::string& s);
+
+    int64_t Seek(int32_t fd, int64_t offset, int64_t whence);
+
+    bool Stat(int32_t fd, struct stat& statbuf);
+    void Truncate(int32_t fd, int64_t len);
+    void Sync(int32_t fd);
+    void Close(int32_t fd);
+
+    /*
+     * Directory Functions
+     */
+    DIR* Opendir(const std::string& dirName);
+    DIRENT* Readdir(DIR* dir);
+    void Closedir(DIR* dir);
+
+    void Makedir(const std::string& dirName, uint32_t mode = UT_OPEN_MODE_RWX,
+        bool ignoreExist = true);
+    void MakedirRecurse(const std::string& dirName, uint32_t mode = UT_OPEN_MODE_RWX);
+
+    /*
+     * stat Function
+     */
+    bool Stat(const std::string& name, struct stat& statbuf);
+    bool StatL(const std::string& name, struct stat& statbuf);
+    bool IsFile(const struct stat& statbuf);
+    bool IsDirectory(const struct stat& statbuf);
+    bool IsSymlink(const struct stat& statbuf);
+    bool ExistFile(const std::string& name);
+    bool ExistDirectory(const std::string& name);
+    bool Exist(const std::string& name);
+
+    int64_t GetFileSize(const std::string& fileName);
+
+    /*
+     * Symlink Function
+     */
+    std::string GetSymlink(const std::string& symlinkName);
+    bool Symlink(const std::string& targetFileName, const std::string& symlinkName);
+
+    /*
+     * Remove and Rename Function
+     */
+    void RemoveFile(const std::string& fileName, bool ignoreNoExist = true);
+    void RemoveDirectory(const std::string& dirName, bool ignoreNoExist = true);
+
+    void Remove(const std::string& name, bool ignoreNoExist = true);
+    void Rename(const std::string& oldName, const std::string& newName);
+
+    /*
+     * Path proccess Function
+     */
+    std::string& NormalizePath(std::string& s, bool withEndDelim = true);
+    std::string GetFatherDirectory(const std::string& s, bool withEndDelim = true);
+    std::string GetFileName(const std::string& s);
+    std::string GetLastName(const std::string& s);
+
+    /*
+     * Realpath
+     */
+    std::string GetRealName(const std::string& name);
+
+    bool IsSame(const std::string& name1, const std::string& name2);
+
+    /*
+     * MMap Function
+     */
+    void* MmapRead(int32_t fd, int64_t offset, int64_t len);
+    void Munmap(void *addr, int64_t len);
+
+    /*
+     * Mode and Owner
+     */
+    bool Chmod(const std::string& name, mode_t mode);
+    bool Chown(const std::string& name, uid_t uid, gid_t gid);
+
+    bool ChmodL(const std::string& name, mode_t mode);
+    bool ChownL(const std::string& name, uid_t uid, gid_t gid);
+
+    bool UTime(const std::string& name, struct utimbuf& times);
+    bool UTime(const std::string& name, struct timeval times[2]);
+
+    bool Chattr(const std::string& name, const struct stat& statbuf);
+
+    /*
+     * ParseModeString
+     * @param s: like "0755", "0666", "766"
+     * return mode
+     */
+    uint32_t ParseModeString(const std::string& s);
+
+private:
+    FileSystemHelper();
+    ~FileSystemHelper();
+
+private:
+    int32_t mOldMask;
+};
+
+/*
+ * FDCloser
+ */
+class FDCloser
+{
+public:
+    explicit FDCloser() :
+        mFd(UT_FD_INVALID)
+    {}
+
+    explicit FDCloser(int32_t fd) :
+        mFd(fd)
+    {}
+
+    ~FDCloser()
+    {
+        Close();
+    }
+
+    void SetFd(int32_t fd)
+    {
+        //close old fd
+        Close();
+        //set new fd
+        mFd = fd;
+    }
+
+private:
+    void Close()
+    {
+        if (mFd > 0)
+        {
+            FileSystemHelper::Instance()->Close(mFd);
+            mFd = UT_FD_INVALID;
+        }
+    }
+
+private:
+    int32_t mFd;
+};
+
+/*
+ * DIRCloser
+ */
+class DIRCloser
+{
+public:
+    explicit DIRCloser() :
+        mDIR(NULL)
+    {}
+
+    explicit DIRCloser(DIR* dir) :
+        mDIR(dir)
+    {}
+
+    ~DIRCloser()
+    {
+        Close();
+    }
+
+    void SetDir(DIR* dir)
+    {
+        //close old dir
+        Close();
+        //set new dir
+        mDIR = dir;
+    }
+
+private:
+    void Close()
+    {
+        if (mDIR != NULL)
+        {
+            FileSystemHelper::Instance()->Closedir(mDIR);
+            mDIR = NULL;
+        }
+    }
+
+private:
+    DIR* mDIR;
+};
+
+//Function
+void Remove(const std::string& name);
+void Rename(const std::string& oldName, const std::string& newName);
+
+std::string NormalizePath(const std::string& s, bool withEndDelim = false);
+std::string GetFatherDirectory(const std::string& s, bool withEndDelim = false);
+std::string GetFileName(const std::string& s);
+std::string GetLastName(const std::string& s);
+
+void Chattr(const std::string& name, const struct stat& statbuf);
+void Copyattr(const std::string& name, const std::string& templateName);
+}
+
+}
+
+#endif//__UT_FILE_SYSTEM_HPP__

unitree_SDK/include/unitree/common/json/json.hpp

@@ -0,0 +1,39 @@
+#ifndef __UT_JSON_HPP__
+#define __UT_JSON_HPP__
+
+#include <unitree/common/any.hpp>
+
+namespace unitree
+{
+namespace common
+{
+typedef std::map<std::string,Any> JsonMap;
+typedef std::vector<Any> JsonArray;
+
+Any FromJsonString(const std::string& s);
+
+std::string ToJsonString(const Any& a, bool pretty = false);
+
+static inline bool IsNull(const Any& a)
+{
+    return a.Empty();
+}
+
+static inline bool IsJsonArray(const Any& a)
+{
+    return a.GetTypeInfo() == typeid(JsonArray);
+}
+
+static inline bool IsJsonMap(const Any& a)
+{
+    return a.GetTypeInfo() == typeid(JsonMap);
+}
+
+static inline bool IsJsonObject(const Any& a)
+{
+    return IsJsonMap(a);
+}
+
+}
+}
+#endif//__UT_JSON_HPP__

unitree_SDK/include/unitree/common/json/json_config.hpp

@@ -0,0 +1,194 @@
+#ifndef __UT_JSON_CONFIG_HPP__
+#define __UT_JSON_CONFIG_HPP__
+
+#include <unitree/common/json/json.hpp>
+
+#define UT_JSON_CONF_KEY_PARAMETER  "Parameter"
+
+namespace unitree
+{
+namespace common
+{
+class JsonConfig
+{
+public:
+    JsonConfig();
+    virtual ~JsonConfig();
+
+    JsonConfig(const std::string& configFileName);
+
+    virtual void Parse(const std::string& configFileName);
+    virtual void ParseContent(const std::string& content);
+
+    //top-level field
+    bool Has(const std::string& name) const;
+
+    //top-level field
+    const Any& Get(const std::string& name) const;
+
+    //top-level field
+    template<typename T>
+    const T& Get(const std::string& name) const
+    {
+        return AnyCast<T>(Get(name));
+    }
+
+    //top-level field
+    template<typename T>
+    T GetNumber(const std::string& name) const
+    {
+        return AnyNumberCast<T>(Get(name));
+    }
+
+    //top-level field
+    template<typename T>
+    T Get(const std::string& name, const T& defValue) const
+    {
+        const Any& a = Get(name);
+
+        if (a.Empty())
+        {
+            return defValue;
+        }
+        else
+        {
+            return AnyCast<T>(a);
+        }
+    }
+
+    //top-level field
+    template<typename T>
+    T GetNumber(const std::string& name, const T& defValue) const
+    {
+        const Any& a = Get(name);
+
+        if (a.Empty())
+        {
+            return defValue;
+        }
+        else
+        {
+            return AnyNumberCast<T>(a);
+        }
+    }
+
+    //JsonMap field
+    bool Has(const JsonMap& jsonMap, const std::string& name) const;
+
+    //JsonMap field
+    const Any& Get(const JsonMap& jsonMap, const std::string& name) const;
+
+    //JsonMap field
+    template<typename T>
+    const T& Get(const JsonMap& jsonMap, const std::string& name) const
+    {
+        return AnyCast<T>(Get(jsonMap, name));
+    }
+
+    //JsonMap field
+    template<typename T>
+    T GetNumber(const JsonMap& jsonMap, const std::string& name) const
+    {
+        return AnyNumberCast<T>(Get(jsonMap, name));
+    }
+
+    //JsonMap field
+    template<typename T>
+    T Get(const JsonMap& jsonMap, const std::string& name, const T& defValue) const
+    {
+        const Any& a = Get(jsonMap, name);
+
+        if (a.Empty())
+        {
+            return defValue;
+        }
+        else
+        {
+            return AnyCast<T>(a);
+        }
+    }
+
+    //JsonMap field
+    template<typename T>
+    T GetNumber(const JsonMap& jsonMap, const std::string& name, const T& defValue) const
+    {
+        const Any& a = Get(jsonMap, name);
+
+        if (a.Empty())
+        {
+            return defValue;
+        }
+        else
+        {
+            return AnyNumberCast<T>(a);
+        }
+    }
+
+    //top-level field
+    const Any& GetGlobalParameter(const std::string& name) const;
+
+    //top-level field: Parameter
+    bool HasParameter(const std::string& name) const;
+
+    //top-level field: Parameter
+    const JsonMap& GetParameter() const;
+
+    //get field/value from top-level field: Parameter
+    const Any& GetParameter(const std::string& name) const;
+
+    //get field/value from top-level field: Parameter
+    template<typename T>
+    const T& GetParameter(const std::string& name) const
+    {
+        return AnyCast<T>(GetParameter(name));
+    }
+
+    //get field/value from top-level field: Parameter
+    template<typename T>
+    T GetNumberParameter(const std::string& name) const
+    {
+        return AnyNumberCast<T>(GetParameter(name));
+    }
+
+    //get field/value from top-level field: Parameter
+    template<typename T>
+    T GetParameter(const std::string& name, const T& defValue) const
+    {
+        const Any& a = GetParameter(name);
+
+        if (a.Empty())
+        {
+            return defValue;
+        }
+        else
+        {
+            return AnyCast<T>(a);
+        }
+    }
+
+    template<typename T>
+    T GetNumberParameter(const std::string& name, const T& defValue) const
+    {
+        const Any& a = GetParameter(name);
+
+        if (a.Empty())
+        {
+            return defValue;
+        }
+        else
+        {
+            return AnyNumberCast<T>(a);
+        }
+    }
+
+protected:
+    JsonMap mParameter;
+    Any mContent;
+};
+
+typedef std::shared_ptr<JsonConfig> JsonConfigPtr;
+
+}
+}
+
+#endif//__UT_JSON_CONFIG_HPP__

unitree_SDK/include/unitree/common/json/jsonize.hpp

@@ -0,0 +1,275 @@
+#ifndef __UT_JSONIZE_HPP__
+#define __UT_JSONIZE_HPP__
+
+#define JN_FROM_WEAK(m, name, value) \
+    if (m.find(name) != m.end()){ JN_FROM(m, name, value); }
+
+#define JN_FROM(m, name, value) \
+    unitree::common::FromJson(m[name], value)
+
+#define JN_TO(m, name, value) \
+    unitree::common::ToJson(value, m[name])
+
+#include <unitree/common/json/json.hpp>
+
+namespace unitree
+{
+namespace common
+{
+template<typename T>
+void FromJson(const Any& a, T& t);
+
+template<typename T>
+void ToJson(const T& value, Any& a);
+
+template<typename T>
+void FromJsonString(const std::string& s, T& t)
+{
+    Any a = FromJsonString(s);
+    FromJson<T>(a, t);
+}
+
+template<typename T>
+std::string ToJsonString(const T& t, bool pretty = false)
+{
+    Any a;
+    ToJson<T>(t, a);
+    return ToJsonString(a, pretty);
+}
+
+class Jsonize
+{
+public:
+    virtual void toJson(JsonMap& a) const = 0;
+    virtual void fromJson(JsonMap& a) = 0;
+};
+
+void FromAny(const Any& a, int8_t& value);
+
+void FromAny(const Any& a, uint8_t& value);
+
+void FromAny(const Any& a, int16_t& value);
+
+void FromAny(const Any& a, uint16_t& value);
+
+void FromAny(const Any& a, int32_t& value);
+
+void FromAny(const Any& a, uint32_t& value);
+
+void FromAny(const Any& a, int64_t& value);
+
+void FromAny(const Any& a, uint64_t& value);
+
+void FromAny(const Any& a, float& value);
+
+void FromAny(const Any& a, double& value);
+
+void FromAny(const Any& a, bool& value);
+
+void FromAny(const Any& a, std::string& value);
+
+void FromAny(const Any& a, JsonMap& value);
+
+void FromAny(const Any& a, JsonArray& value);
+
+void FromAny(const Any& a, Jsonize& value);
+
+template<typename E>
+void FromAny(const Any& a, std::vector<E>& value)
+{
+    if (a.Empty())
+    {
+        return;
+    }
+
+    const JsonArray& arr = AnyCast<JsonArray>(a);
+    size_t i, count = arr.size();
+
+    for (i=0; i<count; i++)
+    {
+        E e;
+        const Any& a_in = arr[i];
+        FromJson<E>(a_in, e);
+        value.push_back(std::move(e));
+    }
+}
+
+template<typename E>
+void FromAny(const Any& a, std::list<E>& value)
+{
+    if (a.Empty())
+    {
+        return;
+    }
+
+    const JsonArray& arr = AnyCast<JsonArray>(a);
+    size_t i, count = arr.size();
+
+    for (i=0; i<count; i++)
+    {
+        E e;
+        const Any& a_in = arr[i];
+        FromJson<E>(a_in, e);
+        value.push_back(std::move(e));
+    }
+}
+
+template<typename E>
+void FromAny(const Any& a, std::set<E>& value)
+{
+    if (a.Empty())
+    {
+        return;
+    }
+
+    const JsonArray& arr = AnyCast<JsonArray>(a);
+    size_t i, count = arr.size();
+
+    for (i=0; i<count; i++)
+    {
+        E e;
+        const Any& a_in = arr[i];
+        FromJson<E>(a_in, e);
+        value.insert(std::move(e));
+    }
+}
+
+template<typename E>
+void FromAny(const Any& a, std::map<std::string,E>& value)
+{
+    if (a.Empty())
+    {
+        return;
+    }
+
+    const JsonMap& m = AnyCast<JsonMap>(a);
+    JsonMap::const_iterator iter;
+
+    for (iter = m.begin(); iter != m.end(); ++iter)
+    {
+        E e;
+        const std::string& name = iter->first;
+        const Any& a_in = iter->second;
+        FromJson<E>(a_in, e);
+        value[name] = std::move(e);
+    }
+}
+
+template<typename T>
+void FromJson(const Any& a, T& t)
+{
+    FromAny(a, t);
+}
+
+void ToAny(const int8_t& value, Any& a);
+
+void ToAny(const uint8_t& value, Any& a);
+
+void ToAny(const int16_t& value, Any& a);
+
+void ToAny(const uint16_t& value, Any& a);
+
+void ToAny(const int32_t& value, Any& a);
+
+void ToAny(const uint32_t& value, Any& a);
+
+void ToAny(const int64_t& value, Any& a);
+
+void ToAny(const uint64_t& value, Any& a);
+
+void ToAny(const float& value, Any& a);
+
+void ToAny(const double& value, Any& a);
+
+void ToAny(const bool& value, Any& a);
+
+void ToAny(const std::string& value, Any& a);
+
+void ToAny(const JsonMap& value, Any& a);
+
+void ToAny(const JsonArray& value, Any& a);
+
+void ToAny(const Jsonize& value, Any& a);
+
+template<typename E>
+void ToAny(const std::vector<E>& value, Any& a)
+{
+    JsonArray arr;
+
+    size_t i, count = value.size();
+    for (i=0; i<count; i++)
+    {
+        Any a_in;
+        const E& e = value[i];
+
+        ToJson<E>(e, a_in);
+        arr.push_back(std::move(a_in));
+    }
+
+    a = Any(arr);
+}
+
+template<typename E>
+void ToAny(const std::list<E>& value, Any& a)
+{
+    JsonArray arr;
+
+    typename std::list<E>::const_iterator iter;
+    for (iter = value.begin(); iter != value.end(); ++iter)
+    {
+        Any a_in;
+        const E& e = *iter;
+
+        ToJson<E>(e, a_in);
+        arr.push_back(std::move(a_in));
+    }
+
+    a = Any(arr);
+}
+
+template<typename E>
+void ToAny(const std::set<E>& value, Any& a)
+{
+    JsonArray arr;
+
+    typename std::set<E>::const_iterator iter;
+    for (iter = value.begin(); iter != value.end(); ++iter)
+    {
+        Any a_in;
+        const E& e = *iter;
+
+        ToJson<E>(e, a_in);
+        arr.push_back(std::move(a_in));
+    }
+
+    a = Any(arr);
+}
+
+template<typename E>
+void ToAny(const std::map<std::string,E>& value, Any& a)
+{
+    JsonMap m;
+    typename std::map<std::string,E>::const_iterator iter;
+
+    for (iter = value.begin(); iter != value.end(); ++iter)
+    {
+        Any a_in;
+        const std::string& name = iter->first;
+        const E& e = iter->second;
+
+        ToJson<E>(e, a_in);
+        m[name] = a_in;
+    }
+
+    a = Any(m);
+}
+
+template<typename T>
+void ToJson(const T& value, Any& a)
+{
+    //std::cout << typeid(value).name() << std::endl;
+    ToAny(value, a);
+}
+}
+}
+#endif//__UT_JSONIZE_HPP__

unitree_SDK/include/unitree/common/lock/lock.hpp

@@ -0,0 +1,198 @@
+#ifndef __UT_LOCK_HPP__
+#define __UT_LOCK_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree
+{
+namespace common
+{
+enum
+{
+    UT_LOCK_MODE_READ   = 0,
+    UT_LOCK_MODE_WRITE  = 1,
+    UT_LOCK_MODE_UNLCK  = 2
+};
+
+enum
+{
+    UT_LOCK_ENO_INTR     = EINTR,
+    UT_LOCK_ENO_BUSY     = EBUSY,
+    UT_LOCK_ENO_TIMEDOUT = ETIMEDOUT
+};
+
+class Spinlock
+{
+public:
+    explicit Spinlock();
+    ~Spinlock();
+
+    void Lock();
+    void Unlock();
+
+    bool Trylock();
+
+    pthread_spinlock_t & GetNative();
+
+private:
+    pthread_spinlock_t mNative;
+};
+
+class CaspinLock
+{
+public:
+    explicit CaspinLock();
+    ~CaspinLock();
+
+    void Lock();
+    void Unlock();
+
+    bool Trylock();
+
+private:
+    volatile int32_t mData;
+};
+
+class Mutex
+{
+public:
+    explicit Mutex();
+    ~Mutex();
+
+    void Lock();
+    void Unlock();
+
+    bool Trylock();
+
+    pthread_mutex_t & GetNative();
+
+private:
+    pthread_mutex_t mNative;
+};
+
+class Cond
+{
+public:
+    explicit Cond();
+    ~Cond();
+
+    void Wait(Mutex& mutex);
+    bool Wait(Mutex& mutex, uint64_t microsec);
+
+    void Notify();
+    void NotifyAll();
+
+private:
+    pthread_cond_t mNative;
+};
+
+class MutexCond
+{
+public:
+    explicit MutexCond();
+    ~MutexCond();
+
+    void Lock();
+    void Unlock();
+
+    bool Wait(int64_t microsec = 0);
+    void Notify();
+    void NotifyAll();
+
+private:
+    Mutex mMutex;
+    Cond mCond;
+};
+
+class Rwlock
+{
+public:
+    explicit Rwlock();
+    ~Rwlock();
+
+    void Lock(int32_t mode);
+    void Unlock();
+
+    bool Trylock(int32_t mode);
+
+    pthread_rwlock_t & GetNative();
+
+private:
+    pthread_rwlock_t mNative;
+};
+
+class Filelock
+{
+public:
+    explicit Filelock(const std::string& fileName);
+    explicit Filelock(int32_t fd);
+    ~Filelock();
+
+    void Lock(int32_t mode, int64_t start = 0, int64_t len = 0);
+    void Unlock();
+
+    bool Trylock(int32_t mode, int64_t start = 0, int64_t len = 0);
+
+private:
+    void SetLockMember(int32_t mode, int64_t start, int64_t len);
+    void SetLockMode(int32_t mode);
+
+private:
+    int32_t mFd;
+    bool mCloseFd;
+    struct flock mLock;
+};
+
+template<typename LOCK_TYPE>
+class LockGuard
+{
+public:
+    explicit LockGuard(LOCK_TYPE& lockPtr)
+    {
+        mLockPtr = &lockPtr;
+        mLockPtr->Lock();
+    }
+
+    explicit LockGuard(LOCK_TYPE* lockPtr)
+    {
+        mLockPtr = lockPtr;
+        mLockPtr->Lock();
+    }
+
+    ~LockGuard()
+    {
+        mLockPtr->Unlock();
+    }
+
+private:
+    LOCK_TYPE* mLockPtr;
+};
+
+template<typename LOCK_TYPE>
+class RwLockGuard
+{
+public:
+    explicit RwLockGuard(LOCK_TYPE& lockPtr, int32_t mode)
+    {
+        mLockPtr = &lockPtr;
+        lockPtr.Lock(mode);
+    }
+
+    explicit RwLockGuard(LOCK_TYPE* lockPtr, int32_t mode)
+    {
+        mLockPtr = lockPtr;
+        lockPtr->Lock(mode);
+    }
+
+    ~RwLockGuard()
+    {
+        mLockPtr->Unlock();
+    }
+
+private:
+    LOCK_TYPE* mLockPtr;
+};
+}
+}
+
+#endif//__UT_LOCK_HPP__

unitree_SDK/include/unitree/common/log/log.hpp

@@ -0,0 +1,6 @@
+#ifndef __UT_LOG_HPP__
+#define __UT_LOG_HPP__
+
+#include <unitree/common/log/log_initor.hpp>
+
+#endif//__UT_LOG_HPP__

unitree_SDK/include/unitree/common/log/log_buffer.hpp

@@ -0,0 +1,44 @@
+#ifndef __UT_LOG_BUFFER_HPP__
+#define __UT_LOG_BUFFER_HPP__
+
+#include <unitree/common/log/log_decl.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class LogBuffer
+{
+public:
+    explicit LogBuffer();
+    virtual ~LogBuffer();
+
+    virtual bool Append(const std::string& s);
+    virtual bool Get(std::string& s);
+    virtual bool Empty();
+
+protected:
+    std::string mData;
+};
+
+typedef std::shared_ptr<LogBuffer> LogBufferPtr;
+
+class LogBlockBuffer : public LogBuffer
+{
+public:
+    explicit LogBlockBuffer();
+    ~LogBlockBuffer();
+
+    bool Append(const std::string& s);
+    bool Get(std::string& s);
+    bool Empty();
+
+private:
+    Mutex mLock;
+};
+
+typedef std::shared_ptr<LogBlockBuffer> LogBlockBufferPtr;
+}
+}
+
+#endif//__UT_LOG_BUFFER_HPP__

unitree_SDK/include/unitree/common/log/log_decl.hpp

@@ -0,0 +1,261 @@
+#ifndef __UT_LOG_DECL_HPP__
+#define __UT_LOG_DECL_HPP__
+
+#include <unitree/common/exception.hpp>
+#include <unitree/common/os.hpp>
+#include <unitree/common/lock/lock.hpp>
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/common/thread/recurrent_thread.hpp>
+#include <unitree/common/filesystem/file.hpp>
+
+/*
+ * log buffer size
+ */
+#define UT_LOG_BUFFER_SIZE          65535           //64K-1
+#define UT_LOG_CRT_BUFFER_SIZE      64512           //63K
+#define UT_LOG_MAX_BUFFER_SIZE      4194303         //4M-1
+/*
+ * log file size
+ */
+#define UT_LOG_FILE_SIZE            104857600       //100M
+#define UT_LOG_MAX_FILE_SIZE        10737418240     //10G
+#define UT_LOG_MIN_FILE_SIZE        UT_LOG_MAX_BUFFER_SIZE
+
+/*
+ * log write interval(micro second)
+ */
+#define UT_LOG_WRITE_INTER          100000          //100ms
+#define UT_LOG_MAX_WRITE_INTER      1000000         //1s
+#define UT_LOG_MIN_WRITE_INTER      2000            //2ms
+
+/*
+ * log file number
+ */
+#define UT_LOG_FILE_NUMBER          2
+#define UT_LOG_MAX_FILE_NUMBER      100
+#define UT_LOG_MIN_FILE_NUMBER      UT_LOG_FILE_NUMBER
+
+#define UT_LOG_FILE_EXT             ".LOG"
+
+//write log macro wrapper
+#define __UT_LOG(logger, level, ...)\
+    do {                            \
+        if (logger != NULL)         \
+        {                           \
+            logger->Log(level, __VA_ARGS__);    \
+        }                           \
+    } while (0)
+
+#define __UT_CRIT_LOG(logger, key, code, ...)   \
+    do {                            \
+        if (logger != NULL)         \
+        {                           \
+            logger->CritLog(UT_LOG_CRIT, key, code, __VA_ARGS__);\
+        }                           \
+    } while (0)
+
+
+//debug
+#define LOG_DEBUG(logger, ...)      \
+    __UT_LOG(logger, UT_LOG_DEBUG, __VA_ARGS__)
+
+//info
+#define LOG_INFO(logger, ...)       \
+    __UT_LOG(logger, UT_LOG_INFO, __VA_ARGS__)
+
+//warning
+#define LOG_WARNING(logger, ...)    \
+    __UT_LOG(logger, UT_LOG_WARNING, __VA_ARGS__)
+
+//error
+#define LOG_ERROR(logger, ...)      \
+    __UT_LOG(logger, UT_LOG_ERROR, __VA_ARGS__)
+
+//fatal
+#define LOG_FATAL(logger, ...)      \
+    __UT_LOG(logger, UT_LOG_FATAL, __VA_ARGS__)
+
+//critical log. the 1st args is CRITICAL KEY 
+#define CRIT_LOG(logger, ...)       \
+    __UT_CRIT_LOG(logger, __VA_ARGS__)
+
+#define CRIT_LOG(logger, ...)       \
+    __UT_CRIT_LOG(logger, __VA_ARGS__)
+
+//write log format macro wrapper
+/*
+ * FMT_DEBUG(logger, ("key1", val1)("key2", val2)("keyn", ""));
+ */
+#define __UT_LOG_FMT(logger, level, keyvalues)  \
+    do {                                        \
+        if (logger != NULL)                     \
+        {                                       \
+            logger->LogFormat(level, unitree::common::LogBuilder() keyvalues);    \
+        }                                       \
+    } while (0)
+
+#define __UT_CRIT_LOG_FMT(logger, key, code, keyvalues)    \
+    do {                                        \
+        if (logger != NULL)                     \
+        {                                       \
+            logger->CritLogFormat(UT_LOG_CRIT, key, code, unitree::common::LogBuilder() keyvalues);   \
+        }                                       \
+    } while (0)
+
+//debug
+#define FMT_DEBUG(logger, keyvalues)            \
+    __UT_LOG_FMT(logger, UT_LOG_DEBUG, keyvalues)
+
+//info
+#define FMT_INFO(logger, keyvalues)             \
+    __UT_LOG_FMT(logger, UT_LOG_INFO, keyvalues)
+
+//warning
+#define FMT_WARNING(logger, keyvalues)          \
+    __UT_LOG_FMT(logger, UT_LOG_WARNING, keyvalues)
+
+//error
+#define FMT_ERROR(logger, keyvalues)            \
+    __UT_LOG_FMT(logger, UT_LOG_ERROR, keyvalues)
+
+//fatal
+#define FMT_FATAL(logger, keyvalues)            \
+    __UT_LOG_FMT(logger, UT_LOG_FATAL, keyvalues)
+
+#define CRIT_FMT(logger, critkey, keyvalues)    \
+    __UT_CRIT_LOG_FMT(logger, critkey, keyvalues)
+
+
+/*
+//declare and define log level
+#define UT_LOG_DECL_LEVEL(name, level, desc)    \
+    const int32_t name = level;                 \
+    const std::string name##_DESC = desc;
+#define UT_LOG_DESC_LEVEL(name) name##_DESC
+*/
+
+//declare and define log store type 
+#define UT_LOG_DECL_STORE_TYPE(name, type, desc)\
+    const int32_t name = type;                  \
+    const std::string name##_DESC = desc;
+#define UT_LOG_DESC_STORE_TYPE(name) name##_DESC
+
+//define log level
+#define UT_LOG_NONE                     0
+#define UT_LOG_CRIT                     1
+#define UT_LOG_FATAL                    2
+#define UT_LOG_ERROR                    3
+#define UT_LOG_WARNING                  4
+#define UT_LOG_INFO                     5
+#define UT_LOG_DEBUG                    6
+#define UT_LOG_ALL                      7
+
+#define UT_LOG_DESC_NONE                "NONE"
+#define UT_LOG_DESC_CRIT                "CRIT"
+#define UT_LOG_DESC_FATAL               "FATAL"
+#define UT_LOG_DESC_ERROR               "ERROR"
+#define UT_LOG_DESC_WARNING             "WARNING"
+#define UT_LOG_DESC_INFO                "INFO"
+#define UT_LOG_DESC_DEBUG               "DEBUG"
+#define UT_LOG_DESC_ALL                 "ALL"
+
+//define log store type
+#define UT_LOG_STORE_FILE_ASYNC         0
+#define UT_LOG_STORE_FILE               1
+#define UT_LOG_STORE_STDOUT             2
+#define UT_LOG_STORE_STDERR             3
+
+#define UT_LOG_STORE_DESC_FILE_ASYNC    "FILEASYNC"
+#define UT_LOG_STORE_DESC_ASYNC_FILE    "ASYNCFILE"
+#define UT_LOG_STORE_DESC_ASYNC         "ASYNC"
+#define UT_LOG_STORE_DESC_FILE          "FILE"
+#define UT_LOG_STORE_DESC_STDOUT        "STDOUT"
+#define UT_LOG_STORE_DESC_STDERR        "STDERR"
+
+namespace unitree
+{
+namespace common
+{
+static inline int32_t GetLogLevel(const std::string& desc)
+{
+    if (desc == UT_LOG_DESC_NONE)           {
+        return UT_LOG_NONE;     }
+    else if (desc == UT_LOG_DESC_CRIT)      {
+        return UT_LOG_CRIT;     }
+    else if (desc == UT_LOG_DESC_FATAL)     {
+        return UT_LOG_FATAL;    }
+    else if (desc == UT_LOG_DESC_ERROR)     {
+        return UT_LOG_ERROR;    }
+    else if (desc == UT_LOG_DESC_WARNING)   {
+        return UT_LOG_WARNING;  }
+    else if (desc == UT_LOG_DESC_INFO)      {
+        return UT_LOG_INFO;     }
+    else if (desc == UT_LOG_DESC_DEBUG)     {
+        return UT_LOG_DEBUG;    }
+    else if (desc == UT_LOG_DESC_ALL)       {
+        return UT_LOG_ALL;      }
+
+    UT_THROW(CommonException, std::string("unknown log level desc:") + desc);
+}
+
+static inline const char* GetLogLevelDesc(int32_t level)
+{
+    switch (level)
+    {
+    case UT_LOG_NONE:
+        return UT_LOG_DESC_NONE;
+    case UT_LOG_CRIT:
+        return UT_LOG_DESC_CRIT;
+    case UT_LOG_FATAL:
+        return UT_LOG_DESC_FATAL;
+    case UT_LOG_ERROR:
+        return UT_LOG_DESC_ERROR;
+    case UT_LOG_WARNING:
+        return UT_LOG_DESC_WARNING;
+    case UT_LOG_INFO:
+        return UT_LOG_DESC_INFO;
+    case UT_LOG_DEBUG:
+        return UT_LOG_DESC_DEBUG;
+    case UT_LOG_ALL:
+        return UT_LOG_DESC_ALL;
+    }
+
+    UT_THROW(CommonException, "unknown log level");
+}
+
+static inline int32_t GetLogStoreType(const std::string& desc)
+{
+    if (desc == UT_LOG_STORE_DESC_FILE_ASYNC ||
+        desc == UT_LOG_STORE_DESC_ASYNC_FILE ||
+        desc == UT_LOG_STORE_DESC_ASYNC)      {
+        return UT_LOG_STORE_FILE_ASYNC;  }
+    else if (desc == UT_LOG_STORE_DESC_FILE)  {
+        return UT_LOG_STORE_FILE;        }
+    else if (desc == UT_LOG_STORE_DESC_STDOUT){
+        return UT_LOG_STORE_STDOUT;      }
+    else if (desc == UT_LOG_STORE_DESC_STDERR){
+        return UT_LOG_STORE_STDERR;      }
+
+    UT_THROW(CommonException, std::string("unknown log store type desc:") + desc);
+}
+
+static inline const char* GetLogStoreTypeDesc(int32_t type)
+{
+    switch (type)
+    {
+    case UT_LOG_STORE_FILE_ASYNC:
+        return UT_LOG_STORE_DESC_FILE_ASYNC;
+    case UT_LOG_STORE_FILE:
+        return UT_LOG_STORE_DESC_FILE;
+    case UT_LOG_STORE_STDOUT:
+        return UT_LOG_STORE_DESC_STDOUT;
+    case UT_LOG_STORE_STDERR:
+        return UT_LOG_STORE_DESC_STDERR;
+    }
+
+    UT_THROW(CommonException, "unknown log store type");
+}
+}
+}
+
+#endif//__UT_LOG_DECL_HPP__

unitree_SDK/include/unitree/common/log/log_initor.hpp

@@ -0,0 +1,44 @@
+#ifndef __UT_LOG_INITOR_HPP__
+#define __UT_LOG_INITOR_HPP__
+
+#include <unitree/common/log/log_policy.hpp>
+#include <unitree/common/log/log_store.hpp>
+#include <unitree/common/log/log_logger.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class LogInitor
+{
+public:
+    explicit LogInitor();
+    ~LogInitor();
+
+    void Init(const std::string& configFileName, bool stdoutDefault = false);
+    Logger* GetLogger(const std::string& tag);
+
+    void Final();
+
+    void ParseConf(Any json);
+    void SetStdoutPolicy();
+    void InitLogger();
+
+private:
+    std::set<std::string> mStoreNames;
+    std::vector<LogPolicyPtr> mPolicis;
+    std::vector<LogStorePolicyPtr> mStorePolicis;
+    std::map<std::string, LoggerPtr> mLoggerMap;
+};
+
+using LogInitorPtr = std::shared_ptr<LogInitor>;
+
+void LogInit(const std::string& configFileName = "", bool stdoutDefault = false);
+void LogFinal();
+
+Logger* GetLogger(const std::string& tag);
+
+}
+}
+
+#endif//__UT_LOG_INITOR_HPP__

unitree_SDK/include/unitree/common/log/log_keeper.hpp

@@ -0,0 +1,44 @@
+#ifndef __UT_LOG_FILE_KEEPER_H__
+#define __UT_LOG_FILE_KEEPER_H__
+
+#include <unitree/common/log/log_policy.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class LogKeeper
+{
+public:
+    LogKeeper(LogStorePolicyPtr storePolicyPtr);
+    ~LogKeeper();
+
+    LogStorePolicyPtr GetStorePolicy() const;
+
+    bool Append(const std::string& s, bool rotate);
+
+private:
+    void Rotate();
+
+    void AppendFile(const std::string& s);
+    void OpenFile();
+    void CloseFile();
+
+    void CheckFileSize();
+
+    std::string MakeRegexExpress();
+
+private:
+    volatile int64_t mFileSize;
+    std::string mFileName;
+    std::string mDirectory;
+    FilePtr mFilePtr;
+    LogStorePolicyPtr mStorePolicyPtr;
+};
+
+typedef std::shared_ptr<LogKeeper> LogKeeperPtr;
+
+}
+}
+
+#endif//__UT_LOG_FILE_KEEPER_H__

unitree_SDK/include/unitree/common/log/log_logger.hpp

@@ -0,0 +1,139 @@
+#ifndef __UT_LOGGER_HPP__
+#define __UT_LOGGER_HPP__
+
+#include <unitree/common/log/log_store.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class LogBuilder
+{
+public:
+    LogBuilder()
+    {}
+
+    template<typename T>
+    LogBuilder& operator()(const std::string& key, const T& value)
+    {
+        mOs << "\t" << key << ":" << value;
+        return *this;
+    }
+
+    std::ostringstream mOs;
+};
+
+class Logger
+{
+public:
+    explicit Logger(int32_t level, LogStorePtr storePtr) :
+        mLevel(level), mStorePtr(storePtr)
+    {}
+
+    template<typename ...Args>
+    void Log(int32_t level, Args&&... args)
+    {
+        if (level > mLevel || mStorePtr == NULL)
+        {
+            return;
+        }
+
+        std::ostringstream os;
+        LogBegin(os, level);
+        LogPend(os, std::forward<Args>(args)...);
+        LogEnd(os);
+
+        mStorePtr->Append(os.str());
+    }
+
+    void LogFormat(int32_t level, LogBuilder& builder)
+    {
+        if (level > mLevel || mStorePtr == NULL)
+        {
+            return;
+        }
+
+        std::ostringstream os;
+        LogBegin(os, level);
+        LogPendBuilder(os, builder);
+        LogEnd(os);
+
+        mStorePtr->Append(os.str());
+    }
+
+    template<typename ...Args>
+    void CritLog(int32_t level, const std::string& key, int32_t code, Args&&... args)
+    {
+        if (level > mLevel || mStorePtr == NULL)
+        {
+            return;
+        }
+
+        std::ostringstream os;
+        LogBegin(os, level);
+        LogPendCrit(os, key, code);
+        LogPend(os, std::forward<Args>(args)...);
+        LogEnd(os);
+
+        mStorePtr->Append(os.str());
+    }
+
+    void CritLogFormat(int32_t level, const std::string& key, int32_t code, LogBuilder& builder)
+    {
+        if (level > mLevel || mStorePtr == NULL)
+        {
+            return;
+        }
+
+        std::ostringstream os;
+        LogBegin(os, level);
+        LogPendCrit(os, key, code);
+        LogPendBuilder(os, builder);
+        LogEnd(os);
+
+        mStorePtr->Append(os.str());
+    }
+
+private:
+    void LogPendCrit(std::ostringstream& os, const std::string& key, int32_t code)
+    {
+        os << " [__KEY__:" << key << "]";
+        os << " [__CODE__:" << code << "]";
+    }
+
+    template<typename ...Args>
+    void LogPend(std::ostringstream& os, Args&&... args)
+    {
+        os << " ";
+        std::initializer_list<int32_t>{ (os << args, 0)... };
+    }
+
+    void LogPendBuilder(std::ostringstream& os, LogBuilder& builder)
+    {
+        os << builder.mOs.str();
+    }
+
+    void LogBegin(std::ostringstream& os, int32_t level)
+    {
+        os << "[" << GetTimeMillisecondString() << "] ";
+        os << "[" << GetLogLevelDesc(level) << "] ";
+        os << "[" << OsHelper::Instance()->GetProcessId() << "] ";
+        os << "[" << OsHelper::Instance()->GetTid() << "]";
+        os << std::setprecision(6) << std::fixed;
+    }
+
+    void LogEnd(std::ostringstream& os)
+    {
+        os << std::endl;
+    }
+
+private:
+    int32_t mLevel;
+    LogStorePtr mStorePtr;
+};
+
+typedef std::shared_ptr<Logger> LoggerPtr;
+}
+}
+
+#endif//__UT_LOGGER_HPP__

unitree_SDK/include/unitree/common/log/log_policy.hpp

@@ -0,0 +1,52 @@
+#ifndef __UT_LOG_POLICY_HPP__
+#define __UT_LOG_POLICY_HPP__
+
+#include <unitree/common/log/log_decl.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class LogPolicy
+{
+public:
+    LogPolicy();
+    LogPolicy(const std::string& tag, int32_t level, const std::string& store);
+
+    void Dump();
+
+public:
+    std::string mTag;
+    int32_t mLevel;
+    std::string mStore;
+};
+
+typedef std::shared_ptr<LogPolicy> LogPolicyPtr;
+
+class LogStorePolicy
+{
+public:
+    LogStorePolicy();
+    LogStorePolicy(const std::string& name, int32_t type, int32_t fileNumber, int64_t fileSize,
+        int64_t fileWriteInter = UT_LOG_WRITE_INTER, int32_t cpuId = UT_CPU_ID_NONE,
+        const std::string& fileName = "", const std::string& directory = "");
+
+    void Dump();
+
+public:
+    std::string mName;
+    int32_t mType;
+    int32_t mFileNumber;
+    int64_t mFileSize;
+    int64_t mFileWriteInter;
+    int32_t mCpuId;
+    std::string mFileName;
+    std::string mDirectory;
+};
+
+typedef std::shared_ptr<LogStorePolicy> LogStorePolicyPtr;
+
+}
+}
+
+#endif//__UT_LOG_POLICY_HPP__

unitree_SDK/include/unitree/common/log/log_store.hpp

@@ -0,0 +1,76 @@
+#ifndef __LOG_STORE_HPP__
+#define __LOG_STORE_HPP__
+
+#include <unitree/common/log/log_writer.hpp>
+#include <unitree/common/log/log_keeper.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class LogStore
+{
+public:
+    explicit LogStore()
+    {}
+    virtual ~LogStore()
+    {
+        mWriterPtr.reset();
+    }
+
+    virtual void Append(const std::string& s) = 0;
+
+protected:
+    LogWriterPtr mWriterPtr;
+};
+
+typedef std::shared_ptr<LogStore> LogStorePtr;
+
+class LogStdoutStore : public LogStore
+{
+public:
+    explicit LogStdoutStore();
+    ~LogStdoutStore();
+
+    void Append(const std::string& s);
+};
+
+typedef std::shared_ptr<LogStdoutStore> LogStdoutStorePtr;
+
+class LogStderrStore : public LogStore
+{
+public:
+    explicit LogStderrStore();
+    ~LogStderrStore();
+
+    void Append(const std::string& s);
+};
+
+typedef std::shared_ptr<LogStderrStore> LogStderrStorePtr;
+
+class LogFileStore : public LogStore
+{
+public:
+    explicit LogFileStore(LogKeeperPtr keeperPtr);
+    ~LogFileStore();
+
+    void Append(const std::string& s);
+};
+
+typedef std::shared_ptr<LogFileStore> LogFileStorePtr;
+
+class LogFileAsyncStore : public LogStore
+{
+public:
+    explicit LogFileAsyncStore(LogKeeperPtr keeperPtr);
+    ~LogFileAsyncStore();
+
+    void Append(const std::string& s);
+};
+
+typedef std::shared_ptr<LogFileAsyncStore> LogFileAsyncStorePtr;
+
+}
+}
+
+#endif//__LOG_STORE_HPP__

unitree_SDK/include/unitree/common/log/log_writer.hpp

@@ -0,0 +1,88 @@
+#ifndef __LOG_WRITER_HPP__
+#define __LOG_WRITER_HPP__
+
+#include <unitree/common/log/log_buffer.hpp>
+#include <unitree/common/log/log_keeper.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class LogWriter
+{
+public:
+    virtual void Write(const std::string& s) = 0;
+};
+
+typedef std::shared_ptr<LogWriter> LogWriterPtr;
+
+class LogDirectWriter : public LogWriter
+{
+public:
+    explicit LogDirectWriter(int32_t fd);
+    virtual ~LogDirectWriter();
+
+    void Write(const std::string& s);
+
+private:
+    Mutex mLock;
+    int32_t mFd;
+};
+
+class LogStdoutWriter : public LogDirectWriter
+{
+public:
+    explicit LogStdoutWriter() :
+        LogDirectWriter(UT_FD_STDOUT)
+    {}
+    ~LogStdoutWriter()
+    {}
+};
+
+class LogStderrWriter : public LogDirectWriter
+{
+public:
+    explicit LogStderrWriter() :
+        LogDirectWriter(UT_FD_STDERR)
+    {}
+    ~LogStderrWriter()
+    {}
+};
+
+class LogBufferWriter : public LogWriter
+{
+public:
+    explicit LogBufferWriter(LogKeeperPtr keeperPtr);
+    ~LogBufferWriter();
+
+    void Write(const std::string& s);
+
+private:
+    LogBufferPtr mBufferPtr;
+    LogKeeperPtr mKeeperPtr;
+    Mutex mLock;
+};
+
+class LogAsyncBufferWriter : public LogWriter
+{
+public:
+    explicit LogAsyncBufferWriter(LogKeeperPtr keeperPtr);
+    ~LogAsyncBufferWriter();
+
+    void Write(const std::string& s);
+
+private:
+    void DoWrite();
+
+private:
+    volatile bool mRotate;
+    std::string mTempBuf;
+    LogBufferPtr mBufferPtr;
+    LogKeeperPtr mKeeperPtr;
+    ThreadPtr mThreadPtr;
+    Mutex mLock;
+};
+}
+}
+
+#endif//__LOG_WRITER_HPP__

unitree_SDK/include/unitree/common/os.hpp

@@ -0,0 +1,93 @@
+#ifndef __UT_OS_HPP__
+#define __UT_OS_HPP__
+
+#include <unitree/common/decl.hpp>
+#include <unitree/common/lock/lock.hpp>
+
+namespace unitree
+{
+namespace common
+{
+enum UT_SCHED_POLICY
+{
+    UT_SCHED_POLICY_NORMAL = SCHED_OTHER,
+    UT_SCHED_POLICY_FIFO = SCHED_FIFO,
+    UT_SCHED_POLICY_RR = SCHED_RR,
+    UT_SCHED_POLICY_BATCH = SCHED_BATCH,
+    UT_SCHED_POLICY_IDLE = SCHED_IDLE,
+    UT_SCHED_POLICY_DEADLINE = SCHED_DEADLINE
+};
+
+class OsHelper
+{
+public:
+    static OsHelper* Instance()
+    {
+        static OsHelper inst;
+        return &inst;
+    }
+
+    uid_t GetUID() const;
+    gid_t GetGID() const;
+    std::string GetUser() const;
+    const struct passwd& GetPasswd() const;
+
+    bool GetPasswd(const std::string& name, struct passwd& pwd);
+    bool GetUIDAndGID(const std::string& name, uid_t& uid, gid_t& gid);
+
+    int32_t GetProcessorNumber() const;
+    int32_t GetProcessorNumberConf() const;
+
+    int32_t GetPageSize() const;
+
+    int64_t Align(int64_t len) const;
+    bool IsAligned(int64_t len) const;
+
+    const std::string& GetHostname() const;
+
+    uint32_t GetProcessId();
+    const std::string& GetProcessFileName();
+
+    std::string GetProcessName();
+    std::string GetProcessDirectory(bool withEndDelim = true);
+
+    uint64_t GetThreadId();
+    int32_t GetTid();
+
+    bool GetNetworkInterfaceIps(std::map<std::string,std::string>& networkInterfaceIpMap);
+    bool GetIps(std::set<std::string>& ips);
+
+    void CpuSet(const std::string& cpuIds);
+    void CpuSet(uint64_t threadId, size_t cpuId);
+
+    void SetThreadName(uint64_t threadId, const std::string& name);
+
+    void SetScheduler(int32_t policy, int32_t priority);
+    void SetScheduler(uint64_t threadId, int32_t policy, int32_t priority);
+
+    void SingletonProcessInstance();
+
+private:
+    OsHelper();
+
+    void __Getpwuid();
+    void __GetProcessor();
+    void __GetPageSize();
+    void __GetProcessFileName();
+    void __GetHostname();
+
+private:
+    uid_t mUID;
+    struct passwd mPasswd;
+    int32_t mProcessorNumber;
+    int32_t mProcessorNumberConf;
+    int32_t mPageSize;
+    std::string mHostname;
+    std::string mProcessFileName;
+    std::shared_ptr<Filelock> mInstanceLockPtr;
+};
+
+}
+}
+
+#endif//__UT_OS_HPP__

unitree_SDK/include/unitree/common/service/base/service_application.hpp

@@ -0,0 +1,51 @@
+#ifndef __UT_SERVICE_APPLICATION_HPP__
+#define __UT_SERVICE_APPLICATION_HPP__
+
+#include <unitree/common/service/base/service_base.hpp>
+
+#define SERVICE_REGISTER(SERVICE)       \
+class SERVICE##Register                 \
+{                                       \
+public:                                 \
+    SERVICE##Register()                 \
+    {                                   \
+        unitree::common::ServiceApplication::Instance()->RegistService<SERVICE>(); \
+    }                                   \
+    static SERVICE##Register mRegister; \
+};                                      \
+SERVICE##Register SERVICE##Register::mRegister = SERVICE##Register();
+
+namespace unitree
+{
+namespace common
+{
+class ServiceApplication
+{
+public:
+    static ServiceApplication* Instance()
+    {
+        static ServiceApplication inst;
+        return &inst;
+    }
+
+    template<typename SERVICE>
+    void RegistService()
+    {
+        mServicePtr = ServicePtr(new SERVICE());
+    }
+
+    void Init(const std::string& configFileName);
+    void Start();
+    void Stop();
+
+private:
+    ServiceApplication();
+
+protected:
+    ServicePtr mServicePtr;
+};
+
+}
+}
+
+#endif//__UT_SERVICE_APPLICATION_HPP__

unitree_SDK/include/unitree/common/service/base/service_base.hpp

@@ -0,0 +1,104 @@
+#ifndef __UT_SERVICE_BASE_HPP__
+#define __UT_SERVICE_BASE_HPP__
+
+#include <unitree/common/service/base/service_config.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class ServiceBase
+{
+public:
+    ServiceBase();
+    virtual ~ServiceBase();
+
+    virtual void Parse(const std::string& configFileName);
+
+    virtual void Init();
+
+    virtual void Register();
+
+    virtual void Start();
+
+    virtual void Wait();
+
+    virtual void Stop();
+
+protected:
+    /*
+     * Parse config content
+     */
+    virtual void ParseConfigContent(const std::string& content);
+
+    /*
+     * Get field: ServiceName
+     */
+    const std::string& GetServiceName() const;
+
+    /*
+     * Get field: CpuIds
+     */
+    const std::string& GetCpuIds() const;
+
+    /*
+     * Get top-level field/value
+     */
+    const Any& GetGlobalParameter(const std::string& name) const;
+
+    /*
+     * Get top-level field: Parameter
+     */
+    bool HasParameter(const std::string& name) const;
+
+    /*
+     * Get top-level field: Parameter
+     */
+    const JsonMap& GetParameter() const;
+
+    /*
+     * Get field from top-level field: Parameter
+     */
+    const Any& GetParameter(const std::string& name) const;
+
+    /*
+     * Get field from top-level field: Parameter
+     */
+    template<typename T>
+    const T& GetParameter(const std::string& name) const
+    {
+        return mConfig.GetParameter<T>(name);
+    }
+
+    /*
+     * Get field from top-level field: Parameter
+     */
+    template<typename T>
+    T GetNumberParameter(const std::string& name) const
+    {
+        return mConfig.GetNumberParameter<T>(name);
+    }
+
+    /*
+     * Get field from top-level field: Parameter
+     */
+    template<typename T>
+    T GetParameter(const std::string& name, const T& defValue) const
+    {
+        return mConfig.GetParameter<T>(name, defValue);
+    }
+
+    template<typename T>
+    T GetNumberParameter(const std::string& name, const T& defValue) const
+    {
+        return mConfig.GetNumberParameter<T>(name, defValue);
+    }
+
+private:
+    ServiceConfig mConfig;
+};
+typedef std::shared_ptr<ServiceBase> ServicePtr;
+
+}
+}
+#endif//__UT_SERVICE_BASE_HPP__

unitree_SDK/include/unitree/common/service/base/service_config.hpp

@@ -0,0 +1,37 @@
+#ifndef __UT_SERVICE_CONFIG_HPP__
+#define __UT_SERVICE_CONFIG_HPP__
+
+#include <unitree/common/service/base/service_decl.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class ServiceConfig : public common::JsonConfig
+{
+public:
+    ServiceConfig();
+    virtual ~ServiceConfig();
+
+    ServiceConfig(const std::string& configFileName);
+
+    virtual void Parse(const std::string& configFileName);
+    virtual void ParseContent(const std::string& content);
+
+    //top-level field: ServiceName
+    const std::string& GetServiceName() const;
+
+    //top-level field: CpuIds
+    const std::string& GetCpuIds() const;
+
+protected:
+    std::string mCpuIds;
+    std::string mServiceName;
+};
+
+typedef std::shared_ptr<ServiceConfig> ServiceConfigPtr;
+
+}
+}
+
+#endif//__UT_SERVICE_CONFIG_HPP__

unitree_SDK/include/unitree/common/service/base/service_decl.hpp

@@ -0,0 +1,10 @@
+#ifndef __UT_SERVICE_DECL_HPP__
+#define __UT_SERVICE_DECL_HPP__
+
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <unitree/common/json/json.hpp>
+#include <unitree/common/json/json_config.hpp>
+#include <unitree/common/log/log.hpp>
+
+#endif//__UT_SERVICE_DECL_HPP__

unitree_SDK/include/unitree/common/service/dds_service.hpp

@@ -0,0 +1,85 @@
+#ifndef __UT_DDS_SERVICE_HPP__
+#define __UT_DDS_SERVICE_HPP__
+
+#include <unitree/common/service/base/service_base.hpp>
+#include <unitree/common/service/base/service_config.hpp>
+#include <unitree/common/dds/dds_easy_model.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class DdsService : public ServiceBase
+{
+public:
+    DdsService()
+    {
+        mLogger = GetLogger("/unitree/service/dds_service");
+        mQuit = false;
+    }
+
+    virtual ~DdsService()
+    {}
+
+    virtual void Register()
+    {}
+
+    virtual void Init()
+    {
+        mModel.Init(AnyCast<JsonMap>(GetGlobalParameter("DdsParameter")));
+        LOG_INFO(mLogger, "parse init dds model success.");
+    }
+
+    virtual void Start()
+    {}
+
+    virtual void Wait()
+    {
+        while (!mQuit) { sleep(1); }
+    }
+
+    virtual void Stop()
+    {
+        mQuit = true;
+    }
+
+public:
+    virtual void Parse(const std::string& configFileName)
+    {
+        ServiceBase::Parse(configFileName);
+        LOG_INFO(mLogger, "parse config success. filename:", configFileName);
+    }
+
+protected:
+    template<typename MSG>
+    void RegistTopicMessageHandler(const std::string& topic, const DdsMessageHandler& handler)
+    {
+        mModel.SetTopic<MSG>(topic, handler);
+        LOG_INFO(mLogger, "regist topic reader callback. topic:", topic);
+    }
+
+    template<typename MSG>
+    void RegistTopic(const std::string& topic)
+    {
+        mModel.SetTopic<MSG>(topic);
+        LOG_INFO(mLogger, "regist topic. topic:", topic);
+    }
+
+    /*
+     * Write message to topic
+     */
+    template<typename MSG>
+    void WriteMessage(const std::string& topic, const MSG& message)
+    {
+        mModel.WriteMessage<MSG>(topic, message);
+    }
+
+private:
+    bool mQuit;
+    DdsEasyModel mModel;
+    Logger* mLogger;
+};
+
+}
+}
+#endif//__UT_DDS_SERVICE_HPP__

unitree_SDK/include/unitree/common/string_tool.hpp

@@ -0,0 +1,77 @@
+#ifndef __UT_STRING_TOOL_HPP__
+#define __UT_STRING_TOOL_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree
+{
+namespace common
+{
+/*
+ * trim delim
+ */
+const char UT_TRIM_DELIM_STR[] = {0x09,0x0A,0x0B,0x0C,0x0D,0x20,0x00};
+
+/*
+ * string cast functions
+ */
+std::string ToString(const char* s);
+std::string ToString(const std::string& s);
+
+/*
+ * template ToString
+ */
+template <typename T>
+std::string ToString(const T& t)
+{
+    return std::to_string(t);
+}
+
+/*
+ * string functions
+ */
+void StringTo(const std::string& s, int32_t& val);
+void StringTo(const std::string& s, uint32_t& val);
+void StringTo(const std::string& s, int64_t& val);
+void StringTo(const std::string& s, uint64_t& val);
+void StringTo(const std::string& s, float& val);
+void StringTo(const std::string& s, double& val);
+void StringTo(const std::string& s, long double& val);
+
+template<typename T>
+T StringTo(const std::string& s)
+{
+    T t;
+    StringTo(s, t);
+    return t;
+}
+
+/*
+ * string tool functions
+ */
+std::string& ToUpper(std::string& s);
+std::string& ToLower(std::string& s);
+
+int32_t Compare(const std::string& s1, const std::string& s2,
+    bool caseSensitive = true);
+int32_t Compare(const std::string& s1, size_t pos, size_t len,
+    const std::string& s2, bool caseSensitive = true);
+
+std::string& TrimLeft(std::string& s, const std::string& delim = std::string(UT_TRIM_DELIM_STR));
+std::string& TrimRight(std::string& s, const std::string& delim = std::string(UT_TRIM_DELIM_STR));
+std::string& Trim(std::string& s, const std::string& delim = std::string(UT_TRIM_DELIM_STR));
+
+void Split(const std::string& s, std::vector<std::string>& parts,
+    const std::string& delim);
+
+bool StartWith(const std::string& s, const std::string& start,
+    bool caseSensitive = true);
+
+bool EndWith(const std::string& s, const std::string& end,
+    bool caseSensitive = true);
+
+std::string Replace(const std::string& s, const std::string& partten,
+    const std::string& target);
+}
+}
+#endif//__UT_STRING_TOOL_HPP__

unitree_SDK/include/unitree/common/thread/future.hpp

@@ -0,0 +1,102 @@
+#ifndef __UT_FUTURE_HPP__
+#define __UT_FUTURE_HPP__
+
+#include <unitree/common/thread/thread_decl.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class Future
+{
+public:
+    enum
+    {
+       DEFER = 0,
+       READY = 1,
+       FAULT = 2
+    };
+
+    Future()
+    {}
+
+    virtual ~Future()
+    {}
+
+    bool IsDeferred()
+    {
+        return GetState() == DEFER;
+    }
+
+    bool IsReady()
+    {
+        return GetState() == READY;
+    }
+
+    bool IsFault()
+    {
+        return GetState() == FAULT;
+    }
+
+    virtual int32_t GetState() = 0;
+    virtual bool Wait(int64_t microsec = 0) = 0;
+    virtual const Any& GetValue(int64_t microsec = 0) = 0;
+    virtual const Any& GetFaultMessage() = 0;
+
+public:
+    virtual void Ready(const Any& value) = 0;
+    virtual void Fault(const Any& message) = 0;
+};
+
+typedef std::shared_ptr<Future> FuturePtr;
+
+class FutureWrapper : public Future
+{
+public:
+    FutureWrapper();
+    virtual ~FutureWrapper();
+
+    virtual int32_t GetState()
+    {
+        return mFuturePtr->GetState();       
+    }
+
+    virtual bool Wait(int64_t microsec = 0)
+    {
+        return mFuturePtr->Wait(microsec);
+    }
+
+    virtual const Any& GetValue(int64_t microsec = 0)
+    {
+        return mFuturePtr->GetValue(microsec);
+    }
+
+    virtual const Any& GetFaultMessage()
+    {
+        return mFuturePtr->GetFaultMessage();
+    }
+
+    std::shared_ptr<Future> GetFuture()
+    {
+        return mFuturePtr;
+    }
+
+public:
+    virtual void Ready(const Any& value)
+    {
+        return mFuturePtr->Ready(value);
+    }
+
+    virtual void Fault(const Any& message)
+    {
+        return mFuturePtr->Fault(message);
+    }
+
+protected:
+    std::shared_ptr<Future> mFuturePtr;
+};
+
+}
+}
+
+#endif//__UT_FUTURE_HPP__

unitree_SDK/include/unitree/common/thread/recurrent_thread.hpp

@@ -0,0 +1,84 @@
+#ifndef __UT_RECURRENT_THREAD_HPP__
+#define __UT_RECURRENT_THREAD_HPP__
+
+#include <unitree/common/thread/thread.hpp>
+
+#define UT_THREAD_TIME_INTERVAL_MICROSEC 1000000
+
+namespace unitree
+{
+namespace common
+{
+class RecurrentThread : public Thread
+{
+public:
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    explicit RecurrentThread(uint64_t intervalMicrosec, __UT_THREAD_TMPL_FUNC_ARG__)
+        : mQuit(false), mIntervalMicrosec(intervalMicrosec)
+    {
+        //recurrent function
+        mFunc = std::bind(__UT_THREAD_BIND_FUNC_ARG__);
+
+        //Call Thread::Run for runing thread
+        if (mIntervalMicrosec == 0)
+        {
+            Run(&RecurrentThread::ThreadFunc_0, this);
+        }
+        else
+        {
+            Run(&RecurrentThread::ThreadFunc, this);
+        }
+    }
+
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    explicit RecurrentThread(const std::string& name, int32_t cpuId, uint64_t intervalMicrosec,
+        __UT_THREAD_TMPL_FUNC_ARG__)
+        : Thread(name, cpuId), mQuit(false), mIntervalMicrosec(intervalMicrosec)
+    {
+        //recurrent function
+        mFunc = std::bind(__UT_THREAD_BIND_FUNC_ARG__);
+
+        //Call Thread::Run for runing thread
+        if (mIntervalMicrosec == 0)
+        {
+            Run(&RecurrentThread::ThreadFunc_0, this);
+        }
+        else
+        {
+            Run(&RecurrentThread::ThreadFunc, this);
+        }
+    }
+
+    virtual ~RecurrentThread();
+
+    int32_t ThreadFunc();
+    int32_t ThreadFunc_0();
+
+    bool Wait(int64_t microsec = 0);
+
+private:
+    volatile bool mQuit;
+    uint64_t mIntervalMicrosec;
+    std::function<void()> mFunc;
+};
+
+typedef std::shared_ptr<RecurrentThread> RecurrentThreadPtr;
+
+__UT_THREAD_DECL_TMPL_FUNC_ARG__
+ThreadPtr CreateRecurrentThread(uint64_t intervalMicrosec, __UT_THREAD_TMPL_FUNC_ARG__)
+{
+    return ThreadPtr(new RecurrentThread(intervalMicrosec, __UT_THREAD_BIND_FUNC_ARG__));
+}
+
+__UT_THREAD_DECL_TMPL_FUNC_ARG__
+ThreadPtr CreateRecurrentThreadEx(const std::string& name, int32_t cpuId, uint64_t intervalMicrosec,
+    __UT_THREAD_TMPL_FUNC_ARG__)
+{
+    return ThreadPtr(new RecurrentThread(name, cpuId, intervalMicrosec,
+        __UT_THREAD_BIND_FUNC_ARG__));
+}
+
+}
+}
+
+#endif//__UT_RECURRENT_THREAD_HPP__

unitree_SDK/include/unitree/common/thread/thread.hpp

@@ -0,0 +1,79 @@
+#ifndef __UT_THREAD_HPP__
+#define __UT_THREAD_HPP__
+
+#include <unitree/common/thread/future.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class Thread : public FutureWrapper
+{
+public:
+    Thread()
+        : mThreadId(0), mCpuId(UT_CPU_ID_NONE)
+    {}
+
+    Thread(const std::string& name, int32_t cpuId)
+        : mThreadId(0), mName(name), mCpuId(cpuId)
+    {}
+
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    explicit Thread(__UT_THREAD_TMPL_FUNC_ARG__)
+        : Thread()
+    {
+        Run(__UT_THREAD_BIND_FUNC_ARG__);
+    }
+
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    explicit Thread(const std::string& name, int32_t cpuId, __UT_THREAD_TMPL_FUNC_ARG__)
+        : Thread(name, cpuId)
+    {
+        Run(__UT_THREAD_BIND_FUNC_ARG__);
+    }
+
+    virtual ~Thread();
+
+    uint64_t GetThreadId() const;
+
+    void SetCpu();
+    void SetName();
+    void SetPriority(int32_t priority);
+
+    void Wrap();
+
+protected:
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    void Run(__UT_THREAD_TMPL_FUNC_ARG__)
+    {
+        mFunc = std::bind(__UT_THREAD_BIND_FUNC_ARG__);
+        CreateThreadNative();
+    }
+
+    void CreateThreadNative();
+
+protected:
+    pthread_t mThreadId;
+    std::string mName;
+    int32_t mCpuId;
+    std::function<Any()> mFunc;
+};
+
+typedef std::shared_ptr<Thread> ThreadPtr;
+
+__UT_THREAD_DECL_TMPL_FUNC_ARG__
+static inline ThreadPtr CreateThread(__UT_THREAD_TMPL_FUNC_ARG__)
+{
+    return ThreadPtr(new Thread(__UT_THREAD_BIND_FUNC_ARG__));
+}
+
+__UT_THREAD_DECL_TMPL_FUNC_ARG__
+static inline ThreadPtr CreateThreadEx(const std::string& name, int32_t cpuId, __UT_THREAD_TMPL_FUNC_ARG__)
+{
+    return ThreadPtr(new Thread(name, cpuId, __UT_THREAD_BIND_FUNC_ARG__));
+}
+
+}
+}
+
+#endif//__UT_THREAD_HPP__

unitree_SDK/include/unitree/common/thread/thread_decl.hpp

@@ -0,0 +1,20 @@
+#ifndef __UT_THREAD_DECL_HPP__
+#define __UT_THREAD_DECL_HPP__
+
+#include <unitree/common/any.hpp>
+#include <unitree/common/exception.hpp>
+#include <unitree/common/lock/lock.hpp>
+#include <unitree/common/time/time_tool.hpp>
+
+#define __UT_THREAD_DECL_TMPL_FUNC_ARG__    \
+    template<class Func, class... Args>
+
+#define __UT_THREAD_TMPL_FUNC_ARG__         \
+    Func&& func, Args&&... args
+
+#define __UT_THREAD_BIND_FUNC_ARG__         \
+    std::forward<Func>(func), std::forward<Args>(args)...
+
+#define UT_CPU_ID_NONE  -1
+
+#endif//__THREAD_DECL_HPP__

unitree_SDK/include/unitree/common/thread/thread_pool.hpp

@@ -0,0 +1,98 @@
+#ifndef __UT_THREAD_POOL_HPP__
+#define __UT_THREAD_POOL_HPP__
+
+#include <unitree/common/log/log.hpp>
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/common/thread/thread_task.hpp>
+#include <unitree/common/block_queue.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class ThreadPool
+{
+public:
+    enum
+    {
+        /*
+         * minimum threads can be created.
+         */
+        MIN_THREAD_NUMBER = 1,
+        /*
+         * maximum threads can be created.
+         */
+        MAX_THREAD_NUMBER = 1000,
+        /*
+         * default timeout get task from blockqueue.
+         * 1 second
+         */
+        QUEUE_GET_TIMEOUT_MICROSEC = 1000000,
+        /*
+         * default max queue size.
+         * as UT_QUEUE_MAX_LEN
+         */
+        MAX_QUEUE_SIZE = UT_QUEUE_MAX_LEN,
+        /*
+         * default in queue time in microsecond.
+         * 7 days
+         */
+        MAX_QUEUE_MICROSEC = 25200000000
+    };
+
+    explicit ThreadPool(uint32_t threadNumber = MIN_THREAD_NUMBER,
+        uint32_t queueMaxSize = UT_QUEUE_MAX_LEN,
+        uint64_t taskMaxQueueMicrosec = MAX_QUEUE_MICROSEC);
+
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    bool AddTask(__UT_THREAD_TMPL_FUNC_ARG__)
+    {
+        return AddTaskInner(ThreadTaskPtr(new ThreadTask(__UT_THREAD_BIND_FUNC_ARG__)));
+    }
+
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    FuturePtr AddTaskFuture(__UT_THREAD_TMPL_FUNC_ARG__)
+    {
+        ThreadTaskFuturePtr taskPtr = ThreadTaskFuturePtr(
+            new ThreadTaskFuture(__UT_THREAD_BIND_FUNC_ARG__));
+
+        if (AddTaskInner(std::dynamic_pointer_cast<ThreadTask>(taskPtr)))
+        {
+            return taskPtr->GetFuture();
+        }
+
+        return FuturePtr();
+    }
+
+    int32_t DoTask();
+    uint64_t GetTaskSize();
+
+    bool IsQuit();
+    void Quit(bool waitThreadExit = true);
+
+    bool IsTaskOverdue(uint64_t enqueueTime);
+
+private:
+    bool AddTaskInner(ThreadTaskPtr taskptr);
+
+    void InitCreateThread();
+    void WaitThreadExit();
+
+private:
+    volatile bool mQuit;
+
+    uint32_t mThreadNumber;
+    uint32_t mTaskQueueMaxSize;
+    uint64_t mTaskMaxQueueTime;
+
+    BlockQueue<ThreadTaskPtr> mTaskQueue;
+    std::vector<ThreadPtr> mThreadList;
+
+    Logger* mLogger;
+};
+
+typedef std::shared_ptr<ThreadPool> ThreadPoolPtr;
+
+}
+}
+#endif//__UT_THREAD_POOL_HPP__

unitree_SDK/include/unitree/common/thread/thread_task.hpp

@@ -0,0 +1,47 @@
+#ifndef __UT_THREAD_TASK_HPP__
+#define __UT_THREAD_TASK_HPP__
+
+#include <unitree/common/thread/future.hpp>
+
+namespace unitree
+{
+namespace common
+{
+class ThreadTask
+{
+public:
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    explicit ThreadTask(__UT_THREAD_TMPL_FUNC_ARG__)
+    {
+        mFunc = std::bind(__UT_THREAD_BIND_FUNC_ARG__);
+    }
+
+    virtual void Execute();
+
+    void SetEnqueueTime();
+    uint64_t GetEnqueueTime() const;
+
+protected:
+    uint64_t mEnqueueTimeMicrosec;
+    std::function<Any()> mFunc;
+};
+
+typedef std::shared_ptr<ThreadTask> ThreadTaskPtr;
+
+class ThreadTaskFuture : public ThreadTask, public FutureWrapper
+{
+public:
+    __UT_THREAD_DECL_TMPL_FUNC_ARG__
+    explicit ThreadTaskFuture(__UT_THREAD_TMPL_FUNC_ARG__)
+        : ThreadTask(__UT_THREAD_BIND_FUNC_ARG__)
+    {}
+
+    void Execute();
+};
+
+typedef std::shared_ptr<ThreadTaskFuture> ThreadTaskFuturePtr;
+
+}
+}
+
+#endif//__UT_THREAD_TASK_HPP__

unitree_SDK/include/unitree/common/time/sleep.hpp

@@ -0,0 +1,17 @@
+#ifndef __UT_SLEEP_HPP__
+#define __UT_SLEEP_HPP__
+
+#include <unitree/common/time/time_tool.hpp>
+
+namespace unitree
+{
+namespace common
+{
+void MicroSleep(uint64_t microsecond);
+void MilliSleep(uint64_t millisecond);
+void Sleep(uint64_t second);
+
+}
+}
+
+#endif//__UT_SLEEP_HPP__

unitree_SDK/include/unitree/common/time/time_tool.hpp

@@ -0,0 +1,81 @@
+#ifndef __UT_TIME_TOOL_HPP__
+#define __UT_TIME_TOOL_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree
+{
+namespace common
+{
+#define UT_NUMER_NANO   1000000000
+#define UT_NUMER_MICRO  1000000
+#define UT_NUMER_MILLI  1000
+
+//default time format: "YEAR-MON-DAY HOUR:MIN:SEC"
+#define UT_TIME_FORMAT_STR  "%d-%02d-%02d %02d:%02d:%02d"
+
+//default precise time format: "YEAR-MON-DAY HOUR:MIN:SEC.[MILLI/MICRO]"
+#define UT_TIME_MICROSEC_FORMAT_STR "%d-%02d-%02d %02d:%02d:%02d.%06d"
+#define UT_TIME_MILLISEC_FORMAT_STR "%d-%02d-%02d %02d:%02d:%02d.%03d"
+
+void GetCurrentTimeval(struct timeval& tv);
+void GetCurrentTimespec(struct timespec& ts);
+
+uint64_t GetCurrentTime();
+uint64_t GetCurrentTimeNanosecond();
+uint64_t GetCurrentTimeMicrosecond();
+uint64_t GetCurrentTimeMillisecond();
+
+uint64_t GetCurrentCpuTimeNanosecond();
+uint64_t GetCurrentThreadCpuTimeNanosecond();
+uint64_t GetCurrentMonotonicTimeNanosecond();
+
+uint64_t GetCurrentCpuTimeMicrosecond();
+uint64_t GetCurrentThreadCpuTimeMicrosecond();
+uint64_t GetCurrentMonotonicTimeMicrosecond();
+
+uint64_t TimevalToMicrosecond(const struct timeval& tv);
+uint64_t TimevalToMillisecond(const struct timeval& tv);
+
+uint64_t TimespecToMicrosecond(const struct timespec& ts);
+uint64_t TimespecToMillisecond(const struct timespec& ts);
+
+void MicrosecondToTimeval(uint64_t microsec, struct timeval& tv);
+void MillisecondToTimeval(uint64_t millisec, struct timeval& tv);
+
+void MicrosecondToTimespec(uint64_t microsec, struct timespec& ts);
+void MillisecondToTimespec(uint64_t millisec, struct timespec& ts);
+
+std::string TimeFormatString(struct tm* tmptr, const char* format = UT_TIME_FORMAT_STR);
+std::string TimeFormatString(struct tm* tmptr, uint64_t precise, const char* format);
+std::string TimeFormatString(uint64_t sec, const char* format = UT_TIME_FORMAT_STR);
+
+std::string TimeMicrosecondFormatString(const uint64_t& microsec,
+    const char* format = UT_TIME_MICROSEC_FORMAT_STR);
+
+std::string TimeMillisecondFormatString(const uint64_t& millisec,
+    const char* format = UT_TIME_MILLISEC_FORMAT_STR);
+
+std::string GetTimeString();
+std::string GetTimeMicrosecondString();
+std::string GetTimeMillisecondString();
+
+class Timer
+{
+public:
+    Timer();
+    ~Timer();
+
+    void Start();
+    void Restart();
+
+    uint64_t Stop();
+
+private:
+    uint64_t mMicrosecond;
+};
+
+}
+}
+
+#endif//__UT_TIME_TOOL_HPP__