restructure. pcl reconnect. rescan. change wheelbase calculation.

src/.vscode/c_cpp_properties.json

@@ -10,7 +10,8 @@
                 "/opt/ros/kinetic/include/**",
                 "/home/youchen/Documents/measure/MainStructure/elecfence_ws/src/wj_716_lidar/include/**",
                 "/usr/include/**",
-                "/usr/local/include/snap7"
+                "/usr/local/include/snap7",
+                "${workspaceFolder}/findwheel/src/include"
             ],
             "name": "ROS",
             "intelliSenseMode": "gcc-x64",

src/findwheel/CMakeLists.txt

@@ -21,8 +21,6 @@ find_package(catkin REQUIRED COMPONENTS
 include_directories("/usr/include/eigen3")
 
 find_package(PCL REQUIRED)
-include_directories(include ${PCL_INCLUDE_DIRS})
-link_directories(${PCL_LIBRARY_DIRS})
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -127,12 +125,13 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
+include
   ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
 /usr/local/include/snap7
 /usr/local/include
 ${OpenCV_INLCUDE_DIRS}
-./
+./include
 )
 
 ## Declare a C++ library
@@ -150,6 +149,7 @@ ${OpenCV_INLCUDE_DIRS}
 ## The recommended prefix ensures that target names across packages don't collide
 
 link_directories(/usr/local/lib)
+link_directories(${PCL_LIBRARY_DIRS})
 
 add_executable(${PROJECT_NAME}_node 
     src/find_wheel_node.cpp
@@ -160,13 +160,9 @@ add_executable(${PROJECT_NAME}_node
     src/EleFence.pb.cc
     src/StdCondition.cpp
     src/region_worker.cpp
-    src/TaskQueue/BaseTask.cpp 
-    src/TaskQueue/ThreadTaskQueue.cpp 
-    src/TaskQueue/TQFactory.cpp 
-    src/wheelCalcTask.cpp
     src/globalmsg.pb.cc
     src/s7_plc.cpp
-        src/PlcData.cpp src/PlcData.h src/define.h)
+        src/PlcData.cpp)
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} snap7 libnnxx.a libnanomsg.so ${OpenCV_LIBS})
 
 ## Rename C++ executable without prefix

src/findwheel/src/EleFence.pb.cc

@@ -1,7 +1,7 @@
 // Generated by the protocol buffer compiler.  DO NOT EDIT!
 // source: EleFence.proto
 
-#include "EleFence.pb.h"
+#include "include/EleFence.pb.h"
 
 #include <algorithm>
 

src/findwheel/src/FenceController.cpp

@@ -1,7 +1,7 @@
 //
 // Created by zx on 2019/12/6.
 //
-#include "FenceController.h"
+#include "include/FenceController.h"
 
 bool FenceController::ReadProtoParam(std::string path)
 {
@@ -142,6 +142,7 @@ void FenceController::wheelMsgRecvThread(FenceController* fc)
 
 void FenceController::wheelMsgHandlingThread(FenceController* fc){
     if(fc==0) return;
+    int count = fc->m_retry_count >0 ? fc->m_retry_count:1;
     while(!fc->m_cond_wheel_exit.WaitFor(1)){
 
         MsgTask task;
@@ -161,37 +162,45 @@ void FenceController::wheelMsgHandlingThread(FenceController* fc){
             {
                 int terminal_id = atoi(task.cmd.substr(8, task.cmd.length() - 8).c_str());
                 std::cout << "terminal id: " << terminal_id << std::endl;
-                for (int i = 0; i < fc->p_region_workers.size(); ++i)
+                while (count-- >= 0 && !result.correctness())
                 {
-                    if (terminal_id >= 0 && fc->p_region_workers[i]->get_id() == terminal_id)
+                    for (int i = 0; i < fc->p_region_workers.size(); ++i)
                     {
-                        std::cout << "--------callback find terminal------" << terminal_id << std::endl;
-                        double x = 0, y = 0, c = 0, wheelbase = 0, width = 0;
-
-
-                        std::cout << "--------callback get time------" << std::endl;
-                        fc->m_cloud_mutex.lock();
-                        pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-                        for (int i = 0; i < fc->p_lidars.size(); ++i) {
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-                            if (fc->p_lidars[i]->GetCloud(cloud)) {
-                                total_cloud->operator+=(*cloud);
+                        if (terminal_id >= 0 && fc->p_region_workers[i]->get_id() == terminal_id)
+                        {
+                            // std::cout << "--------callback find terminal------" << terminal_id << std::endl;
+                            double x = 0, y = 0, c = 0, wheelbase = 0, width = 0;
+                            // std::cout << "--------callback get time------" << std::endl;
+                            fc->m_cloud_mutex.lock();
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+                            for (int i = 0; i < fc->p_lidars.size(); ++i)
+                            {
+                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+                                if (fc->p_lidars[i]->GetCloud(cloud))
+                                {
+                                    total_cloud->operator+=(*cloud);
+                                }
                             }
+                            fc->m_cloud_mutex.unlock();
+                            // std::cout << "--------callback get cloud------" << std::endl;
+                            if (total_cloud->size() > 0 &&
+                                fc->p_region_workers[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width))
+                            {
+                                result.set_correctness(true);
+                            }
+                            else
+                            {
+                                result.set_correctness(false);
+                            }
+                            // std::cout << "--------callback get result------" << std::endl;
+                            result.set_x(x * 1000.0);
+                            result.set_y(y * 1000.0);
+                            result.set_c(c);
+                            result.set_wheel_base(wheelbase * 1000.0);
+                            result.set_width(width * 1000.0);
+                            if(result.correctness())
+                                break;
                         }
-                        fc->m_cloud_mutex.unlock();
-                        std::cout << "--------callback get cloud------" << std::endl;
-                        if (total_cloud->size() > 0 &&
-                            fc->p_region_workers[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width)) {
-                            result.set_correctness(true);
-                        } else {
-                            result.set_correctness(false);
-                        }
-                        std::cout << "--------callback get result------" << std::endl;
-                        result.set_x(x*1000.0);
-                        result.set_y(y*1000.0);
-                        result.set_c(c);
-                        result.set_wheel_base(wheelbase*1000.0);
-                        result.set_width(width*1000.0);
                     }
                 }
             }
@@ -210,15 +219,4 @@ void FenceController::wheelMsgHandlingThread(FenceController* fc){
         // std::cout<<"++++++++++++++++"<<std::endl;
 
     }
-}
-
-// termID start from 0
-void FenceController::sendWheelResultCallback(int termID, nnxx::message_control* ctl, void *p){
-    std::cout<<"--------callback start------"<<std::endl;
-    FenceController * fc = ((FenceController *)p);
-    fc->m_mutex_wheel_handling.lock();
-    std::cout<<"--------callback start2------"<<std::endl;
-
-    std::cout<<"--------callback end------"<<std::endl;
-    fc->m_mutex_wheel_handling.unlock();
 }

src/findwheel/src/Lidar.cpp

@@ -2,8 +2,8 @@
 // Created by zx on 2019/12/5.
 //
 
-#include "Lidar.h"
-#include "LogFiles.h"
+#include "include/Lidar.h"
+#include "include/LogFiles.h"
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr  laserScan2Cloud(const sensor_msgs::LaserScan& msg,
                                                      Eigen::MatrixXf transform2d, float dist_limit)

src/findwheel/src/LogFiles.cpp

@@ -1,5 +1,5 @@
 
-#include "LogFiles.h"
+#include "include/LogFiles.h"
 #include <string.h>
 
 const string CLogFiles::m_strFileNameLoglaserBinary1 = "laser1.data";

src/findwheel/src/PlcData.cpp

@@ -2,7 +2,7 @@
 // Created by zx on 2019/12/9.
 //
 
-#include "PlcData.h"
+#include "include/PlcData.h"
 #include <string.h>
 
 PlcData* PlcData::g_instance=0;
@@ -37,8 +37,8 @@ void PlcData::UpdateData(stateCode code,int id)
     ////判断越界
     if(offset>=0 && offset+length<=MAX_REGIONS)
         memcpy(m_data+offset,data,length*sizeof(short));
-    free(data);
     m_lock.unlock();
+    free(data);
 }
 
 void PlcData::plcThread(PlcData* p){
@@ -52,15 +52,23 @@ void PlcData::plcThread(PlcData* p){
         }
         std::cout<<"]"<<std::endl;*/
         // 写plc
-        p->p_plc->WriteShorts(ELE_FENCE_DB_NUM, ELE_FENCE_START_ADDR, MAX_REGIONS, p->m_data);
-
+        if(p->p_plc->getConnection()){
+            p->p_plc->WriteShorts(ELE_FENCE_DB_NUM, ELE_FENCE_START_ADDR, MAX_REGIONS, p->m_data);
+        }else{
+            p->p_plc->disconnect();
+            if(p->m_ip_str != ""){
+                p->p_plc->connect(p->m_ip_str);
+            }
+        }
         usleep(p->m_send_interval_milli * 1000);
     }
 }
 
 PlcData::PlcData(std::string ip):
-p_send_thread(0)
+p_send_thread(0),
+m_ip_str("")
 {
+    m_ip_str = ip;
     memset(m_data, 0, MAX_REGIONS* sizeof(short));
     p_plc = new S7PLC();
     if(ip!=""){

src/findwheel/src/StdCondition.cpp

@@ -1,4 +1,4 @@
-#include "StdCondition.h"
+#include "include/StdCondition.h"
 
 StdCondition::StdCondition():m_value(false)
 {

src/findwheel/src/TaskQueue/BaseTask.cpp

@@ -1,33 +0,0 @@
-//
-//  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();
-    }
-}

src/findwheel/src/TaskQueue/BaseTask.h

@@ -1,29 +0,0 @@
-//
-//  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();
-        void Cancel();
-        bool IsCancelled() const;
-        
-        virtual void Main() = 0;
-        virtual TaskCategory GetCategory() const {return NoCategory;}
-    };
-}
-#endif /* defined(__LibDriveRating_CXX__BaseTaskQueue__) */

src/findwheel/src/TaskQueue/TQFactory.cpp

@@ -1,23 +0,0 @@
-//
-//  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;
-    }
-    
-    
-}

src/findwheel/src/TaskQueue/TQFactory.h

@@ -1,24 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/TQInterface.h

@@ -1,58 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/TaskPool.h

@@ -1,89 +0,0 @@
-#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

src/findwheel/src/TaskQueue/ThreadTaskQueue.cpp

@@ -1,286 +0,0 @@
-//
-//  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.
-    }
-    
-}

src/findwheel/src/TaskQueue/ThreadTaskQueue.h

@@ -1,89 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/threadpp/impl/pthread_lock.h

@@ -1,35 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/threadpp/impl/pthread_lock.hpp

@@ -1,73 +0,0 @@
-//
-//  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

src/findwheel/src/TaskQueue/threadpp/impl/pthread_thread.h

@@ -1,45 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/threadpp/impl/pthread_thread.hpp

@@ -1,110 +0,0 @@
-//
-//  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

src/findwheel/src/TaskQueue/threadpp/impl/std_lock.h

@@ -1,33 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/threadpp/impl/std_lock.hpp

@@ -1,52 +0,0 @@
-//
-//  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

src/findwheel/src/TaskQueue/threadpp/impl/std_thread.h

@@ -1,40 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/threadpp/impl/std_thread.hpp

@@ -1,64 +0,0 @@
-//
-//  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

src/findwheel/src/TaskQueue/threadpp/impl/win_lock.h

@@ -1,37 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/threadpp/impl/win_lock.hpp

@@ -1,80 +0,0 @@
-//
-//  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

src/findwheel/src/TaskQueue/threadpp/impl/win_thread.h

@@ -1,53 +0,0 @@
-//
-//  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__) */

src/findwheel/src/TaskQueue/threadpp/impl/win_thread.hpp

@@ -1,81 +0,0 @@
-//
-//  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

src/findwheel/src/TaskQueue/threadpp/recursive_lock.h

@@ -1,101 +0,0 @@
-//
-//  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

src/findwheel/src/TaskQueue/threadpp/threadpp.h

@@ -1,40 +0,0 @@
-//
-//  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

src/findwheel/src/TaskQueue/threadpp/threadpp_assert.h

@@ -1,33 +0,0 @@
-//
-//  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

src/findwheel/src/find_wheel_node.cpp

@@ -1,4 +1,4 @@
-#include "FenceController.h"
+#include "include/FenceController.h"
 
 
 ros::Publisher g_cluster_publisher[4];
@@ -14,7 +14,7 @@ int main(int argc, char** argv)
 //    }
 
     ros::init(argc, argv, "find_wheel");
-    //FenceController fc("/home/zx/zzw/catkin_ws/src/FindWheel/scripts/setting.prototxt");
+    // FenceController fc("/home/youchen/Documents/measure/MainStructure/elecfence_ws/src/findwheel/scripts/setting.prototxt");
     FenceController fc("/home/zx/zzw/catkin_ws/src/findwheel/scripts/setting.prototxt");
     if(fc.m_initialized) {
 //

src/findwheel/src/globalmsg.pb.cc

@@ -1,7 +1,7 @@
 // Generated by the protocol buffer compiler.  DO NOT EDIT!
 // source: globalmsg.proto
 
-#include "globalmsg.pb.h"
+#include "include/globalmsg.pb.h"
 
 #include <algorithm>
 

src/findwheel/src/FenceController.h

@@ -20,7 +20,6 @@ using google::protobuf::io::CodedOutputStream;
 using google::protobuf::Message;
 
 #include <ros/ros.h>
-
 #include "EleFence.pb.h"
 #include "globalmsg.pb.h"
 
@@ -42,9 +41,6 @@ using google::protobuf::Message;
 #include <cstring>
 #include <nnxx/message>
 
-#include "TaskQueue/TQFactory.h"
-#include "wheelCalcTask.h"
-
 #include "threadSafeQueue.h"
 
 #ifndef FENCECONTROLLER_H
@@ -69,8 +65,6 @@ public:
     // 中心轮距指令接收线程
     static void wheelMsgRecvThread(FenceController* fc);
     static void wheelMsgHandlingThread(FenceController* fc);
-    // 获取轮距回调
-    static void sendWheelResultCallback(int termID, nnxx::message_control* ctl, void *fc);
 
 private:
     bool ReadProtoParam(std::string path);
@@ -83,6 +77,8 @@ public:
     nnxx::socket                            m_wheel_soc{nnxx::SP_RAW, nnxx::REP};
     // wheel handling lock
     std::mutex                              m_mutex_wheel_handling;
+    // retry count
+    const int                               m_retry_count = 3;
 private:
     // 全局系统配置
     EleFence::globalParam                   m_global_param;

src/findwheel/src/PlcData.h

@@ -30,14 +30,15 @@ private:
     std::thread*                                    p_send_thread;
     S7PLC*                                          p_plc;
     const int                                       m_send_interval_milli = 200;
-
+    
 
     PlcData(std::string ip);
 
 protected:
     std::mutex m_lock;
     short m_data[MAX_REGIONS];
-
+public:
+    std::string                                     m_ip_str;
 };
 
 

src/findwheel/src/region_detect.h

@@ -54,15 +54,8 @@ public:
 
 private:
     bool preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
-    bool clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
-    bool findWheelbase();
-
+    bool clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f>& corner_points);
     bool isRect(std::vector<cv::Point2f>& points);
-private:
-    std::mutex              m_seg_mutex;
-    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_cloud_segs;
-    // 剪切后点云
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cut_cloud;
 
 public:
     EleFence::Region m_region_param;

src/findwheel/src/s7_plc.h

@@ -15,6 +15,7 @@ public:
     ~S7PLC();
 
     bool connect(std::string ip);
+    bool getConnection();
     bool ReadShorts(int DBNumber,int start,int size,short* pdata);
     bool WriteShorts(int DBNumber,int start,int size,short* pdata);
     void disconnect();

src/findwheel/src/region_detect.cpp

@@ -1,41 +1,39 @@
 //
 // Created by zx on 2019/12/6.
 //
-#include "region_detect.h"
+#include "include/region_detect.h"
 
 Region_detector::Region_detector(int id, EleFence::Region region):m_region_id(-1)
 {
     m_region_param.CopyFrom(region);
     m_region_id = id;
-    for (int i = 0; i < 4; ++i) {
-        m_cloud_segs.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
-    }
-    cut_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
 }
 
 Region_detector::~Region_detector()
 {
-
 }
 
-bool Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out){
-    if(!m_region_param.has_maxx() || !m_region_param.has_maxy() || !m_region_param.has_minx() || !m_region_param.has_miny())
+bool Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out)
+{
+    if (!m_region_param.has_maxx() || !m_region_param.has_maxy() || !m_region_param.has_minx() || !m_region_param.has_miny())
+        return false;
+    if (cloud_in->size() <= 0)
         return false;
-//    if(cloud_in->size() <= 0)
-//        return false;
     cloud_out->clear();
     pcl::copyPointCloud(*cloud_in, *cloud_out);
-    if(cloud_out->size() <= 0)
+    if (cloud_out->size() <= 0)
         return false;
     // 直通滤波, 筛选xy
     // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PassThrough<pcl::PointXYZ> pass;
     pass.setInputCloud(cloud_out);
-    pass.setFilterFieldName("x");//设置想在哪个坐标轴上操作
-    pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx());//将x轴的0到1范围内
-    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
+    pass.setFilterFieldName("x");                                       //设置想在哪个坐标轴上操作
+    pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx()); //将x轴的0到1范围内
+    pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
     pass.filter(*cloud_out);//输出到结果指针
 
+    if (cloud_out->size() <= 0)
+        return false;
     pass.setInputCloud(cloud_out);
     pass.setFilterFieldName("y");//设置想在哪个坐标轴上操作
     pass.setFilterLimits(m_region_param.miny(), m_region_param.maxy());//将x轴的0到1范围内
@@ -58,14 +56,6 @@ bool Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, p
         return true;
 }
 
-bool Region_detector::findWheelbase()
-{
-    if (cut_cloud->size() <= 0)
-        return false;
-    
-    return true;
-}
-
 double distance(cv::Point2f p1, cv::Point2f p2)
 {
     return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
@@ -177,7 +167,7 @@ bool Region_detector::isRect(std::vector<cv::Point2f>& points)
 
 }
 
-bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {
+bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f>& corner_points) {
     if (cloud_in->size() == 0) return false;
     /////聚类
     pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
@@ -215,7 +205,8 @@ bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {
 
     std::cout << " cluster class:" << clusters.size() << std::endl;
 
-    std::vector<cv::Point2f> cvPoints;
+    // std::vector<cv::Point2f> cvPoints;
+    corner_points.clear();
     for(int i=0;i<clusters.size();++i)
     {
         if(seg_clouds[i]->size()==0)
@@ -229,48 +220,43 @@ bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {
 
         float center_x=sumX/float(seg_clouds[i]->size());
         float center_y=sumY/float(seg_clouds[i]->size());
-        cvPoints.push_back(cv::Point2f(center_x,center_y));
+        corner_points.push_back(cv::Point2f(center_x,center_y));
     }
-    return isRect(cvPoints);
-
+    return isRect(corner_points);
 }
 
 bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
 {
     return false;
     bool result = false;
-    preprocess(cloud_in, cut_cloud);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    preprocess(cloud_in, cloud_filtered);
 
-    if (cut_cloud->size() <= 0)
+    if (cloud_filtered->size() <= 0)
         return false;
-    m_seg_mutex.lock();
-    result = clustering(cut_cloud);
-    m_seg_mutex.unlock();
+    std::vector<cv::Point2f> corner_points;
+    result = clustering(cloud_filtered, corner_points);
     return result;
 }
 
 bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, double &x, double &y, double &c, double &wheelbase, double &width)
 {
     bool result = false;
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filterd(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
-    preprocess(cloud_in, cloud_filterd);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    preprocess(cloud_in, cloud_filtered);
     // std::cout<<"--------detector after preprocess------"<<std::endl;
 
-    if (cloud_filterd->size() <= 0)
+    if (cloud_filtered->size() <= 0)
         return false;
-
-    m_seg_mutex.lock();
-    result = clustering(cloud_filterd);
-
-
-
+    std::vector<cv::Point2f> corner_points;
+    result = clustering(cloud_filtered, corner_points);
     if(result)
     {
         // convert all points after preprocessing into 2d
         std::vector<cv::Point2f> all_points;
         // car center
-        for (int j = 0; j < cloud_filterd->size(); ++j) {
-            all_points.push_back(cv::Point2f(cloud_filterd->points[j].x, cloud_filterd->points[j].y));
+        for (int j = 0; j < cloud_filtered->size(); ++j) {
+            all_points.push_back(cv::Point2f(cloud_filtered->points[j].x, cloud_filtered->points[j].y));
         }
          std::cout<<"--------detector find center------"<<std::endl;
         cv::RotatedRect wheel_box = cv::minAreaRect(all_points);
@@ -294,11 +280,19 @@ bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, doubl
         }
         float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
         c = angle_x;
+        // calculate wheelbase according to corner points
+        double dist0=0, dist1=0, dist2=0;
         wheelbase = std::max(wheel_box.size.width, wheel_box.size.height);
+        cv::RotatedRect wheel_center_box = cv::minAreaRect(corner_points);
+        cv::Point2f center_vertice[4];
+        wheel_box.points(center_vertice);
+        len1 = pow(center_vertice[0].x - center_vertice[1].x, 2.0) + pow(center_vertice[0].y - center_vertice[1].y, 2.0);
+        len2 = pow(center_vertice[1].x - center_vertice[2].x, 2.0) + pow(center_vertice[1].y - center_vertice[2].y, 2.0);
+        if(len1 > 0 && len2 > 0){
+            wheelbase = std::max(len1, len2);
+        }
         width = std::min(wheel_box.size.width, wheel_box.size.height);
         std::cout<<"--------detector find all------"<<std::endl;
     }
-
-    m_seg_mutex.unlock();
     return result;
 }

src/findwheel/src/region_worker.cpp

@@ -1,8 +1,8 @@
 //
 // Created by zx on 2019/12/9.
 //
-#include "region_worker.h"
-#include "PlcData.h"
+#include "include/region_worker.h"
+#include "include/PlcData.h"
 
 Region_worker::Region_worker(int id, EleFence::Region region){
     m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

src/findwheel/src/s7_plc.cpp

@@ -1,4 +1,4 @@
-#include "s7_plc.h"
+#include "include/s7_plc.h"
 
 S7PLC::S7PLC():bConnected_(false)
 {
@@ -9,6 +9,9 @@ S7PLC::~S7PLC()
     disconnect();
 }
 
+bool S7PLC::getConnection(){
+    return bConnected_;
+}
 
 bool S7PLC::connect(std::string ip)    
 {

src/findwheel/src/wheelCalcTask.cpp

@@ -1,31 +0,0 @@
-#include "wheelCalcTask.h"
-
-WheelCalcTask::WheelCalcTask(std::string termStr, nnxx::message_control *ctl, msgCallback func, void* p){
-    // std::cout<<"task contructor start"<<std::endl;
-    m_msg_callback_func = func;
-    m_base_string = termStr;
-    m_wind = p;
-    // std::cout<<"task contructor 2"<<std::endl;
-    m_ctl=ctl;
-    // std::cout<<"task contructor end"<<std::endl;
-}
-
-WheelCalcTask::~WheelCalcTask(){
-    if(m_msg_callback_func)
-        m_msg_callback_func=NULL;
-}
-
-void WheelCalcTask::Main(){
-    if(m_msg_callback_func){
-        // std::cout<<"recv: "<<m_base_string<<std::endl;
-        if(m_base_string.size()>8 && m_base_string.substr(0, 8) == "Terminal"){
-            int termID = atoi(m_base_string.substr(8, m_base_string.length()-8).c_str());
-            std::cout<<"terminal id: "<<termID<<std::endl;
-            m_msg_callback_func(termID, m_ctl, m_wind);
-        }
-    }
-}
-
-void WheelCalcTask::Cancel(){
-
-}

src/findwheel/src/wheelCalcTask.h

@@ -1,35 +0,0 @@
-#ifndef WHEEL_CALC_TASK_H
-#define WHEEL_CALC_TASK_H
-
-#include <string.h>
-#include <string>
-#include <iostream>
-#include <mutex>
-#include <thread>
-#include "TaskQueue/BaseTask.h"
-#include "globalmsg.pb.h"
-
-#include <nnxx/message.h>
-#include <nnxx/message_control.h>
-#include <nnxx/socket.h>
-#include <nnxx/reqrep.h>
-//#include <nnxx/unittest.h>
-#include <nnxx/timeout.h>
-#include <nnxx/error.h>
-
-typedef void(*msgCallback)(int termID,nnxx::message_control *ctl, void *p);
-
-class WheelCalcTask : public tq::BaseTask{
-public:
-    WheelCalcTask(std::string termStr, nnxx::message_control *ctl, msgCallback func, void* p);
-    ~WheelCalcTask();
-    virtual void   Main();
-    virtual void   Cancel();
-private:
-    std::string     m_base_string;
-    msgCallback     m_msg_callback_func;
-    void*           m_wind;
-    nnxx::message_control* m_ctl;
-};
-
-#endif // WHEEL_CALC_TASK_H