init

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__