elecFence restructure and rewrite. create a thread deal with calculation tasks in a thread safe queue, rather than using a thread queue .

src/findwheel/src/FenceController.cpp

@@ -27,12 +27,12 @@ m_initialized(0)
 
     // init wheel handling thread and queue
     m_cond_wheel_exit.Notify(false);
-    m_wheel_calc_queue = tq::TQFactory::CreateDefaultQueue();
-    m_wheel_calc_queue->Start(3);
+
     if(m_wheel_soc){
         m_wheel_soc.bind(CONNECTSTRING);
     }
-    m_wheel_recv_thread = new std::thread(wheelMsgHandlingThread, this);
+    m_wheel_recv_thread = new std::thread(wheelMsgRecvThread, this);
+    m_wheel_send_thread=new std::thread(wheelMsgHandlingThread,this);
 
     // init lidar instances
     for (int i = 0; i < m_global_param.lidar_params_size(); ++i) {
@@ -70,12 +70,12 @@ FenceController::~FenceController()
         m_wheel_recv_thread = 0;
     }
     std::cout<<"exit 01"<<std::endl;
-    if (m_wheel_calc_queue)
-    {
-        m_wheel_calc_queue->CancelAll();
-        m_wheel_calc_queue->Stop();
-        delete m_wheel_calc_queue;
-        m_wheel_calc_queue = 0;
+    if(m_wheel_send_thread){
+        if(m_wheel_send_thread->joinable()){
+            m_wheel_send_thread->join();
+        }
+        delete m_wheel_send_thread;
+        m_wheel_send_thread = 0;
     }
     std::cout<<"exit 02"<<std::endl;
     // delete lidar instances
@@ -117,85 +117,108 @@ void FenceController::cloudMergeUpdate(FenceController* fc)
     }
 }
 
-void FenceController::wheelMsgHandlingThread(FenceController* fc){
+void FenceController::wheelMsgRecvThread(FenceController* fc)
+{
     if(fc==0) return;
     while(!fc->m_cond_wheel_exit.WaitFor(1)){
-        fc->m_mutex_wheel_handling.lock();
         if(fc->m_wheel_soc){
-            nnxx::message_control* c1 = new nnxx::message_control();
+            nnxx::message_control* c1=new nnxx::message_control();
+            fc->m_mutex_wheel_handling.lock();
             auto m1 = fc->m_wheel_soc.recv(1, *c1);
-            if(!m1.empty() && fc->m_wheel_calc_queue){
-                std::cout<<"-------not empty-------"<<std::endl;
-                tq::BaseTask* task = new WheelCalcTask(nnxx::to_string(m1), c1, fc->sendWheelResultCallback, (void *)fc);
-                fc->m_wheel_calc_queue->AddTask(task);
-                // std::cout<<"-------task added-------"<<std::endl;
-            } 
+            fc->m_mutex_wheel_handling.unlock();
+            if(!m1.empty() ){
+
+                MsgTask task;
+                task.cmd=nnxx::to_string(m1);
+                task.sock_controller=c1;
+                fc->m_msg_queue.push(task);
+
+                std::cout<<"-------recv msg -------"<<task.cmd<<std::endl;
+            }
         }
         // std::cout<<"++++++++++++++++"<<std::endl;
-        fc->m_mutex_wheel_handling.unlock();
     }
 }
 
-// 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;
-    for (int i = 0; i < fc->p_region_workers.size(); ++i) {
-        if(termID>=0 && fc->p_region_workers[i]->get_id() == termID){
-            std::cout<<"--------callback find terminal------"<<std::endl;
-            double x=0,y=0,c=0,wheelbase=0, width=0;
-            globalmsg::resultInfo* result = new globalmsg::resultInfo();
+void FenceController::wheelMsgHandlingThread(FenceController* fc){
+    if(fc==0) return;
+    while(!fc->m_cond_wheel_exit.WaitFor(1)){
+
+        MsgTask task;
+        if(fc->m_msg_queue.try_pop(task))
+        {
+            globalmsg::resultInfo result;
             time_t tt = time(0);
             struct tm *tc = localtime(&tt);
             char buf[255] = {0};
             memset(buf, 0, 255);
             sprintf(buf, "%d-%02d-%02d %02d:%02d:%02d ", tc->tm_year + 1900,
                     tc->tm_mon + 1, tc->tm_mday, tc->tm_hour, tc->tm_min, tc->tm_sec);
-            result->set_time(buf);
-            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);
-            result->set_y(y);
-            result->set_c(c);
-            result->set_wheel_base(wheelbase);
-            result->set_width(width);
-            std::cout<<"--------callback set result------"<<std::endl;
-
-            // std::cout<<"--------callback serializing------"<<std::endl;
-            std::string result_str = result->SerializeAsString();
-            std::cout<<"--------callback serialized------"<<std::endl;
-            nnxx::message_ostream s;
-            s << result_str;
-            std::cout<<"--------callback handle message------"<<std::endl;
-            nnxx::message m = s.move_msg();
-            
-            if (fc->m_wheel_soc)
+            result.set_time(buf);
+            result.set_correctness(false);
+
+            if(task.cmd.size()>8 && task.cmd.substr(0, 8) == "Terminal")
             {
-                fc->m_wheel_soc.send(std::move(m), 0, std::move(*ctl));
-                std::cout<<"------- callback msg sent -------"<<std::endl;
+                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)
+                {
+                    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(ctl){
-                delete ctl;
+            std::string result_str = result.SerializeAsString();
+            std::cout << "--------callback serialized: " <<std::endl<< result.DebugString() << "------" << std::endl;
+            fc->m_mutex_wheel_handling.lock();
+            if (fc->m_wheel_soc) {
+                fc->m_wheel_soc.send(result_str.data(), result_str.length(), 0,
+                                     std::move(*task.sock_controller));
+                std::cout << "------- callback msg sent -------" << std::endl;
             }
+            fc->m_mutex_wheel_handling.unlock();
+            delete task.sock_controller;
         }
+
+        // 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/FenceController.h

@@ -30,6 +30,7 @@ using google::protobuf::Message;
 #include "unistd.h"
 #include "s7_plc.h"
 #include "PlcData.h"
+#include "string.h"
 
 #include <nnxx/message.h>
 #include <nnxx/message_control.h>
@@ -44,11 +45,19 @@ using google::protobuf::Message;
 #include "TaskQueue/TQFactory.h"
 #include "wheelCalcTask.h"
 
+#include "threadSafeQueue.h"
+
 #ifndef FENCECONTROLLER_H
 #define FENCECONTROLLER_H
 
 #define CONNECTSTRING "tcp://127.0.0.1:9000"
 
+typedef struct MSGTASK
+{
+    std::string cmd;
+    nnxx::message_control* sock_controller;
+}MsgTask;
+
 class FenceController{
 public:
     FenceController(std::string path);
@@ -58,6 +67,7 @@ public:
     // 外部获取点云
     void GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
     // 中心轮距指令接收线程
+    static void wheelMsgRecvThread(FenceController* fc);
     static void wheelMsgHandlingThread(FenceController* fc);
     // 获取轮距回调
     static void sendWheelResultCallback(int termID, nnxx::message_control* ctl, void *fc);
@@ -90,9 +100,10 @@ private:
 
     // 中心轮距相关变量
     std::thread*                            m_wheel_recv_thread;
+    std::thread*                            m_wheel_send_thread;
     StdCondition                            m_cond_wheel_exit;
     // wheel calculate queue
-    tq::IQueue                              *m_wheel_calc_queue;
+    threadsafe_queue<MsgTask>               m_msg_queue;
 
 };
 

src/findwheel/src/PlcData.cpp

@@ -46,11 +46,11 @@ void PlcData::plcThread(PlcData* p){
     if(p->p_plc == 0) return;
     while(!p->m_cond_exit.WaitFor(1)){
         // 显示雷达结果
-        std::cout<<"result: [";
+        /*std::cout<<"result: [";
         for (int i = 0; i < MAX_REGIONS; ++i) {
             std::cout<<p->m_data[i]<<" ";
         }
-        std::cout<<"]"<<std::endl;
+        std::cout<<"]"<<std::endl;*/
         // 写plc
         p->p_plc->WriteShorts(ELE_FENCE_DB_NUM, ELE_FENCE_START_ADDR, MAX_REGIONS, p->m_data);
 

src/findwheel/src/find_wheel_node.cpp

@@ -15,7 +15,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) {
 //
 //        ::ros::NodeHandle g_ros_handle;

src/findwheel/src/region_detect.cpp

@@ -47,7 +47,7 @@ bool Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, p
     // 离群点过滤
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
     sor.setInputCloud(cloud_out);
-    sor.setMeanK(4); //K近邻搜索点个数
+    sor.setMeanK(10); //K近邻搜索点个数
     sor.setStddevMulThresh(4.0); //标准差倍数
     sor.setNegative(false); //保留未滤波点（内点）
     sor.filter(*cloud_out);  //保存滤波结果到cloud_filter
@@ -66,10 +66,122 @@ bool Region_detector::findWheelbase()
     return true;
 }
 
-bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in){
+double distance(cv::Point2f p1, cv::Point2f p2)
+{
+    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
+}
+bool Region_detector::isRect(std::vector<cv::Point2f>& points)
+{
+    if (points.size() == 4)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        int max_index = 0;
+        cv::Point2f ps = points[0], pt = points[1];
+        cv::Point2f pc = points[2];
+        for (int i = 1; i < 3; ++i) {
+            if (L[i] > max_l) {
+                max_index = i;
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.13) {
+            std::cout << " angle cos >0.13 =" << cosa << "  i=" << max_index;
+            std::cout << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
+            return false;
+        }
+
+        float width=std::min(l1,l2);
+        float length=std::max(l1,l2);
+        if(width<1.400 || width >2.000 || length >3.300 ||length < 2.200)
+        {
+            std::cout<<"\t width<1400 || width >2100 || length >3300 ||length < 2100 "
+                      <<"  length:"<<length<<"  width:"<<width;
+            return false;
+        }
+
+        double d = distance(pc, points[3]);
+        cv::Point2f center1 = (ps + pt) * 0.5;
+        cv::Point2f center2 = (pc + points[3]) * 0.5;
+        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
+            std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2
+                    << "  center distance=" << distance(center1, center2);;
+            return false;
+        }
+        std::cout << " rectangle verify OK  cos angle=" << cosa << "  length off=" << fabs(d - max_l)
+                  << "  center distance=" << distance(center1, center2);
+        return true;
+    }
+    else if(points.size()==3)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        int max_index = 0;
+        cv::Point2f ps = points[0], pt = points[1];
+        cv::Point2f pc = points[2];
+        for (int i = 1; i < 3; ++i) {
+            if (L[i] > max_l) {
+                max_index = i;
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.12) {
+            std::cout << "3 wheels angle cos >0.12 =" << cosa << "  i=" << max_index;
+            std::cout << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
+            return false;
+        }
+
+        double l=std::max(l1,l2);
+        double w=std::min(l1,l2);
+        if(l>2.100 && l<3.300 && w>1.400 && w<2.100)
+        {
+            //生成第四个点
+            cv::Point2f vec1=ps-pc;
+            cv::Point2f vec2=pt-pc;
+            cv::Point2f point4=(vec1+vec2)+pc;
+            points.push_back(point4);
+            std::cout << "3 wheels rectangle verify OK  cos angle=" << cosa << "  L=" << l
+                      << "  w=" << w;
+            return true;
+        }
+        else
+        {
+            std::cout<< "3 wheels rectangle verify Failed  cos angle=" << cosa << "  L=" << l
+                      << "  w=" << w;
+            return false;
+        }
+
+    }
+
+}
+
+bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {
+    if (cloud_in->size() == 0) return false;
     /////聚类
     pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
-    kdtree->setInputCloud(cut_cloud);
+    kdtree->setInputCloud(cloud_in);
 
     pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
     // 设置聚类的最小值 2cm (small values may cause objects to be divided
@@ -79,38 +191,53 @@ bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in){
     clustering.setMinClusterSize(10);
     clustering.setMaxClusterSize(500);
     clustering.setSearchMethod(kdtree);
-    clustering.setInputCloud(cut_cloud);
+    clustering.setInputCloud(cloud_in);
     std::vector<pcl::PointIndices> clusters;
     clustering.extract(clusters);
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
+    for (int i = 0; i < clusters.size(); ++i) {
+        seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    }
 
-    for (int i = 0; i < 4; ++i) {
-        m_cloud_segs[i]->clear();
+    int j = 0;
+    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it) {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
+        //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
+        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
+            cloud_cluster->points.push_back(cloud_in->points[*pit]);
+            cloud_cluster->width = cloud_cluster->points.size();
+            cloud_cluster->height = 1;
+            cloud_cluster->is_dense = true;
+        }
+        seg_clouds[j] = cloud_cluster;
+        j++;
     }
-    if(clusters.size()==4)
+
+    std::cout << " cluster class:" << clusters.size() << std::endl;
+
+    std::vector<cv::Point2f> cvPoints;
+    for(int i=0;i<clusters.size();++i)
     {
-        int j = 0;
-        for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
+        if(seg_clouds[i]->size()==0)
+            continue;
+        float sumX=0,sumY=0;
+        for(int j=0;j<seg_clouds[i]->size();++j)
         {
-            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
-            //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
-            for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
-            {
-                cloud_cluster->points.push_back(cut_cloud->points[*pit]);
-                cloud_cluster->width = cloud_cluster->points.size();
-                cloud_cluster->height = 1;
-                cloud_cluster->is_dense = true;
-            }
-            m_cloud_segs[j]=cloud_cluster;
-            j++;
+            sumX+=seg_clouds[i]->points[j].x;
+            sumY+=seg_clouds[i]->points[j].y;
         }
-        // std::cout<<" cluster class:"<<clusters.size()<<std::endl;
-        return true;
+
+        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));
     }
-    return false;
+    return isRect(cvPoints);
+
 }
 
 bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
 {
+    return false;
     bool result = false;
     preprocess(cloud_in, cut_cloud);
 
@@ -134,29 +261,22 @@ bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, doubl
 
     m_seg_mutex.lock();
     result = clustering(cloud_filterd);
-    // std::cout<<"--------detector after clustering------"<<std::endl;
-    if(result){
-        // 获得四类，计算中心点角度等
-        std::vector<Eigen::Vector4f> centers;
-        std::vector<cv::Point2f> center_2d_points;
-        Eigen::Vector4f car_center;
-        for (size_t i = 0; i < 4; i++)
-        {
-            // 创建存储点云重心的对象
-            Eigen::Vector4f centroid;
-            if(0 == pcl::compute3DCentroid(*m_cloud_segs[i], centroid))
-                return false;
-            centers.push_back(centroid);
-            car_center += centroid;
-            center_2d_points.push_back(cv::Point2f(centroid[0], centroid[1]));
-        }
-        // std::cout<<"--------detector find center------"<<std::endl;
-        car_center /= 4.0;
-        x = car_center[0];
-        y = car_center[1];
 
-        cv::RotatedRect wheel_box = cv::minAreaRect(center_2d_points);
-        // std::cout<<"--------detector find rect -----"<<std::endl;
+
+
+    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));
+        }
+         std::cout<<"--------detector find center------"<<std::endl;
+        cv::RotatedRect wheel_box = cv::minAreaRect(all_points);
+        x=wheel_box.center.x;
+        y=wheel_box.center.y;
+         std::cout<<"--------detector find rect -----"<<std::endl;
         cv::Point2f vec;
         cv::Point2f vertice[4];
         wheel_box.points(vertice);

src/findwheel/src/region_detect.h

@@ -57,6 +57,7 @@ private:
     bool clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
     bool findWheelbase();
 
+    bool isRect(std::vector<cv::Point2f>& points);
 private:
     std::mutex              m_seg_mutex;
     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_cloud_segs;

src/findwheel/src/threadSafeQueue.h

@@ -0,0 +1,101 @@
+//
+// Created by zx on 2019/12/17.
+//
+
+#ifndef SRC_THREADSAFEQUEUE_H
+#define SRC_THREADSAFEQUEUE_H
+
+
+#include <queue>
+#include <memory>
+#include <mutex>
+#include <condition_variable>
+template<typename T>
+class threadsafe_queue
+{
+private:
+    mutable std::mutex mut;
+    std::queue<T> data_queue;
+    std::condition_variable data_cond;
+public:
+    threadsafe_queue() {}
+    threadsafe_queue(threadsafe_queue const& other)
+    {
+        std::lock_guard<std::mutex> lk(other.mut);
+        data_queue = other.data_queue;
+    }
+    ~threadsafe_queue()
+    {
+        while (!empty())
+        {
+            try_pop();
+        }
+    }
+    size_t size()
+    {
+        return data_queue.size();
+    }
+    void push(T new_value)//��Ӳ���
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        data_queue.push(new_value);
+        data_cond.notify_one();
+    }
+    void wait_and_pop(T& value)//ֱ����Ԫ�ؿ���ɾ��Ϊֹ
+    {
+        std::unique_lock<std::mutex> lk(mut);
+        data_cond.wait(lk, [this] {return !data_queue.empty(); });
+        value = data_queue.front();
+        data_queue.pop();
+    }
+    std::shared_ptr<T> wait_and_pop()
+    {
+        std::unique_lock<std::mutex> lk(mut);
+        data_cond.wait(lk, [this] {return !data_queue.empty(); });
+        std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
+        data_queue.pop();
+        return res;
+    }
+    //ֻ���� �� pop
+    bool front(T& value)
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        if (data_queue.empty())
+            return false;
+        value = data_queue.front();
+        return true;
+    }
+
+    bool try_pop(T& value)//������û�ж���Ԫ��ֱ�ӷ���
+    {
+        if (data_queue.empty())
+            return false;
+        std::lock_guard<std::mutex> lk(mut);
+        value = data_queue.front();
+        data_queue.pop();
+        return true;
+    }
+    std::shared_ptr<T> try_pop()
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        if (data_queue.empty())
+            return std::shared_ptr<T>();
+        std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
+        data_queue.pop();
+        return res;
+    }
+    bool empty() const
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        return data_queue.empty();
+    }
+    void clear()
+    {
+        while (!empty()) {
+            try_pop();
+        }
+    }
+};
+
+
+#endif //SRC_THREADSAFEQUEUE_H