20210906 vehicle status and parking records update, minor bug fixed.

tool/TaskQueue/BaseTask.cpp

@@ -0,0 +1,33 @@
+//
+//  BaseTaskQueue.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "BaseTask.h"
+
+namespace tq {
+    BaseTask::BaseTask():ITask(),_cancelled(false)
+    {
+    }
+    
+    void BaseTask::Cancel()
+    {
+        _cancelled = true;
+    }
+    
+    bool BaseTask::IsCancelled() const
+    {
+        return _cancelled;
+    }
+    
+    void BaseTask::Run()
+    {
+        if (_cancelled) {
+            return;
+        }
+        Main();
+    }
+}

tool/TaskQueue/BaseTask.h

@@ -0,0 +1,29 @@
+//
+//  BaseTaskQueue.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__BaseTaskQueue__
+#define __LibDriveRating_CXX__BaseTaskQueue__
+
+#include "TQInterface.h"
+namespace tq
+{
+    class BaseTask : public ITask
+    {
+    private:
+        volatile bool _cancelled;
+    public:
+        BaseTask();
+        void Run();
+        virtual void Cancel();
+        bool IsCancelled() const;
+        
+        virtual void Main() = 0;
+        TaskCategory GetCategory() const {return NoCategory;}
+    };
+}
+#endif /* defined(__LibDriveRating_CXX__BaseTaskQueue__) */

tool/TaskQueue/TQFactory.cpp

@@ -0,0 +1,23 @@
+//
+//  TQFactory.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/10/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "TQFactory.h"
+#include "ThreadTaskQueue.h"
+namespace tq {
+    IQueue* TQFactory::CreateDefaultQueue()
+    {
+        return new ThreadTaskQueue();
+    }
+    
+    void TQFactory::ReleaseQueue(IQueue* queue)
+    {
+        delete queue;
+    }
+    
+    
+}

tool/TaskQueue/TQFactory.h

@@ -0,0 +1,24 @@
+//
+//  TQFactory.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/10/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__TQFactory__
+#define __LibDriveRating_CXX__TQFactory__
+
+#include "TQInterface.h"
+
+namespace tq {
+    class TQFactory
+    {
+    public:
+        static IQueue* CreateDefaultQueue();
+
+        static void ReleaseQueue(IQueue* queue);
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__TQFactory__) */

tool/TaskQueue/TQInterface.h

@@ -0,0 +1,58 @@
+//
+//  TQInterface.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__TQInterface__
+#define __LibDriveRating_CXX__TQInterface__
+
+namespace tq {
+    typedef unsigned long TaskCategory;
+
+    const TaskCategory NoCategory = 0;
+
+    class ITask
+    {
+    public:
+        virtual void Run() = 0;
+        virtual void Cancel() = 0;
+        virtual bool IsCancelled() const = 0;
+        virtual TaskCategory GetCategory() const = 0;
+        virtual ~ITask(){}
+    };
+    
+    typedef void (*TaskRecycler)(ITask* task,void* context);
+
+    class IQueue
+    {
+    public:
+        virtual void Start(unsigned int nThreads = 1) = 0;
+
+        virtual void Stop() = 0;
+
+        virtual void AddTask(ITask* task) = 0;
+        
+        virtual void GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const= 0;
+        
+        virtual unsigned int TaskCount() const = 0;
+        
+        virtual void CancelAll() = 0;
+        
+        virtual void WaitForFinish() = 0;
+
+        virtual void Suspend() = 0;
+        
+        virtual void Resume() = 0;
+        
+        virtual void SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context){}
+
+        virtual void ClearTaskRecycler(TaskCategory cat){}
+
+        virtual ~IQueue() {}
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__TQInterface__) */

tool/TaskQueue/TaskPool.h

@@ -0,0 +1,89 @@
+#ifndef H_TASK_POOL_H
+#define H_TASK_POOL_H
+#include <list>
+#include "threadpp/threadpp.h"
+#include <cstdlib>
+#include <cstdio>
+namespace tq
+{
+    template <typename TaskType>
+    class TaskPool
+    {
+    public:
+        TaskPool(unsigned capacity = 10);
+        ~TaskPool();
+        TaskType* GetTask(TaskType const& taskPrototype);
+        void RecycleTask(TaskType* task);
+        void Purge();
+    private:
+        threadpp::lock _mutex;
+        std::list<TaskType*> _tasks;
+        unsigned _capacity;
+    };
+
+    template <typename TaskType>
+    TaskType* TaskPool<TaskType>::GetTask(TaskType const& taskPrototype)
+    {
+        _mutex.lock();
+        if(!_tasks.empty())
+        {
+
+            TaskType* taskptr = _tasks.front();
+            _tasks.pop_front();
+            _mutex.unlock();
+            new (taskptr)TaskType(taskPrototype);
+            return taskptr;
+        }
+        else
+        {
+            _mutex.unlock();
+            static int newcount = 0;
+            TaskType* taskptr = new TaskType(taskPrototype);
+            return taskptr;
+        }
+    }
+
+    template <typename TaskType>
+    void TaskPool<TaskType>::RecycleTask(TaskType* task)
+    {
+        (*task).~TaskType();
+        _mutex.lock();
+        if(_tasks.size()<_capacity)
+        {
+            _tasks.push_back(task);
+        }
+        else
+        {
+            free(task);
+        }
+        _mutex.unlock();
+    }
+    
+    template <typename TaskType>
+    void TaskPool<TaskType>::Purge()
+    {
+        _mutex.lock();
+        if(!_tasks.empty())
+        {
+            for(typename std::list<TaskType*>::const_iterator it = _tasks.begin();it!=_tasks.end();++it)
+            {
+                free(*it);
+            }
+            _tasks.clear();
+        }
+        _mutex.unlock();
+    }
+
+    template <typename TaskType>
+    TaskPool<TaskType>::TaskPool(unsigned capacity):
+    _capacity(capacity)
+    {
+    }
+
+    template <typename TaskType>
+    TaskPool<TaskType>::~TaskPool()
+    {
+        this->Purge();
+    }
+}
+#endif

tool/TaskQueue/ThreadTaskQueue.cpp

@@ -0,0 +1,288 @@
+//
+//  ThreadTaskQueue.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "ThreadTaskQueue.h"
+#include <algorithm>
+#define WAIT_TIMEOUT 5000
+
+using namespace threadpp;
+
+namespace tq{
+    
+    class QueueRunnable
+    {
+        ThreadTaskQueue* queue;
+        ITask* currentTask;
+    protected:
+        static void run_callback(void*);
+        thread* th;
+        
+        QueueRunnable(ThreadTaskQueue* q):queue(q),currentTask(NULL){}
+        void run();
+        void CancelCurrentTask();
+        bool TaskIsRunning() const;
+        friend class ThreadTaskQueue;
+    };
+    
+    void QueueRunnable::run_callback(void *ctx)
+    {
+        ((QueueRunnable*) ctx)->run();
+    }
+    
+    void QueueRunnable::run()
+    {
+        while (queue->IsStarted()) {
+            queue->LockQueue();
+            ITask* task = queue->NextTask();
+            if (task == NULL) {
+                queue->UnlockQueue();
+                continue;
+            }
+            currentTask = task;
+            queue->UnlockQueue();
+            task->Run();
+
+            queue->LockQueue();
+            currentTask = NULL;
+            queue->FinishTask(task);
+            queue->NotifyQueue();
+            queue->UnlockQueue();
+        }
+    }
+    
+    void QueueRunnable::CancelCurrentTask()
+    {
+        queue->LockQueue();
+        if(currentTask)
+        {
+            currentTask->Cancel();
+        }
+        queue->UnlockQueue();
+    }
+    
+    bool QueueRunnable::TaskIsRunning() const
+    {
+        return currentTask != NULL;
+    }
+    
+    ThreadTaskQueue::ThreadTaskQueue():_tasklist(),_started(false),_suspended(false)
+    {
+        
+    }
+    
+    void ThreadTaskQueue::Start(unsigned int nThreads)
+    {
+        _mutex.lock();
+        if (_started) {
+            _mutex.unlock();
+            return;
+        }
+        _started = true;
+        _threads.reserve(nThreads);
+        for (int i = 0; i<nThreads; ++i) {
+            QueueRunnable* runnable = new QueueRunnable(this);
+            runnable->th = new thread(QueueRunnable::run_callback, runnable);
+            _threads.push_back(runnable);
+        }
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::Stop()
+    {
+        _mutex.lock();
+        if (!_started) {
+            _mutex.unlock();
+            return;
+        }
+        _started = false;
+        for (std::list<ITask*>::iterator it = _tasklist.begin(); it!= _tasklist.end(); ++it) {
+            delete *it;
+        }
+        _tasklist.clear();
+        _mutex.notify_all();
+        std::vector<QueueRunnable*> copy(_threads);
+        _threads.clear();
+        _mutex.unlock();
+        for (std::vector<QueueRunnable*>::iterator it = copy.begin(); it!=copy.end(); ++it) {
+            (*it)->th->join();
+            thread* t = (*it)->th;
+            delete (*it);
+            delete t;
+        }
+    }
+    
+    bool ThreadTaskQueue::IsStarted() const
+    {
+        return _started;
+    }
+
+
+    void ThreadTaskQueue::AddTask(ITask* task)
+    {
+        _mutex.lock();
+        if (_started) {
+            _tasklist.push_back(task);
+            _mutex.notify_all();
+        }
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const
+    {
+        recursivelock* mutex = const_cast<recursivelock*>(&_mutex);
+        mutex->lock();
+        size_t count = 0;
+        for (std::list<ITask*>::const_iterator it = _tasklist.begin(); it!=_tasklist.end(); ++it) {
+            if (count<taskBufSize) {
+                tasksBuf[count] = *it;
+                count++;
+            }
+            else
+            {
+                break;
+            }
+        }
+        mutex->unlock();
+    }
+    
+    unsigned int ThreadTaskQueue::TaskCount() const
+    {
+        recursivelock* mutex = const_cast<recursivelock*>(&_mutex);
+        mutex->lock();
+        unsigned int count = (unsigned int)_tasklist.size();
+        mutex->unlock();
+        return count;
+    }
+    
+    void ThreadTaskQueue::CancelAll()
+    {
+        _mutex.lock();
+        for (std::vector<QueueRunnable*>::iterator it = _threads.begin(); it!=_threads.end(); ++it) {
+            (*it)->CancelCurrentTask();
+        }
+        for (std::list<ITask*>::const_iterator it = _tasklist.begin(); it!=_tasklist.end(); ++it) {
+            (*it)->Cancel();
+        }
+        _mutex.unlock();
+        
+    }
+    
+    void ThreadTaskQueue::WaitForFinish()
+    {
+        
+        while (true) {
+            _mutex.lock();
+            bool isExecuting = false;
+            for (std::vector<QueueRunnable*>::iterator it = _threads.begin(); it!=_threads.end(); ++it) {
+                if ((*it)->TaskIsRunning()) {
+                    isExecuting = true;
+                    break;
+                }
+            }
+            if (!isExecuting&&_tasklist.size() == 0) {
+                _mutex.unlock();
+                break;
+            }
+            _mutex.wait(100);
+            _mutex.unlock();
+        }
+        
+    }
+    
+    void ThreadTaskQueue::Suspend()
+    {
+        _mutex.lock();
+        _suspended = true;
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::Resume()
+    {
+        _mutex.lock();
+        _suspended = false;
+        _mutex.notify_all();
+        _mutex.unlock();
+
+    }
+    
+    void ThreadTaskQueue::NotifyQueue()
+    {
+        _mutex.notify_all();
+    }
+    
+    ITask* ThreadTaskQueue::NextTask()
+    {
+        while (_started && (_tasklist.empty()||_suspended)) {
+            _mutex.wait(WAIT_TIMEOUT);//defensive waiting time limit.
+        }
+        ITask* task = NULL;
+        if (_tasklist.size()>0) {
+            task = _tasklist.front();
+            _tasklist.pop_front();
+        }
+        return task;
+    }
+    
+    inline
+    void ThreadTaskQueue::LockQueue()
+    {
+        _mutex.lock();
+    }
+    
+    inline
+    void ThreadTaskQueue::UnlockQueue()
+    {
+        _mutex.unlock();
+    }
+
+    inline
+    void ThreadTaskQueue::FinishTask(ITask* task)
+    {
+        if(task->GetCategory() != NoCategory)
+        {
+            _recyclerMutex.lock();
+            std::map<TaskCategory,RecyclerPair>::iterator it = _recyclers.find(task->GetCategory());
+            if(it!=_recyclers.end())
+            {
+                RecyclerPair pair = it->second;
+                _recyclerMutex.unlock();
+                pair.recycler(task,pair.context);
+                return;
+            }
+            _recyclerMutex.unlock();
+        }
+        //析构任务
+        delete task;
+
+    }
+    
+    void ThreadTaskQueue::SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context)
+    {
+        _recyclerMutex.lock();
+        std::pair<TaskCategory,RecyclerPair> pair(cat, RecyclerPair(recycler,context));
+        _recyclers.insert(pair);
+        _recyclerMutex.unlock();
+    }
+
+    void ThreadTaskQueue::ClearTaskRecycler(TaskCategory cat)
+    {
+        _recyclerMutex.lock();
+        std::map<TaskCategory,RecyclerPair>::iterator it = _recyclers.find(cat);
+        if(it!=_recyclers.end())
+        {
+            _recyclers.erase(it);
+        }
+        _recyclerMutex.unlock();
+    }
+
+    ThreadTaskQueue::~ThreadTaskQueue()
+    {
+        this->Stop();//Defensive stop.
+    }
+    
+}

tool/TaskQueue/ThreadTaskQueue.h

@@ -0,0 +1,89 @@
+//
+//  ThreadTaskQueue.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__ThreadTaskQueue__
+#define __LibDriveRating_CXX__ThreadTaskQueue__
+
+#include "TQInterface.h"
+#include "BaseTask.h"
+#include <list>
+#include <map>
+#include <vector>
+#include "threadpp/threadpp.h"
+
+namespace tq
+{
+    class QueueRunnable;
+    
+    class ThreadTaskQueue:public IQueue
+    {
+    public:
+        ThreadTaskQueue();
+        
+        void Start(unsigned int nThreads = 1);
+        
+        void Stop();
+        
+        bool IsStarted() const;
+
+        void AddTask(ITask* task);//Will transfer the ownership of task.
+        
+        void GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const;
+        
+        unsigned int TaskCount() const;
+        
+        void CancelAll();
+        
+        void WaitForFinish() ;
+        
+        void Suspend();
+        
+        void Resume();
+
+        void SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context);
+
+        void ClearTaskRecycler(TaskCategory cat);
+        
+        ~ThreadTaskQueue();
+        
+    protected:
+        
+        void LockQueue();
+        
+        void UnlockQueue();
+        
+        void NotifyQueue();
+        
+        ITask* NextTask();
+
+        void FinishTask(ITask* task);
+        
+        friend class QueueRunnable;
+
+    private:
+        struct RecyclerPair
+        {
+            TaskRecycler recycler;
+            void *context;
+            RecyclerPair(TaskRecycler r,void* c):
+                recycler(r),context(c)
+            {
+            }
+        };
+
+        std::map<TaskCategory,RecyclerPair> _recyclers;
+        std::list<ITask*> _tasklist;
+        std::vector<QueueRunnable*> _threads;
+        threadpp::recursivelock _mutex;
+        threadpp::lock _recyclerMutex;
+        bool _started;
+        bool _suspended;
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__ThreadTaskQueue__) */

tool/TaskQueue/threadpp/impl/pthread_lock.h

@@ -0,0 +1,35 @@
+//
+//  pthread_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_lock__
+#define __threadpp__pthread_lock__
+//extern "C"
+//{
+#include <pthread.h>
+//}
+namespace threadpp
+{
+    class pthread_lock
+    {
+        pthread_mutex_t _mutex;
+        pthread_cond_t _cond;
+        void operator=(const pthread_lock& l){};
+        pthread_lock(const pthread_lock& l){};
+    public:
+        pthread_lock();
+        ~pthread_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+#include "pthread_lock.hpp"
+#endif /* defined(__threadpp__pthread_lock__) */

tool/TaskQueue/threadpp/impl/pthread_lock.hpp

@@ -0,0 +1,73 @@
+//
+//  pthread_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__pthread_lock__hpp__
+#define __threadpp__pthread_lock__hpp__
+#include "../threadpp_assert.h"
+#include <errno.h>
+#include <cstring>
+
+#include <sys/time.h>
+
+static inline void timespec_for(struct timespec* t,unsigned long millisecs) {
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    t->tv_nsec = tv.tv_usec*1000 + millisecs*1000000;
+    t->tv_sec = tv.tv_sec + (t->tv_nsec/1000000000);
+    t->tv_nsec = t->tv_nsec%1000000000;
+}
+
+namespace threadpp{
+    inline pthread_lock::pthread_lock()
+    {
+        pthread_mutex_init(&_mutex, NULL);
+        pthread_cond_init(&_cond, NULL);
+    }
+    
+    inline pthread_lock::~pthread_lock()
+    {
+        pthread_mutex_destroy(&_mutex);
+        pthread_cond_destroy(&_cond);
+    }
+    
+    inline void pthread_lock::lock()
+    {
+        int code = pthread_mutex_lock(&_mutex);
+        ASSERT(code == 0, "lock failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::unlock()
+    {
+        int code = pthread_mutex_unlock(&_mutex);
+        ASSERT(code == 0, "unlock failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::wait()
+    {
+        int code = pthread_cond_wait(&_cond, &_mutex);
+        ASSERT(code == 0 || code == ETIMEDOUT, "wait failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::wait(unsigned long millisecs)
+    {
+        struct timespec ts;
+        timespec_for(&ts,millisecs);
+        int code = pthread_cond_timedwait(&_cond, &_mutex, &ts);
+        ASSERT(code == 0 || code == ETIMEDOUT, "timed wait failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::notify()
+    {
+        pthread_cond_signal(&_cond);
+    }
+    
+    inline void pthread_lock::notify_all()
+    {
+        pthread_cond_broadcast(&_cond);
+    }
+}
+#endif

tool/TaskQueue/threadpp/impl/pthread_thread.h

@@ -0,0 +1,45 @@
+//
+//  pthread_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_thread__
+#define __threadpp__pthread_thread__
+#include <pthread.h>
+namespace threadpp
+{
+    class pthread_thread
+    {
+        struct pthread_context
+        {
+            void(*fp)(void* context);
+            void* context;
+        } _context;
+        
+        pthread_t _thread;
+        pthread_thread(){};
+        void operator=(const pthread_thread& t){};
+        pthread_thread(const pthread_thread& t){};
+        static void* pthread_fp_delegate(void*);
+    public:
+        typedef void (*runnable)(void* ctx);
+        typedef unsigned long long id_type;
+        
+        static id_type null_id();
+        
+        pthread_thread(runnable r,void* t);
+        
+        ~pthread_thread();
+        void join();
+        void detach();
+        bool is_equal(const pthread_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+#include "pthread_thread.hpp"
+#endif /* defined(__threadpp__pthread_thread__) */

tool/TaskQueue/threadpp/impl/pthread_thread.hpp

@@ -0,0 +1,110 @@
+//
+//  pthread_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_thread__hpp__
+#define __threadpp__pthread_thread__hpp__
+
+#include <errno.h>
+#include "../threadpp_assert.h"
+#include <cstring>
+#include <cmath>
+#include <unistd.h>
+namespace threadpp
+{
+    inline pthread_thread::id_type pthread_thread::null_id()
+    {
+        return 0;
+    }
+    
+    inline void* pthread_thread::pthread_fp_delegate(void* ctx)
+    {
+        pthread_thread::pthread_context* pctx =static_cast<pthread_thread::pthread_context*>(ctx);
+        pctx->fp(pctx->context);
+        return NULL;
+    }
+    
+    inline pthread_thread::pthread_thread(runnable r,void* t)
+    {
+        _context.fp = r;
+        _context.context = t;
+        int code = pthread_create(&_thread, NULL, pthread_thread::pthread_fp_delegate, &_context);
+        ASSERT(code==0,"create thread failed,error:%s",strerror(code));
+    }
+    
+//    pthread_thread::pthread_thread(runnable r,void* t,float priority)
+//    {
+//        _context.fp = r;
+//        _context.context = t;
+//        pthread_attr_t tattr;
+//        pthread_attr_init(&tattr);
+//        struct sched_param schp;
+//        int policy = SCHED_FIFO;
+//        pthread_attr_getschedpolicy(&tattr, &policy);
+//        pthread_attr_getschedparam(&tattr, &schp);
+//        float pr =fminf(1.0f,fmaxf(0.0f, priority));
+//        schp.sched_priority = sched_get_priority_min(policy) + pr*(sched_get_priority_max(policy) - sched_get_priority_min(policy));
+//        pthread_attr_setschedparam(&tattr, &schp);
+//        int code = pthread_create(&_thread, &tattr, pthread_thread::pthread_fp_delegate, &_context);
+//        ASSERT(code==0,"create thread failed,error:%s",strerror(code));
+//        pthread_attr_destroy(&tattr);
+//    }
+    
+    inline pthread_thread::~pthread_thread()
+    {
+        ASSERT(_thread == 0,"%s","must join or detach a thread before destructing it");
+    }
+    
+    inline void pthread_thread::join()
+    {
+        void* ret = NULL;
+        int code = pthread_join(_thread, &ret);
+        _thread = 0;
+        ASSERT(code==0,"join thread failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_thread::detach()
+    {
+        int code = pthread_detach(_thread);
+        _thread = 0;
+        ASSERT(code==0,"join thread failed,error:%s",strerror(code));
+
+    }
+    
+    inline bool pthread_thread::is_equal(const pthread_thread& t) const
+    {
+        return pthread_equal(_thread, t._thread);
+    }
+    
+    inline void pthread_thread::sleep(unsigned long millisecs)
+    {
+        usleep((useconds_t)millisecs*1000);
+    }
+    
+    inline pthread_thread::id_type pthread_thread::get_id() const
+    {
+#ifdef __APPLE__
+        __uint64_t tid;
+        return pthread_threadid_np(_thread, &tid);
+        return tid;
+#else
+        return (unsigned long long)(_thread);
+#endif
+    }
+    
+    inline pthread_thread::id_type pthread_thread::current_thread_id()
+    {
+#ifdef __APPLE__
+        __uint64_t tid;
+        pthread_threadid_np(pthread_self(), &tid);
+        return tid;
+#else
+        return (unsigned long long)(pthread_self());
+#endif
+    }
+}
+#endif

tool/TaskQueue/threadpp/impl/std_lock.h

@@ -0,0 +1,33 @@
+//
+//  std_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__std_lock__
+#define __threadpp__std_lock__
+#include <mutex>
+#include <condition_variable>
+namespace threadpp
+{
+    class std_lock
+    {
+        std::mutex _mutex;
+        std::condition_variable_any _cond;
+        void operator=(const std_lock& l){};
+        std_lock(const std_lock& l){};
+    public:
+        std_lock();
+        ~std_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+#include "std_lock.hpp"
+#endif /* defined(__threadpp__std_lock__) */

tool/TaskQueue/threadpp/impl/std_lock.hpp

@@ -0,0 +1,52 @@
+//
+//  std_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__std_lock__hpp__
+#define __threadpp__std_lock__hpp__
+#include "../threadpp_assert.h"
+
+namespace threadpp{
+    
+    inline std_lock::std_lock():_mutex(),_cond()
+    {
+    }
+    
+    inline std_lock::~std_lock()
+    {
+    }
+    
+    inline void std_lock::lock()
+    {
+        _mutex.lock();
+    }
+    
+    inline void std_lock::unlock()
+    {
+        _mutex.unlock();
+    }
+    
+    inline void std_lock::wait()
+    {
+        _cond.wait(_mutex);
+    }
+    
+    inline void std_lock::wait(unsigned long millisecs)
+    {
+        _cond.wait_for(_mutex, std::chrono::milliseconds(millisecs));
+    }
+    
+    inline void std_lock::notify()
+    {
+        _cond.notify_one();
+    }
+    
+    inline void std_lock::notify_all()
+    {
+        _cond.notify_all();
+    }
+}
+#endif

tool/TaskQueue/threadpp/impl/std_thread.h

@@ -0,0 +1,40 @@
+//
+//  std_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__std_thread__
+#define __threadpp__std_thread__
+#include <thread>
+
+namespace threadpp
+{
+    class std_thread
+    {
+        std::thread _thread;
+        std_thread(){};
+        void operator=(const std_thread& t){};
+        std_thread(const std_thread& t){};
+    public:
+        typedef unsigned long long id_type;
+        
+        typedef void (*runnable)(void* ctx);
+        
+        static id_type null_id();
+        
+        std_thread(runnable r,void* t);
+        
+        ~std_thread();
+        void join();
+        void detach();
+        bool is_equal(const std_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+#include "std_thread.hpp"
+#endif /* defined(__threadpp__std_thread__) */

tool/TaskQueue/threadpp/impl/std_thread.hpp

@@ -0,0 +1,64 @@
+//
+//  std_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__std_thread__hpp__
+#define __threadpp__std_thread__hpp__
+#include <functional>
+#include <algorithm>
+namespace threadpp
+{
+    inline std_thread::id_type std_thread::null_id()
+    {
+        return 0;
+    }
+    
+    inline void* std_fp_delegate(std_thread::runnable r, void* context)
+    {
+        r(context);
+        return nullptr;
+    }
+    
+    inline std_thread::std_thread(runnable r,void* t):_thread(std::bind(std_fp_delegate,r,t))
+    {
+    }
+    
+    inline std_thread::~std_thread()
+    {
+        
+    }
+    
+    inline void std_thread::join()
+    {
+        _thread.join();
+    }
+    
+    inline void std_thread::detach()
+    {
+        _thread.detach();
+    }
+    
+    inline bool std_thread::is_equal(const std_thread& t) const
+    {
+        return _thread.get_id() == t._thread.get_id();
+    }
+    
+    inline std_thread::id_type std_thread::get_id() const
+    {
+        return std::hash<std::thread::id>()(_thread.get_id());
+    }
+    
+    inline void std_thread::sleep(unsigned long millisecs)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(millisecs));
+    }
+    
+    inline std_thread::id_type std_thread::current_thread_id()
+    {
+        return std::hash<std::thread::id>()(std::this_thread::get_id());
+    }
+}
+#endif

tool/TaskQueue/threadpp/impl/win_lock.h

@@ -0,0 +1,37 @@
+//
+//  win_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__win_lock__
+#define __threadpp__win_lock__
+
+#include <windows.h>
+
+namespace threadpp
+{
+    class win_lock
+    {
+        CRITICAL_SECTION _mutex;
+        CONDITION_VARIABLE _cond;
+        volatile unsigned int _owner;
+        volatile unsigned int _count;
+        void operator=(const win_lock& l){};
+        win_lock(const win_lock& l){};
+    public:
+        win_lock();
+        ~win_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+
+#include "win_lock.hpp"
+#endif /* defined(__threadpp__win_lock__) */

tool/TaskQueue/threadpp/impl/win_lock.hpp

@@ -0,0 +1,80 @@
+//
+//  win_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__win_lock__hpp__
+#define __threadpp__win_lock__hpp__
+
+#include "win_thread.h"
+#include "../threadpp_assert.h"
+#include "stdio.h"
+namespace threadpp{
+    
+    static inline void test_error(const char* title)
+    {
+        DWORD e = GetLastError();
+        ASSERT(e==0,"%s failed,error:%d",title,e);
+    }
+    
+    inline win_lock::win_lock():_owner(0),_count(0)
+    {
+        SetLastError(0);
+        InitializeCriticalSection(&_mutex);
+        InitializeConditionVariable(&_cond);
+        test_error("init");
+    }
+    
+    inline win_lock::~win_lock()
+    {
+        DeleteCriticalSection(&_mutex);
+    }
+    
+    inline void win_lock::lock()
+    {
+        SetLastError(0);
+        
+        EnterCriticalSection(&_mutex);
+        
+        test_error("lock");
+    }
+    
+    inline void win_lock::unlock()
+    {
+        SetLastError(0);
+        LeaveCriticalSection(&_mutex);
+        test_error("unlock");
+    }
+    
+    inline void win_lock::wait()
+    {
+        SetLastError(0);
+        SleepConditionVariableCS(&_cond,&_mutex,0xFFFFFFFF);
+        test_error("wait");
+    }
+    
+    inline void win_lock::wait(unsigned long millisecs)
+    {
+        SetLastError(0);
+        SleepConditionVariableCS(&_cond,&_mutex,millisecs);
+        DWORD e = GetLastError();
+        ASSERT(e==0||e == ERROR_TIMEOUT,"timed wait failed,error:",e);
+    }
+    
+    inline void win_lock::notify()
+    {
+        SetLastError(0);
+        WakeConditionVariable(&_cond);
+        test_error("notify");
+    }
+    
+    inline void win_lock::notify_all()
+    {
+        SetLastError(0);
+        WakeAllConditionVariable(&_cond);
+        test_error("notify all");
+    }
+}
+#endif

tool/TaskQueue/threadpp/impl/win_thread.h

@@ -0,0 +1,53 @@
+//
+//  win_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__win_thread__
+#define __threadpp__win_thread__
+
+#include <windows.h>
+
+namespace threadpp
+{
+    class win_thread
+    {
+        struct win_context
+        {
+            void(*fp)(void*);
+            void* context;
+        } _context;
+#if NO_CRT
+        typedef DWORD handle_t;
+#else
+		typedef unsigned handle_t;
+#endif
+		static handle_t __stdcall win_fp_delegate(void* context);
+        handle_t _thread_id;
+        HANDLE _handle;
+        win_thread(){};
+        void operator=(const win_thread& t){};
+        win_thread(const win_thread& t){};
+    public:
+        typedef void (*runnable)(void* ctx);
+        typedef unsigned int id_type;
+        
+        static id_type null_id();
+        
+        win_thread(runnable r,void* t);
+        
+        ~win_thread();
+        void join();
+        void detach();
+        bool is_equal(const win_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+
+#include "win_thread.hpp"
+#endif /* defined(__threadpp__win_thread__) */

tool/TaskQueue/threadpp/impl/win_thread.hpp

@@ -0,0 +1,81 @@
+//
+//  win_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__win_thread__hpp__
+#define __threadpp__win_thread__hpp__
+
+#include "../threadpp_assert.h"
+#if NO_CRT
+#else
+#include <process.h>
+#endif
+namespace threadpp
+{
+
+    inline win_thread::id_type win_thread::null_id()
+    {
+        return 0;
+    }
+    
+	inline win_thread::handle_t win_thread::win_fp_delegate(void *context)
+    {
+        win_context* wctx = static_cast<win_context*>(context);
+        wctx->fp(wctx->context);
+        return 0;
+    }
+    
+    inline win_thread::win_thread(runnable r,void* t)
+    {
+        _context.fp = r;
+        _context.context = t;
+#if NO_CRT
+		_handle = CreateThread(NULL,NULL,win_thread::win_fp_delegate,&_context,0,&_thread_id);
+#else
+		_handle = (HANDLE)_beginthreadex(NULL,0,win_thread::win_fp_delegate,&_context,0,&_thread_id);
+#endif		
+    }
+    
+    inline win_thread::~win_thread()
+    {
+        ASSERT(_handle == 0,"%s","must join or detach a thread before destructing it");
+    }
+    
+    inline void win_thread::join()
+    {
+        unsigned code = WaitForSingleObject(_handle,0xFFFFFFFF);
+        CloseHandle(_handle);
+        _handle = 0;
+        ASSERT(code == WAIT_OBJECT_0 || code == WAIT_ABANDONED, "failed to join,error:%d",code);
+    }
+    
+    inline void win_thread::detach()
+    {
+		CloseHandle(_handle);
+        _handle = 0;
+    }
+    
+    inline bool win_thread::is_equal(const win_thread& t) const
+    {
+        return _thread_id == t._thread_id;
+    }
+    
+    inline void win_thread::sleep(unsigned long millisecs)
+    {
+		Sleep(millisecs);
+    }
+    
+	inline win_thread::id_type win_thread::get_id() const
+	{
+		return static_cast<unsigned int>(_thread_id);
+	}
+
+	inline win_thread::id_type win_thread::current_thread_id()
+	{
+		return  static_cast<unsigned int>(GetCurrentThreadId());
+	}
+}
+#endif

tool/TaskQueue/threadpp/recursive_lock.h

@@ -0,0 +1,101 @@
+//
+//  recursive_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/16/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_recursive_lock_h
+#define threadpp_recursive_lock_h
+#include "threadpp_assert.h"
+namespace threadpp
+{
+    template <typename locktype,typename threadtype>
+    class recursive_lock
+    {
+        locktype _lock;
+        volatile typename threadtype::id_type _owner;
+        volatile unsigned int _count;
+    public:
+        recursive_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+    
+    template <typename locktype,typename threadtype>
+    recursive_lock<locktype,threadtype>::recursive_lock():_owner(threadtype::null_id()),_count(0)
+    {
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::lock()
+    {
+        typename threadtype::id_type tid = threadtype::current_thread_id();
+        if(tid == _owner)
+        {
+            _count++;
+        }
+        else
+        {
+            _lock.lock();
+            _owner = tid;
+            _count=1;
+        }
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::unlock()
+    {
+        typename threadtype::id_type tid = threadtype::current_thread_id();
+        ASSERT(tid == _owner,"%s", "unlock failed,try to unlock not owned mutex");
+        _count--;
+        if (_count == 0) {
+            _owner = threadtype::null_id();
+            _lock.unlock();
+        }
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::wait()
+    {
+        typename threadtype::id_type owner = _owner;
+        unsigned count = _count;
+        _owner = threadtype::null_id();
+        _count = 0;
+        _lock.wait();
+        _owner = owner;
+        _count = count;
+
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::wait(unsigned long millisecs)
+    {
+        typename threadtype::id_type owner = _owner;
+        unsigned count = _count;
+        _owner = threadtype::null_id();
+        _count = 0;
+        _lock.wait(millisecs);
+        _owner = owner;
+        _count = count;
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::notify()
+    {
+        _lock.notify();
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::notify_all()
+    {
+        _lock.notify_all();
+    }
+}
+
+#endif

tool/TaskQueue/threadpp/threadpp.h

@@ -0,0 +1,40 @@
+//
+//  threadpp.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_threadpp_h
+#define threadpp_threadpp_h
+#include "recursive_lock.h"
+#if __posix || __APPLE__ || __linux
+#include "impl/pthread_thread.h"
+#include "impl/pthread_lock.h"
+namespace threadpp
+{
+    typedef pthread_thread thread;
+    typedef pthread_lock lock;
+    typedef recursive_lock<pthread_lock, pthread_thread> recursivelock;
+}
+#elif defined(WIN32)
+#include "impl/win_thread.h"
+#include "impl/win_lock.h"
+namespace threadpp
+{
+    typedef win_thread thread;
+    typedef win_lock lock;
+    typedef recursive_lock<win_lock, win_thread> recursivelock;
+}
+#elif __cplusplus>=201103L
+#include "impl/std_thread.h"
+#include "impl/std_lock.h"
+namespace threadpp
+{
+    typedef std_thread thread;
+    typedef std_lock lock;
+    typedef recursive_lock<std_lock, std_thread> recursivelock;
+}
+#endif
+#endif

tool/TaskQueue/threadpp/threadpp_assert.h

@@ -0,0 +1,33 @@
+//
+//  defines.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_assert_h
+#define threadpp_assert_h
+
+//forward VC++ DEBUG symbol.
+#if defined(_DEBUG) && !defined(DEBUG)
+#define DEBUG _DEBUG
+#endif
+
+#if DEBUG //assertions
+#ifdef __cplusplus
+#include <cassert>
+#include <cstdio>
+#else
+#include <assert.h>
+#include <stdio.h>
+#endif
+#define ASSERT0(__cond__) assert(__cond__)
+#define ASSERT(__cond__,__msg__,...) \
+do {if(!(__cond__)){printf(__msg__,__VA_ARGS__);printf("\n");assert(false);}}while(0)
+#else
+#define ASSERT0(__cond__)
+#define ASSERT(__cond__,__msg__,...)
+#endif //assertions
+
+#endif

tool/common_data.cpp

@@ -0,0 +1,319 @@
+//
+// Created by huli on 2020/9/8.
+//
+
+#include "common_data.h"
+
+void Common_data::copy_data(Car_measure_information& car_measure_information_out, const message::Locate_information& locate_information_in)
+{
+	if ( locate_information_in.has_locate_x() )
+	{
+		car_measure_information_out.car_center_x = locate_information_in.locate_x();
+	}
+	else
+	{
+		car_measure_information_out.car_center_x = 0;
+	}
+	if ( locate_information_in.has_locate_y() )
+	{
+		car_measure_information_out.car_center_y = locate_information_in.locate_y();
+	}
+	else
+	{
+		car_measure_information_out.car_center_y = 0;
+	}
+	if ( locate_information_in.has_locate_angle() )
+	{
+		car_measure_information_out.car_angle = locate_information_in.locate_angle();
+	}
+	else
+	{
+		car_measure_information_out.car_angle = 0;
+	}
+	if ( locate_information_in.has_locate_length() )
+	{
+		car_measure_information_out.car_length = locate_information_in.locate_length();
+	}
+	else
+	{
+		car_measure_information_out.car_length = 0;
+	}
+	if ( locate_information_in.has_locate_width() )
+	{
+		car_measure_information_out.car_width = locate_information_in.locate_width();
+	}
+	else
+	{
+		car_measure_information_out.car_width = 0;
+	}
+	if ( locate_information_in.has_locate_height() )
+	{
+		car_measure_information_out.car_height = locate_information_in.locate_height();
+	}
+	else
+	{
+		car_measure_information_out.car_height = 0;
+	}
+	if ( locate_information_in.has_locate_wheel_base() )
+	{
+		car_measure_information_out.car_wheel_base = locate_information_in.locate_wheel_base();
+	}
+	else
+	{
+		car_measure_information_out.car_wheel_base = 0;
+	}
+	if ( locate_information_in.has_locate_wheel_width() )
+	{
+		car_measure_information_out.car_wheel_width = locate_information_in.locate_wheel_width();
+	}
+	else
+	{
+		car_measure_information_out.car_wheel_width = 0;
+	}
+	if ( locate_information_in.has_locate_front_theta() )
+	{
+		car_measure_information_out.car_front_theta = locate_information_in.locate_front_theta();
+	}
+	else
+	{
+		car_measure_information_out.car_front_theta = 0;
+	}
+	if ( locate_information_in.has_locate_correct() )
+	{
+		car_measure_information_out.correctness = locate_information_in.locate_correct();
+	}
+	else
+	{
+		car_measure_information_out.correctness = false;
+	}
+}
+void Common_data::copy_data(message::Locate_information& locate_information_out, const Car_measure_information& car_measure_information_in)
+{
+	locate_information_out.set_locate_x(car_measure_information_in.car_center_x);
+	locate_information_out.set_locate_y(car_measure_information_in.car_center_y);
+	locate_information_out.set_locate_angle(car_measure_information_in.car_angle);
+	locate_information_out.set_locate_length(car_measure_information_in.car_length);
+	locate_information_out.set_locate_width(car_measure_information_in.car_width);
+	locate_information_out.set_locate_height(car_measure_information_in.car_height);
+	locate_information_out.set_locate_wheel_base(car_measure_information_in.car_wheel_base);
+	locate_information_out.set_locate_wheel_width(car_measure_information_in.car_wheel_width);
+	locate_information_out.set_locate_front_theta(car_measure_information_in.car_front_theta);
+	locate_information_out.set_locate_correct(car_measure_information_in.correctness);
+}
+
+void Common_data::copy_data(Car_information& car_information_out, const message::Car_info& car_info_in)
+{
+	if ( car_info_in.has_license() )
+	{
+		car_information_out.license = car_info_in.license();
+	}
+	else
+	{
+		car_information_out.license.clear();
+	}
+	if ( car_info_in.has_car_length() )
+	{
+		car_information_out.car_length = car_info_in.car_length();
+	}
+	else
+	{
+		car_information_out.car_length = 0;
+	}
+	if ( car_info_in.has_car_width() )
+	{
+		car_information_out.car_width = car_info_in.car_width();
+	}
+	else
+	{
+		car_information_out.car_width = 0;
+	}
+	if ( car_info_in.has_car_height() )
+	{
+		car_information_out.car_height = car_info_in.car_height();
+	}
+	else
+	{
+		car_information_out.car_height = 0;
+	}
+	if ( car_info_in.has_car_wheel_base() )
+	{
+		car_information_out.car_wheel_base = car_info_in.car_wheel_base();
+	}
+	else
+	{
+		car_information_out.car_wheel_base = 0;
+	}
+	if ( car_info_in.has_car_wheel_width() )
+	{
+		car_information_out.car_wheel_width = car_info_in.car_wheel_width();
+	}
+	else
+	{
+		car_information_out.car_wheel_width = 0;
+	}
+}
+void Common_data::copy_data(message::Car_info& car_info_out, const Car_information& car_information_in)
+{
+	car_info_out.set_license(car_information_in.license);
+	car_info_out.set_car_length(car_information_in.car_length);
+	car_info_out.set_car_width(car_information_in.car_width);
+	car_info_out.set_car_height(car_information_in.car_height);
+	car_info_out.set_car_wheel_base(car_information_in.car_wheel_base);
+	car_info_out.set_car_wheel_width(car_information_in.car_wheel_width);
+}
+
+void Common_data::copy_data(Parkspace_information& parkspace_information_out, const message::Parkspace_info& parkspace_info_in)
+{
+
+		parkspace_information_out.parkingspace_index_id = parkspace_info_in.parkingspace_index_id();
+		parkspace_information_out.parkingspace_type = (Common_data::Parkspace_type)parkspace_info_in.parkingspace_type();
+
+		parkspace_information_out.parkingspace_unit_id = parkspace_info_in.parkingspace_unit_id();
+		parkspace_information_out.parkingspace_floor_id = parkspace_info_in.parkingspace_floor_id();
+		parkspace_information_out.parkingspace_room_id = parkspace_info_in.parkingspace_room_id();
+		parkspace_information_out.parkingspace_direction = (Common_data::Direction)parkspace_info_in.parkingspace_direction();
+
+		parkspace_information_out.parkingspace_width = parkspace_info_in.parkingspace_width();
+		parkspace_information_out.parkingspace_height = parkspace_info_in.parkingspace_height();
+		parkspace_information_out.parkingspace_status = (Common_data::Parkspace_status)parkspace_info_in.parkingspace_status();
+
+		copy_data(parkspace_information_out.car_information, (message::Car_info&) parkspace_info_in.car_info());
+		parkspace_information_out.car_entry_time = parkspace_info_in.entry_time();
+		parkspace_information_out.car_leave_time = parkspace_info_in.leave_time();
+
+		parkspace_information_out.parkspace_path = (Common_data::Parkspace_path)parkspace_info_in.parkspace_path();
+		parkspace_information_out.path_estimate_time = parkspace_info_in.path_estimate_time();
+		parkspace_information_out.parkspace_status_target = (Common_data::Parkspace_status)parkspace_info_in.parkspace_status_target();
+
+}
+void Common_data::copy_data(message::Parkspace_info& parkspace_info_out, const Parkspace_information& parkspace_information_in)
+{
+	parkspace_info_out.set_parkingspace_index_id(parkspace_information_in.parkingspace_index_id);
+	parkspace_info_out.set_parkingspace_type((message::Parkspace_type)parkspace_information_in.parkingspace_type);
+	parkspace_info_out.set_parkingspace_unit_id(parkspace_information_in.parkingspace_unit_id);
+	parkspace_info_out.set_parkingspace_room_id(parkspace_information_in.parkingspace_room_id);
+	parkspace_info_out.set_parkingspace_direction((message::Direction)parkspace_information_in.parkingspace_direction);
+	parkspace_info_out.set_parkingspace_floor_id(parkspace_information_in.parkingspace_floor_id);
+	parkspace_info_out.set_parkingspace_width(parkspace_information_in.parkingspace_width);
+	parkspace_info_out.set_parkingspace_height(parkspace_information_in.parkingspace_height);
+	parkspace_info_out.set_parkingspace_status((message::Parkspace_status)parkspace_information_in.parkingspace_status);
+	copy_data(*parkspace_info_out.mutable_car_info(), parkspace_information_in.car_information);
+	parkspace_info_out.set_entry_time(parkspace_information_in.car_entry_time);
+	parkspace_info_out.set_leave_time(parkspace_information_in.car_leave_time);
+	parkspace_info_out.set_parkspace_path((message::Parkspace_path)parkspace_information_in.parkspace_path);
+	parkspace_info_out.set_path_estimate_time(parkspace_information_in.path_estimate_time);
+	parkspace_info_out.set_parkspace_status_target((message::Parkspace_status)parkspace_information_in.parkspace_status_target);
+}
+
+
+
+
+
+
+
+
+
+void Common_data::transform_data(Car_information& car_information_out, const Car_measure_information& car_measure_information_in)
+{
+	car_information_out.license.clear();
+	car_information_out.car_length = car_measure_information_in.car_length;
+	car_information_out.car_width = car_measure_information_in.car_width;
+	car_information_out.car_height = car_measure_information_in.car_height;
+	car_information_out.car_wheel_base = car_measure_information_in.car_wheel_base;
+	car_information_out.car_wheel_width = car_measure_information_in.car_wheel_width;
+}
+void Common_data::transform_data(Car_measure_information& car_measure_information_out, const Car_information& car_informatio_in)
+{
+	car_measure_information_out.car_center_x = 0;
+	car_measure_information_out.car_center_y = 0;
+	car_measure_information_out.car_angle = 0;
+	car_measure_information_out.car_length = car_informatio_in.car_length;
+	car_measure_information_out.car_width = car_informatio_in.car_width;
+	car_measure_information_out.car_height = car_informatio_in.car_height;
+	car_measure_information_out.car_wheel_base = car_informatio_in.car_wheel_base;
+	car_measure_information_out.car_wheel_width = car_informatio_in.car_wheel_width;
+	car_measure_information_out.car_front_theta = 0;
+	car_measure_information_out.correctness = false;
+
+
+}
+
+void Common_data::transform_data(Car_wheel_information& car_wheel_information_out, const Car_measure_information& car_measure_information_in)
+{
+	car_wheel_information_out.car_center_x = car_measure_information_in.car_center_x;
+	car_wheel_information_out.car_center_y = car_measure_information_in.car_center_y;
+	car_wheel_information_out.car_angle = car_measure_information_in.car_angle;
+
+	car_wheel_information_out.car_wheel_base = car_measure_information_in.car_wheel_base;
+	car_wheel_information_out.car_wheel_width = car_measure_information_in.car_wheel_width;
+	car_wheel_information_out.car_front_theta = car_measure_information_in.car_front_theta;
+	car_wheel_information_out.correctness = car_measure_information_in.correctness;
+}
+void Common_data::transform_data(Car_measure_information& car_measure_information_out, const Car_wheel_information& car_wheel_information_in)
+{
+	car_measure_information_out.car_center_x = car_wheel_information_in.car_center_x;
+	car_measure_information_out.car_center_y = car_wheel_information_in.car_center_y;
+	car_measure_information_out.car_angle = car_wheel_information_in.car_angle;
+	car_measure_information_out.car_length = 0;
+	car_measure_information_out.car_width = 0;
+	car_measure_information_out.car_height = 0;
+	car_measure_information_out.car_wheel_base = car_wheel_information_in.car_wheel_base;
+	car_measure_information_out.car_wheel_width = car_wheel_information_in.car_wheel_width;
+	car_measure_information_out.car_front_theta = car_wheel_information_in.car_front_theta;
+	car_measure_information_out.correctness = car_wheel_information_in.correctness;
+}
+
+
+void Common_data::scaling(Car_measure_information& car_measure_information, float rate)
+{
+	car_measure_information.car_center_x *= rate;
+	car_measure_information.car_center_y *= rate;
+	car_measure_information.car_length *= rate;
+	car_measure_information.car_width *= rate;
+	car_measure_information.car_height *= rate;
+	car_measure_information.car_wheel_base *= rate;
+	car_measure_information.car_wheel_width *= rate;
+}
+void Common_data::scaling(Car_information& car_information, float rate)
+{
+	car_information.car_length *= rate;
+	car_information.car_width *= rate;
+	car_information.car_height *= rate;
+	car_information.car_wheel_base *= rate;
+	car_information.car_wheel_width *= rate;
+
+}
+void Common_data::scaling(Parkspace_information& parkspace_information, float rate)
+{
+	parkspace_information.parkingspace_width *= rate;
+	parkspace_information.parkingspace_height *= rate;
+	scaling(parkspace_information.car_information, rate);
+}
+
+bool Common_data::approximate_rate(float a, float b, float rate)
+{
+	if ( a >= b*(1-rate) && a< b*(1+rate))
+	{
+	    return true;
+	}
+	else
+	{
+	    return false;
+	}
+}
+
+bool Common_data::approximate_difference(float a, float b, float difference)
+{
+	if ( a >= (b-difference) && a< (b+difference) )
+	{
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+
+
+

tool/common_data.h

@@ -0,0 +1,314 @@
+//
+// Created by huli on 2020/9/8.
+//
+
+#ifndef NNXX_TESTS_COMMON_DATA_H
+#define NNXX_TESTS_COMMON_DATA_H
+
+#include <chrono>
+#include <cmath>
+#include <string>
+#include <string.h>
+#include "../message/message_base.pb.h"
+
+class Common_data
+{
+public:
+	//万集雷达扫描周期66ms, (频率15hz), 一般设置大一些
+#define WANJI_716_SCAN_CYCLE_MS 		75
+	//vlp16雷达扫描周期100ms, (频率10hz), 一般设置大一些
+#define VLP16_SCAN_CYCLE_MS 		110
+
+
+//车位表
+#define PARKSPACE_ID_BASE							0
+#define PASSAGEWAY_ID_BASE							1100
+
+//唯一码的默认长度 32byte
+#define COMMAND_KEY_DEFAULT_LENGTH					32
+
+
+
+	
+	//整车的测量信息
+	struct Car_measure_information
+	{
+		float car_center_x = 0;				//整车的中心点x值, 四轮的中心
+		float car_center_y = 0;				//整车的中心点y值, 四轮的中心
+		float car_angle = 0;			//整车的车身旋转角,
+		float car_length = 0;			//整车的长度, 用于规避碰撞
+		float car_width = 0;			//整车的宽度, 用于规避碰撞
+		float car_height = 0;			//整车的高度, 用于规避碰撞
+		float car_wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+		float car_wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+		float car_front_theta = 0;			//整车的前轮的旋转角
+
+		bool correctness = false;			//整车的校准标记位
+
+	};
+	
+	
+	//四轮的测量信息
+	struct Car_wheel_information
+	{
+		float car_center_x = 0;				//整车的中心点x值, 四轮的中心
+		float car_center_y = 0;				//整车的中心点y值, 四轮的中心
+		float car_angle = 0;			//整车的车身旋转角,
+
+		float car_wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+		float car_wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+		float car_front_theta = 0;			//整车的前轮的旋转角
+
+		bool correctness = false;			//整车的校准标记位
+		public:
+		Car_wheel_information& operator+=(const Car_wheel_information& other)
+		{
+			this->car_center_x += other.car_center_x;
+			this->car_center_y += other.car_center_y;
+			this->car_angle += other.car_angle;
+			this->car_wheel_base += other.car_wheel_base;
+			this->car_wheel_width += other.car_wheel_width;
+			this->car_front_theta += other.car_front_theta;
+			this->correctness &= other.correctness;
+			return *this;
+		}
+		Car_wheel_information& operator-=(const Car_wheel_information& other)
+		{
+			this->car_center_x -= other.car_center_x;
+			this->car_center_y -= other.car_center_y;
+			this->car_angle -= other.car_angle;
+			this->car_wheel_base -= other.car_wheel_base;
+			this->car_wheel_width -= other.car_wheel_width;
+			this->car_front_theta -= other.car_front_theta;
+			this->correctness &= other.correctness;
+			return *this;
+		}
+		Car_wheel_information& operator/=(int scalar)
+		{
+			if(scalar==0)
+				return *this;
+			this->car_center_x /= scalar;
+			this->car_center_y /= scalar;
+			this->car_angle /= scalar;
+			this->car_wheel_base /= scalar;
+			this->car_wheel_width /= scalar;
+			this->car_front_theta /= scalar;
+			return *this;
+		}
+		// 定义评分规则, 
+		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.5
+		float calc_score()
+		{
+			float weights[] = {1.0f, 1.0f, 0.5f, 0.1f, 0.05f, 0.05f};
+			float final_score = 0.0f;
+			final_score += fabs(weights[0] * this->car_center_x);
+			final_score += fabs(weights[1] * this->car_center_y);
+			final_score += fabs(weights[2] * this->car_angle);
+			final_score += fabs(weights[3] * this->car_wheel_base);
+			final_score += fabs(weights[4] * this->car_wheel_width);
+			final_score += fabs(weights[5] * this->car_front_theta);
+
+			return final_score;
+		}
+
+		std::string to_string()
+        {
+            char buf[512]={0};
+            sprintf(buf, "%.4f %.4f %.4f %.4f %.4f %.4f\n", car_center_x, car_center_y, car_angle, car_wheel_base, car_wheel_width, car_front_theta);
+            return std::string(buf);
+        }
+	};
+
+	// 带时间戳的四轮测量信息
+	struct Car_wheel_information_stamped
+	{
+		Car_wheel_information wheel_data;
+		std::chrono::system_clock::time_point measure_time;
+				public:
+		Car_wheel_information_stamped& operator+=(const Car_wheel_information_stamped& other)
+		{
+			this->wheel_data += other.wheel_data;
+			return *this;
+		}
+		Car_wheel_information_stamped& operator-=(const Car_wheel_information_stamped& other)
+		{
+			this->wheel_data -= other.wheel_data;
+			return *this;
+		}
+		Car_wheel_information_stamped operator-(const Car_wheel_information_stamped& other)
+		{
+            Car_wheel_information_stamped t_info;
+            t_info = *this;
+            t_info.wheel_data -= other.wheel_data;
+			return t_info;
+		}
+		Car_wheel_information_stamped& operator/=(int scalar)
+		{
+			this->wheel_data /= scalar;
+			return *this;
+		}
+		// 定义评分规则, 
+		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.1
+		float calc_score()
+		{
+			return wheel_data.calc_score();
+		}
+	};
+
+
+
+	struct Car_information
+	{
+		std::string                license;              //车辆凭证号
+		float                      car_length=0;           //车长
+		float                      car_width=0;            //车宽
+		float                      car_height=0;           //车高
+		float                      car_wheel_base = 0;	    //整车的轮距; 前后轮的距离; 用于机器人或agv的抓车
+		float                      car_wheel_width = 0;	//整车的轮距; 左右轮的距离; 用于机器人或agv的抓车
+	};
+
+
+//车位状态枚举
+	enum Parkspace_status
+	{
+		PARKSPACE_STATUS_UNKNOW		= 0,
+		PARKSPACE_EMPTY     		= 1,         //空闲，可分配
+		PARKSPACE_OCCUPIED    		= 2,         //被占用，不可分配
+		PARKSPACE_RESERVED  		= 3,         //被预约，预约车辆可分配, 暂时不用
+		PARKSPACE_LOCKED 			= 4,         //临时锁定，不可分配
+		PARKSPACE_ERROR 			= 5,         //车位机械结构或硬件故障
+	};
+
+	enum Direction
+	{
+		DIRECTION_UNKNOW = 0,
+		DIRECTION_FORWARD = 1,
+		DIRECTION_BACKWARD = 2,
+	};
+
+//车位分配路线（根据中跑车的路线来定）
+	enum Parkspace_path
+	{
+		UNKNOW_PATH = 0,
+		OPTIMAL_PATH = 1,
+		LEFT_PATH = 2,
+		RIGHT_PATH = 3,
+		TEMPORARY_CACHE_PATH = 4,
+
+	};
+	//车位类型
+	enum Parkspace_type
+	{
+		UNKNOW_PARKSPACE_TYPE = 0,
+		MIN_PARKINGSPACE = 1,//小车位
+		MID_PARKINGSPACE = 2,//中车位
+		BIG_PARKINGSPACE = 3,//大车位
+	};
+
+	//汽车类型
+	enum Car_type
+	{
+		UNKNOW_CAR_TYPE = 0,
+		MIN_CAR = 1,//小车
+		MID_CAR = 2,//中车
+		BIG_CAR = 3,//大车
+	};
+
+	//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
+	struct Parkspace_information
+	{
+		int              	parkingspace_index_id=0;         //车位编号
+		Parkspace_type     	parkingspace_type=UNKNOW_PARKSPACE_TYPE;               //车位类型
+
+		int              	parkingspace_unit_id=0;            //区块编号
+		int              	parkingspace_floor_id=0;                //楼层
+		int              	parkingspace_room_id=0;                //同层编号
+		Direction    		parkingspace_direction=DIRECTION_UNKNOW;            //前后
+
+		float              	parkingspace_width=0;                //车位宽
+		float              	parkingspace_height=0;               //车位高
+		Parkspace_status   	parkingspace_status=PARKSPACE_EMPTY;     //车位当前状态
+
+		Car_information    	car_information;              //当前车位存入车辆的凭证号
+		std::string        	car_entry_time;          //入场时间
+		std::string        	car_leave_time;          //离场时间
+
+		Parkspace_path     	parkspace_path = UNKNOW_PATH;            // 车位分配路线
+		float              	path_estimate_time = 0;        //车位分配路线 time(s)
+		Parkspace_status   	parkspace_status_target=PARKSPACE_EMPTY;     //车位目标状态
+	};
+
+	static void copy_data(Car_measure_information& car_measure_information_out, const message::Locate_information& locate_information_in);
+	static void copy_data(message::Locate_information& locate_information_out, const Car_measure_information& car_measure_information_in);
+	static void copy_data(Car_information& car_information_out, const message::Car_info& car_info_in);
+	static void copy_data(message::Car_info& car_info_out, const Car_information& car_information_in);
+	static void copy_data(Parkspace_information& parkspace_information_out, const message::Parkspace_info& parkspace_info_in);
+	static void copy_data(message::Parkspace_info& parkspace_info_out, const Parkspace_information& parkspace_information_in);
+
+
+	static void transform_data(Car_information& car_information_out, const Car_measure_information& car_measure_information_in);
+	static void transform_data(Car_measure_information& car_measure_information_out, const Car_information& car_informatio_in);
+	static void transform_data(Car_wheel_information& car_wheel_information_out, const Car_measure_information& car_measure_information_in);
+	static void transform_data(Car_measure_information& car_measure_information_out, const Car_wheel_information& car_wheel_information_in);
+
+	static void scaling(Car_measure_information& car_measure_information, float rate);
+	static void scaling(Car_information& car_information, float rate);
+	static void scaling(Parkspace_information& parkspace_information, float rate);
+
+
+	static bool approximate_rate(float a, float b, float rate);
+	static bool approximate_difference(float a, float b, float difference);
+
+
+
+
+
+
+
+
+
+	//调度模块
+	//调度流程类型
+	enum Dispatch_process_type
+	{
+		DISPATCH_PROCESS_TYPE_UNKNOW 			= 0,    //未知
+		DISPATCH_PROCESS_STORE  				= 101,    //存车
+		DISPATCH_PROCESS_PICKUP					= 102,	//取车
+	};
+
+
+	//调度动作方向   0=未知，1=存车，2=取车
+	enum Dispatch_motion_direction
+	{
+		DISPATCH_MOTION_DIRECTION_UNKNOWN					= 0,
+		DISPATCH_MOTION_DIRECTION_STORE              		= 1,    //
+		DISPATCH_MOTION_DIRECTION_PICKUP					= 2,    //
+	};
+
+	//出入口方向   0=未知，1=入口，2=出口
+	enum Passageway_direction
+	{
+		PASSAGEWAY_DIRECTION_UNKNOWN             			= 0,    //
+		PASSAGEWAY_DIRECTION_INLET               			= 1,    //
+		PASSAGEWAY_DIRECTION_OUTLET							= 2,    //
+	};
+
+	//楼上车位方向   0=未知，1=朝南，2=朝北
+	enum Parkingspace_direction
+	{
+		PARKINGSPACE_DIRECTION_UNKNOWN					= 0,    //
+		PARKINGSPACE_DIRECTION_SOUTH               		= 1,    //
+		PARKINGSPACE_DIRECTION_NORTH               		= 2,    //
+	};
+
+	//防撞雷达标志位  0=未知，1=位置正常，2=位置异常
+	enum Anticollision_lidar_flag
+	{
+		ANTICOLLISION_LIDAR_UNKNOWN               	= 0,    //
+		ANTICOLLISION_LIDAR_NORMAL               	= 1,    //
+		ANTICOLLISION_LIDAR_ERROR               	= 2,    //
+	};
+};
+
+
+#endif //NNXX_TESTS_COMMON_DATA_H

tool/point2D_tool.cpp

@@ -0,0 +1,139 @@
+//
+// Created by huli on 2020/9/1.
+//
+
+#include "point2D_tool.h"
+
+//极坐标 -> 平面坐标
+bool Point2D_tool::Polar_coordinates_to_point2D(Point2D_tool::Polar_coordinates* p_polar_coordinates, Point2D_tool::Point2D* p_point2D)
+{
+	if ( p_polar_coordinates == NULL || p_point2D == NULL)
+	{
+		return false;
+	}
+	else
+	{
+		p_point2D->x = p_polar_coordinates->distance * cos(p_polar_coordinates->angle);
+		p_point2D->y = p_polar_coordinates->distance * sin(p_polar_coordinates->angle);
+		return true;
+	}
+	return true;
+}
+//平面坐标 -> 极坐标
+bool Point2D_tool::Point2D_to_polar_coordinates(Point2D_tool::Point2D* p_point2D, Point2D_tool::Polar_coordinates* p_polar_coordinates)
+{
+	if ( p_polar_coordinates == NULL || p_point2D == NULL)
+	{
+		return false;
+	}
+	else
+	{
+		p_polar_coordinates->distance = sqrt(p_point2D->x * p_point2D->x + p_point2D->y * p_point2D->y);
+		p_polar_coordinates->angle = atan(p_point2D->y / p_point2D->x);
+		return true;
+	}
+	return true;
+}
+
+//判断极坐标点是否在限制范围
+bool Point2D_tool::limit_with_polar_coordinates_box(float distance, float angle, Point2D_tool::Polar_coordinates_box* p_polar_coordinates_box)
+{
+	if ( p_polar_coordinates_box == NULL )
+	{
+		return false;
+	}
+	else
+	{
+		if ( angle >= p_polar_coordinates_box->angle_min &&
+			 angle <= p_polar_coordinates_box->angle_max &&
+			 distance >= p_polar_coordinates_box->distance_min &&
+			 distance <= p_polar_coordinates_box->distance_max )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+//判断平面坐标点是否在限制范围
+bool Point2D_tool::limit_with_point2D_box(float x, float y, Point2D_tool::Point2D_box* p_point2D_box)
+{
+	if ( p_point2D_box == NULL )
+	{
+		return false;
+	}
+	else
+	{
+		if ( x >= p_point2D_box->x_min &&
+			 x <= p_point2D_box->x_max &&
+			 y >= p_point2D_box->y_min &&
+			 y <= p_point2D_box->y_max )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+//平面坐标的转换, 可以进行旋转和平移, 不能缩放
+bool Point2D_tool::transform_with_translation_rotation(float* p_x_in, float* p_y_in, float* p_x_out, float* p_y_out,
+										 Point2D_tool::Point2D_transform* p_point2D_transform)
+{
+	if ( p_x_in == NULL || p_x_in == NULL || p_x_out == NULL || p_y_out == NULL || p_point2D_transform == NULL)
+	{
+		return false;
+	}
+	else
+	{
+		*p_x_out = *p_x_in * p_point2D_transform->m00 + *p_y_in * p_point2D_transform->m01 + p_point2D_transform->m02;
+		*p_y_out = *p_x_in * p_point2D_transform->m10 + *p_y_in * p_point2D_transform->m11 + p_point2D_transform->m12;
+		return true;
+	}
+	return true;
+}
+bool Point2D_tool::transform_with_translation_rotation(pcl::PointXYZ* p_point3D_in, pcl::PointXYZ* p_point3D_out,
+												Point2D_tool::Point2D_transform* p_point2D_transform)
+{
+	if (p_point3D_in == NULL || p_point3D_out == NULL || p_point2D_transform == NULL)
+	{
+		return false;
+	}
+	else
+	{
+		p_point3D_out->x = p_point3D_in->x * p_point2D_transform->m00 + p_point3D_in->y * p_point2D_transform->m01 +
+						   p_point2D_transform->m02;
+		p_point3D_out->y = p_point3D_in->x * p_point2D_transform->m10 + p_point3D_in->y * p_point2D_transform->m11 +
+						   p_point2D_transform->m12;
+		return true;
+	}
+	return true;
+}
+bool Point2D_tool::transform_with_translation_rotation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+												pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out,
+												Point2D_tool::Point2D_transform* p_point2D_transform)
+{
+	if ( p_cloud_in.get() == NULL || p_cloud_out.get() == NULL || p_point2D_transform == NULL)
+	{
+	    return false;
+	}
+	else
+	{
+		pcl::PointXYZ point;
+		for (int i = 0; i < p_cloud_in->size(); ++i)
+		{
+			point.x = p_cloud_in->points[i].x * p_point2D_transform->m00 + p_cloud_in->points[i].y * p_point2D_transform->m01 + p_point2D_transform->m02;
+			point.y = p_cloud_in->points[i].x * p_point2D_transform->m10 + p_cloud_in->points[i].y * p_point2D_transform->m11 + p_point2D_transform->m12;
+			p_cloud_out->push_back(point);
+		}
+		return true;
+	}
+	return true;
+}

tool/point2D_tool.h

@@ -0,0 +1,81 @@
+//
+// Created by huli on 2020/9/1.
+//
+
+#ifndef POINT2D_TOOL_H
+#define POINT2D_TOOL_H
+
+#include <pcl/point_types.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl/common/common.h>
+
+class Point2D_tool
+{
+public:
+	//极坐标
+	struct Polar_coordinates
+	{
+		float angle=0;				//弧度
+		float distance=0;				//距离
+	};
+
+	//极坐标的限定范围
+	struct Polar_coordinates_box
+	{
+		float angle_min=0;			//角度最小值
+		float angle_max=0;			//角度最大值
+		float distance_min=0;			//距离最小值
+		float distance_max=0;			//距离最大值
+	};
+
+	//平面坐标
+	struct Point2D
+	{
+		float x=0;				//x轴
+		float y=0;				//y轴
+	};
+
+	//平面坐标的限定范围
+	struct Point2D_box
+	{
+		float x_min=0;				//x轴最小值
+		float x_max=0;				//x轴最大值
+		float y_min=0;				//y轴最小值
+		float y_max=0;				//y轴最大值
+	};
+
+	//平面坐标的转换矩阵, 可以进行旋转和平移, 不能缩放
+	struct Point2D_transform
+	{
+		float m00=1;
+		float m01=0;
+		float m02=0;
+		float m10=0;
+		float m11=1;
+		float m12=0;
+	};
+
+	//极坐标 -> 平面坐标
+	static bool Polar_coordinates_to_point2D(Point2D_tool::Polar_coordinates* p_polar_coordinates, Point2D_tool::Point2D* p_point2D);
+	//平面坐标 -> 极坐标
+	static bool Point2D_to_polar_coordinates(Point2D_tool::Point2D* p_point2D, Point2D_tool::Polar_coordinates* p_polar_coordinates);
+
+
+	//判断极坐标点是否在限制范围
+	static bool limit_with_polar_coordinates_box(float distance, float angle, Point2D_tool::Polar_coordinates_box* p_polar_coordinates_box);
+	//判断平面坐标点是否在限制范围
+	static bool limit_with_point2D_box(float x, float y, Point2D_tool::Point2D_box* p_point2D_box);
+
+	//平面坐标的转换, 可以进行旋转和平移, 不能缩放
+	static bool transform_with_translation_rotation(float* p_x_in, float* p_y_in, float* p_x_out, float* p_y_out,
+													Point2D_tool::Point2D_transform* p_point2D_transform);
+	static bool transform_with_translation_rotation(pcl::PointXYZ* p_point3D_in, pcl::PointXYZ* p_point3D_out,
+													Point2D_tool::Point2D_transform* p_point2D_transform);
+	static bool transform_with_translation_rotation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+													pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out,
+													Point2D_tool::Point2D_transform* p_point2D_transform);
+};
+
+
+#endif //POINT2D_TOOL_H

tool/point3D_tool.cpp

@@ -0,0 +1,5 @@
+//
+// Created by huli on 2021/3/24.
+//
+
+#include "point3D_tool.h"

tool/point3D_tool.h

@@ -0,0 +1,33 @@
+//
+// Created by huli on 2021/3/24.
+//
+
+#ifndef NNXX_TESTS_POINT3D_TOOL_H
+#define NNXX_TESTS_POINT3D_TOOL_H
+
+
+class Point3D_tool
+{
+public:
+	//坐标
+	struct Point3D
+	{
+		float x=0;				//x轴
+		float y=0;				//y轴
+		float z=0;				//z轴
+	};
+
+	//平面坐标的限定范围
+	struct Point3D_box
+	{
+		float x_min=0;				//x轴最小值
+		float x_max=0;				//x轴最大值
+		float y_min=0;				//y轴最小值
+		float y_max=0;				//y轴最大值
+		float z_min=0;				//z轴最小值
+		float z_max=0;				//z轴最大值
+	};
+};
+
+
+#endif //NNXX_TESTS_POINT3D_TOOL_H

tool/time_tool.cpp

@@ -0,0 +1,133 @@
+//
+// Created by wk on 2020/9/25.
+//
+
+#include "time_tool.h"
+Time_tool::Time_tool()
+{
+}
+Time_tool::~Time_tool()
+{
+	Time_tool_uninit();
+}
+void Time_tool::set_points_digits(int num)
+{
+	std::cout.precision(num);
+	std::cout.setf(std::ios::fixed);
+}
+std::chrono::system_clock::time_point Time_tool::get_system_point()
+{
+	return std::chrono::system_clock::now();
+}
+tm Time_tool::get_current_time_struct()
+{
+	auto now = std::chrono::system_clock::now();
+	time_t tt = std::chrono::system_clock::to_time_t(now);
+	tm time_tm=*localtime(&tt);
+	return time_tm;
+}
+std::string Time_tool::get_current_time_seconds()
+{
+	auto time_tm = get_current_time_struct();
+	char strTime[100] = "";
+	sprintf(strTime, "%d-%02d-%02d %02d:%02d:%02d", time_tm.tm_year + 1900,
+			time_tm.tm_mon + 1, time_tm.tm_mday, time_tm.tm_hour,
+			time_tm.tm_min, time_tm.tm_sec);
+	std::string str=strTime;
+	return str;
+}
+std::string Time_tool::get_current_time_millisecond()
+{
+
+	auto now = std::chrono::system_clock::now();
+	//通过不同精度获取相差的毫秒数
+	uint64_t dis_millseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count()
+							   - std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count() * 1000;
+	std::string strTime=get_current_time_seconds()+" "+std::to_string((int)dis_millseconds);
+	return strTime;
+}
+std::string Time_tool::get_current_time_microsecond()
+{
+
+	auto now = std::chrono::system_clock::now();
+	//通过不同精度获取相差的微秒
+	uint64_t dis_microseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count()
+							   - std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() * 1000;
+	std::string strTime=get_current_time_millisecond()+" "+std::to_string((int)dis_microseconds);
+	return strTime;
+}
+void Time_tool::time_start(int key)
+{
+	timetool_map[key].t_time_start=get_system_point();//保存开始的时间 //单位为微秒
+}
+
+void Time_tool::time_end(int key)
+{
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		timetool_map[key].t_time_end = get_system_point();//保存结束的时间
+		timetool_map[key].t_time_difference = timetool_map[key].t_time_end - timetool_map[key].t_time_start;//保存时差
+	}
+	else
+	{
+	    std::cout << "计时器：" << key<<"还未开始"<<std::endl;
+	}
+
+}
+void Time_tool::cout_time_seconds(int key)
+{
+
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		double dieoutTime=(double)timetool_map[key].t_time_difference.count()/1000000000;
+		std::cout << "计时器:"<<key<<" 计时的时间为：" <<dieoutTime<<" 秒" << std::endl;
+	}
+	else
+	{
+		std::cout<<"没有此计时器:"<<key<<std::endl;
+	}
+}
+void Time_tool::cout_time_millisecond(int key)
+{
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		double dieoutTime=(double)timetool_map[key].t_time_difference.count()/1000000;
+		std::cout << "计时器:"<<key<<" 计时的时间为：" <<dieoutTime<<" 毫秒" << std::endl;
+	}
+	else
+	{
+		std::cout<<"没有此计时器:"<<key<<std::endl;
+	}
+}
+void Time_tool::cout_time_microsecond(int key)
+{
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		double dieoutTime=(double)timetool_map[key].t_time_difference.count()/1000;
+
+		std::cout << "计时器:"<<key<<" 计时的时间为：" <<dieoutTime<<" 微秒" << std::endl;
+	}
+	else
+	{
+		std::cout<<"没有此计时器:"<<key<<std::endl;
+	}
+}
+void Time_tool::cout_time_nanosecond(int key)
+{
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		std::cout << "计时器:"<<key<<" 计时的时间为：" <<timetool_map[key].t_time_difference.count()<<" 纳秒" << std::endl;
+	}
+	else
+	{
+		std::cout<<"没有此计时器:"<<key<<std::endl;
+	}
+}
+void Time_tool::clear_timer()
+{
+	Time_tool_uninit();
+}
+void Time_tool::Time_tool_uninit()
+{
+	timetool_map.clear();
+}

tool/time_tool.h

@@ -0,0 +1,81 @@
+//
+// Created by wk on 2020/9/25.
+//
+
+#ifndef PKGNAME_TIME_TOOL_H
+#define PKGNAME_TIME_TOOL_H
+
+#include <thread>
+#include <map>
+#include <iostream>
+#include<time.h>
+#include <queue>
+#include "./singleton.h"
+
+class Time_tool:public Singleton<Time_tool>
+{
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+   friend class Singleton<Time_tool>;
+struct time_data
+{
+	std::chrono::system_clock::time_point		t_time_start;//计时开始
+	std::chrono::system_clock::time_point 		t_time_end;//计时结束
+	std::chrono::system_clock::duration			t_time_difference;//时差
+};
+private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
+   Time_tool();
+public:
+    //必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Time_tool(const Time_tool& other) = delete;
+    Time_tool& operator =(const Time_tool& other) = delete;
+    ~Time_tool();
+public://API functions
+	void Time_tool_uninit();
+
+    //获取当前系统时间点
+	std::chrono::system_clock::time_point get_system_point();
+
+	//设置输出格式  保留小数点后几位  不设置默认为系统输出
+	void set_points_digits(int);
+
+	//获取当前时间 精确到秒
+	 std::string get_current_time_seconds();
+
+	//获取当前时间 精确到毫秒
+	std::string get_current_time_millisecond();
+
+	//获取当前时间 精确到微妙
+	std::string get_current_time_microsecond();
+
+	//获取当前时间结构体
+	tm get_current_time_struct();
+
+	//计时开始
+	void time_start(int key=1);
+
+	//指定计时器计时结束
+	void time_end(int key=1);
+
+	//打印计时时差 精确到秒
+	void cout_time_seconds(int key=1);
+
+	//打印计时时差 精确到毫秒
+	void cout_time_millisecond(int key=1);
+
+	//打印计时时差 精确到微秒
+	void cout_time_microsecond(int key=1);
+
+	//打印计时时差 精确到纳秒
+	void cout_time_nanosecond(int key=1);
+
+	//清除计时器
+	void clear_timer();
+public://get or set member variable
+protected://member variable
+
+    
+private:
+	std::map<int,time_data> 			timetool_map;
+};
+#endif //PKGNAME_TIME_TOOL_H