change fusion_pointcloud_shenlan. not complete.

src/fusion/fusion_pointcloud_shenlan/fusion_pointcloud_shenlan.pro

@@ -22,7 +22,45 @@ INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/eigen3
 
-LIBS += /usr/lib/aarch64-linux-gnu/libpcl_*.so
+#LIBS += /usr/lib/aarch64-linux-gnu/libpcl_*.so
+
+win32: LIBS += -LC:/File/PCL_1.8/PCL_1.8.1/lib -lpcl_common_debug\
+        -lpcl_features_debug\
+        -lpcl_filters_debug\
+        -lpcl_io_debug\
+        -lpcl_io_ply_debug\
+        -lpcl_kdtree_debug\
+        -lpcl_keypoints_debug\
+        -lpcl_octree_debug\
+        -lpcl_outofcore_debug\
+        -lpcl_people_debug\
+        -lpcl_recognition_debug\
+        -lpcl_registration_debug\
+        -lpcl_sample_consensus_debug\
+        -lpcl_search_debug\
+        -lpcl_segmentation_debug\
+        -lpcl_surface_debug\
+        -lpcl_tracking_debug\
+        -lpcl_visualization_debug
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
 
 HEADERS += \
     lidarmerge.h

src/fusion/fusion_pointcloud_shenlan/fusion_pointcloud_shenlan.yaml

@@ -1,7 +1,3 @@
-lidar:
-  - left
-  - right
-  - center
 left:
   memname: lidarpc_left
   maximum:

src/fusion/fusion_pointcloud_shenlan/lidarmerge.cpp

@@ -59,7 +59,7 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvect
                 {
                     continue;
                 }
-                if(!pcl_isfinite(point.x)||!pcl_isfinite(point.y)||!pcl_isfinite(point.z)||!pcl_isfinite(point.intensity)) continue;
+    //            if(!pcl_isfinite(point.x)||!pcl_isfinite(point.y)||!pcl_isfinite(point.z)||!pcl_isfinite(point.intensity)) continue;
 
                 point_cloud->push_back(point);
                 ++point_cloud->width;

src/fusion/fusion_pointcloud_shenlan/lidarmerge.h

@@ -6,12 +6,18 @@
 #include <pcl/io/io.h>
 #include <pcl/io/pcd_io.h>
 
+#include <mutex>
+
 
 
 namespace  iv {
 struct lidar_data
 {
+    std::mutex mmutex;
     char strmemname[256];
+
+
+
     double foff_x = 0.0;
     double foff_y = 0.0;
     double foff_z = 0.0;

src/fusion/fusion_pointcloud_shenlan/main.cpp

@@ -25,9 +25,48 @@ QMutex gMutex;
 
 static qint64 glasttime = 0;
 static std::vector<iv::lidar_data> gvectorlidar;
-static char gstroutmemname[255];
+
+static iv::lidar_data glidar_data_left;
+static iv::lidar_data glidar_data_right;
+static iv::lidar_data glidar_data_center;
+
+static char gstrcentermemename[256];
+static char gstroutmemname[256];
 static void * gpaout;
 
+static bool gbrun;
+
+
+static void InitLidarData()
+{
+    snprintf(glidar_data_left.strmemname,256,"lidarpc_left");
+    snprintf(glidar_data_right.strmemname,256,"lidarpc_right");
+
+    glidar_data_left.fmax_x = 300;
+    glidar_data_left.fmax_y = 300;
+    glidar_data_left.fmax_z = 300;
+    glidar_data_left.fmin_x = -300;
+    glidar_data_left.fmin_y = -300;
+    glidar_data_left.fmin_z = -300;
+    glidar_data_left.bUpdate = false;
+    glidar_data_left.mupdatetime = 0;
+
+    glidar_data_right.fmax_x = 300;
+    glidar_data_right.fmax_y = 300;
+    glidar_data_right.fmax_z = 300;
+    glidar_data_right.fmin_x = -300;
+    glidar_data_right.fmin_y = -300;
+    glidar_data_right.fmin_z = -300;
+    glidar_data_right.bUpdate = false;
+    glidar_data_right.mupdatetime = 0;
+
+    glidar_data_center.bUpdate = false;
+    glidar_data_center.mupdatetime = 0;
+
+}
+
+
+
 void dec_yaml(const char * stryamlpath)
 {
 
@@ -42,140 +81,34 @@ void dec_yaml(const char * stryamlpath)
         return;
     }
 
-    int i;
-    int nlidarsize;
-    std::vector<std::string> veclidarname;
 
-    if(config["lidar"])
+    if(config["left"]["memname"])
     {
-        qDebug("have lidar size is %d",config["lidar"].size());
-        nlidarsize = config["lidar"].size();
-        for(i=0;i<nlidarsize;i++)
-        {
-            std::string strname = config["lidar"][i].as<std::string>();
-            veclidarname.push_back(strname);
-        }
+        snprintf(glidar_data_left.strmemname,256,"%s",config["left"]["memname"].as<std::string>().data());
     }
-    else
+    if(config["left"]["maximum"]["x"])glidar_data_left.fmax_x = atof(config["left"]["maximum"]["x"].as<std::string>().data());
+    if(config["left"]["maximum"]["y"])glidar_data_left.fmax_y = atof(config["left"]["maximum"]["y"].as<std::string>().data());
+    if(config["left"]["maximum"]["z"])glidar_data_left.fmax_z = atof(config["left"]["maximum"]["z"].as<std::string>().data());
+    if(config["left"]["minimum"]["x"])glidar_data_left.fmin_x = atof(config["left"]["minimum"]["x"].as<std::string>().data());
+    if(config["left"]["minimum"]["y"])glidar_data_left.fmin_y = atof(config["left"]["minimum"]["y"].as<std::string>().data());
+    if(config["left"]["minimum"]["z"])glidar_data_left.fmin_z = atof(config["left"]["minimum"]["z"].as<std::string>().data());
+
+    if(config["right"]["memname"])
     {
-        return;
+        snprintf(glidar_data_right.strmemname,256,"%s",config["right"]["memname"].as<std::string>().data());
     }
-
-    if(nlidarsize <1)return;
-
-    std::string strmemname;
-    std::string stroffset_x;
-    std::string stroffset_y;
-    std::string stroffset_z;
-    std::string stroffset_angle;
-    std::string strtem;
-
-    double fignore_xmax = 0;
-    double fignore_ymax = 0;
-    double fignore_xmin = 0;
-    double fignore_ymin = 0;
-    double fignore_zmax = 0;
-    double fignore_zmin = 0;
-
-    if(config["ignore"]["xmin"])
+    if(config["right"]["maximum"]["x"])glidar_data_right.fmax_x = atof(config["right"]["maximum"]["x"].as<std::string>().data());
+    if(config["right"]["maximum"]["y"])glidar_data_right.fmax_y = atof(config["right"]["maximum"]["y"].as<std::string>().data());
+    if(config["right"]["maximum"]["z"])glidar_data_right.fmax_z = atof(config["right"]["maximum"]["z"].as<std::string>().data());
+    if(config["right"]["minimum"]["x"])glidar_data_right.fmin_x = atof(config["right"]["minimum"]["x"].as<std::string>().data());
+    if(config["right"]["minimum"]["y"])glidar_data_right.fmin_y = atof(config["right"]["minimum"]["y"].as<std::string>().data());
+    if(config["right"]["minimum"]["z"])glidar_data_right.fmin_z = atof(config["right"]["minimum"]["z"].as<std::string>().data());
+
+    if(config["center"]["memname"])
     {
-        strtem = config["ignore"]["xmin"].as<std::string>();
-        fignore_xmin = atof(strtem.data());
+        snprintf(gstrcentermemename,256,"%s",config["center"]["memname"].as<std::string>().data());
     }
 
-    if(config["ignore"]["ymin"])
-    {
-        strtem = config["ignore"]["ymin"].as<std::string>();
-        fignore_ymin = atof(strtem.data());
-    }
-
-    if(config["ignore"]["xmax"])
-    {
-        strtem = config["ignore"]["xmax"].as<std::string>();
-        fignore_xmax = atof(strtem.data());
-    }
-
-    if(config["ignore"]["ymax"])
-    {
-        strtem = config["ignore"]["ymax"].as<std::string>();
-        fignore_ymax = atof(strtem.data());
-    }
-    if(config["ignore"]["zmax"])
-    {
-        strtem = config["ignore"]["zmax"].as<std::string>();
-        fignore_zmax = atof(strtem.data());
-    }
-    if(config["ignore"]["zmin"])
-    {
-        strtem = config["ignore"]["zmin"].as<std::string>();
-        fignore_zmin = atof(strtem.data());
-    }
-
-
-    for(i=0;i<nlidarsize;i++)
-    {
-        if(config[veclidarname[i].data()])
-        {
-            if((config[veclidarname[i].data()]["memname"])&&(config[veclidarname[i].data()]["offset"]["x"])&&(config[veclidarname[i].data()]["offset"]["y"])&&(config[veclidarname[i].data()]["offset"]["z"]))
-            {
-                strmemname = config[veclidarname[i].data()]["memname"].as<std::string>();
-                stroffset_x = config[veclidarname[i].data()]["offset"]["x"].as<std::string>();
-                stroffset_y = config[veclidarname[i].data()]["offset"]["y"].as<std::string>();
-                stroffset_z = config[veclidarname[i].data()]["offset"]["z"].as<std::string>();
-                stroffset_angle = config[veclidarname[i].data()]["offset"]["angle"].as<std::string>();
-
-                iv::lidar_data xlidardata;
-                xlidardata.foff_x = atof(stroffset_x.data());
-                xlidardata.foff_y = atof(stroffset_y.data());
-                xlidardata.foff_z = atof(stroffset_z.data());
-                xlidardata.foff_angle = atof(stroffset_angle.data());
-                if(config[veclidarname[i].data()]["maximum"]["x"])
-                {
-                    strtem = config[veclidarname[i].data()]["maximum"]["x"].as<std::string>();
-                    xlidardata.fmax_x = atof(strtem.data());
-                }
-                if(config[veclidarname[i].data()]["maximum"]["y"])
-                {
-                    strtem = config[veclidarname[i].data()]["maximum"]["y"].as<std::string>();
-                    xlidardata.fmax_y = atof(strtem.data());
-                }
-                if(config[veclidarname[i].data()]["maximum"]["z"])
-                {
-                    strtem = config[veclidarname[i].data()]["maximum"]["z"].as<std::string>();
-                    xlidardata.fmax_z = atof(strtem.data());
-                }
-                if(config[veclidarname[i].data()]["minimum"]["x"])
-                {
-                    strtem = config[veclidarname[i].data()]["minimum"]["x"].as<std::string>();
-                    xlidardata.fmin_x = atof(strtem.data());
-                }
-                if(config[veclidarname[i].data()]["minimum"]["y"])
-                {
-                    strtem = config[veclidarname[i].data()]["minimum"]["y"].as<std::string>();
-                    xlidardata.fmin_y = atof(strtem.data());
-                }
-                if(config[veclidarname[i].data()]["minimum"]["z"])
-                {
-                    strtem = config[veclidarname[i].data()]["minimum"]["z"].as<std::string>();
-                    xlidardata.fmin_z = atof(strtem.data());
-                }
-
-                xlidardata.fignore_xmax = fignore_xmax;
-                xlidardata.fignore_ymax = fignore_ymax;
-                xlidardata.fignore_xmin = fignore_xmin;
-                xlidardata.fignore_ymin = fignore_ymin;
-                xlidardata.fignore_zmax = fignore_zmax;
-                xlidardata.fignore_zmin = fignore_zmin;
-
-
-                strncpy(xlidardata.strmemname,strmemname.data(),255);
-                gvectorlidar.push_back(xlidardata);
-                qDebug("name is %s ",strmemname.data());
-
-            }
-
-        }
-    }
 
     if(config["output"])
     {
@@ -185,6 +118,13 @@ void dec_yaml(const char * stryamlpath)
     {
         strncpy(gstroutmemname,"lidar_pc",255);
     }
+
+    iv::lidar_data * pl = &glidar_data_left;
+    pl = &glidar_data_right;
+
+    char * strmemc = gstrcentermemename;
+    strmemc = gstroutmemname;
+
     return;
 
 }
@@ -224,19 +164,33 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
   //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
     }
 
+    if(strncmp(strmemname,glidar_data_left.strmemname,255) == 0)
+    {
+        glidar_data_left.mmutex.lock();
+        glidar_data_left.mpoint_cloud = point_cloud;
+        glidar_data_left.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_left.bUpdate = true;
+        glidar_data_left.mmutex.unlock();
+    }
 
-    for(i=0;i<gvectorlidar.size();i++)
+    if(strncmp(strmemname,glidar_data_right.strmemname,255) == 0)
     {
-        if(strncmp(strmemname,gvectorlidar[i].strmemname,255) == 0)
-        {
-            gMutex.lock();
-            gvectorlidar[i].mpoint_cloud_old = gvectorlidar[i].mpoint_cloud;
-            gvectorlidar[i].mpoint_cloud = point_cloud;
-            gvectorlidar[i].mupdatetime = QDateTime::currentMSecsSinceEpoch();
-            gvectorlidar[i].bUpdate = true;
-            gMutex.unlock();
-        }
+        glidar_data_right.mmutex.lock();
+        glidar_data_right.mpoint_cloud = point_cloud;
+        glidar_data_right.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_right.bUpdate = true;
+        glidar_data_right.mmutex.unlock();
     }
+
+    if(strncmp(strmemname,gstrcentermemename,255) == 0)
+    {
+        glidar_data_center.mmutex.lock();
+        glidar_data_center.mpoint_cloud = point_cloud;
+        glidar_data_center.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_center.bUpdate = true;
+        glidar_data_center.mmutex.unlock();
+    }
+
 }
 
 void sharepointcloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud,void * pa)
@@ -246,7 +200,8 @@ void sharepointcloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud,void * pa)
     int * pHeadSize = (int *)strOut;
     *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
     memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
-    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+  //  pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    unsigned int * pu32 = (unsigned int *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
     *pu32 = point_cloud->header.seq;
     memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
     pcl::PointXYZI * p;
@@ -257,56 +212,71 @@ void sharepointcloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud,void * pa)
     delete strOut;
 }
 
-void merge()
+
+
+void mergethread()
 {
     int i;
-    std::vector<iv::lidar_data> xvectorlidar;
-    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
-                new pcl::PointCloud<pcl::PointXYZI>());
+    int64_t nleftupdatetime = 0;
+    int64_t nrightupdatetime = 0;
+    int64_t ncenterupdatetime = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_left = NULL;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_right = NULL;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_center = NULL;
+    while(gbrun)
+    {
 
-    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(
-                new pcl::PointCloud<pcl::PointXYZI>());
+        bool bCenterUpdate = false;
+        bool bLeftUpdate = false;
+        bool bRightUpdate = false;
+        glidar_data_left.mmutex.lock();
+        if(glidar_data_left.bUpdate)
+        {
+            point_cloud_left = glidar_data_left.mpoint_cloud;
+            nleftupdatetime = glidar_data_left.mupdatetime;
+            glidar_data_left.bUpdate = false;
+            bLeftUpdate = true;
+        }
+        glidar_data_left.mmutex.unlock();
 
+        glidar_data_right.mmutex.lock();
+        if(glidar_data_right.bUpdate)
+        {
+            point_cloud_right = glidar_data_right.mpoint_cloud;
+            nrightupdatetime = glidar_data_right.mupdatetime;
+            glidar_data_right.bUpdate = false;
+            bRightUpdate = true;
+        }
+        glidar_data_right.mmutex.unlock();
 
+        glidar_data_center.mmutex.lock();
+        if(glidar_data_center.bUpdate)
+        {
+            point_cloud_center = glidar_data_left.mpoint_cloud;
+            ncenterupdatetime = glidar_data_left.mupdatetime;
+            glidar_data_left.bUpdate = false;
+            bCenterUpdate = true;
+        }
+        glidar_data_left.mmutex.unlock();
 
-    gMutex.lock();
-    xvectorlidar = gvectorlidar;
-    for(i=0;i<gvectorlidar.size();i++)
-    {
-        gvectorlidar[i].bUpdate = false;
-    }
-    gMutex.unlock();
-    point_cloud = mergefunc(xvectorlidar);
-    pcl::RadiusOutlierRemoval<pcl::PointXYZI> pcFilter;
-    pcFilter.setInputCloud(point_cloud);
-    pcFilter.setRadiusSearch(0.8);
-    pcFilter.setMinNeighborsInRadius(20);
-    pcFilter.filter(*cloud_filtered);
-    sharepointcloud(cloud_filtered,gpaout);
-}
+        if(bLeftUpdate)
+        {
 
-void mergethread()
-{
-    int i;
-    while(1)
-    {
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-        bool bNOtAllOK = false;
-        for(i=0;i<gvectorlidar.size();i++)
+        }
+        if(bRightUpdate)
         {
-            if(!gvectorlidar[i].bUpdate)
-            {
-                bNOtAllOK = true;
-                break;
-            }
+
         }
-        if(((QDateTime::currentMSecsSinceEpoch() - glasttime)>=150) ||(bNOtAllOK == false))
+        //transform
+
+        if(bCenterUpdate)
         {
-            merge();
-            qDebug("time is %ld",glasttime);
-            glasttime = QDateTime::currentMSecsSinceEpoch();
 
         }
+        //Merge
+
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
 
     }
 }
@@ -314,17 +284,17 @@ void mergethread()
 
 int main(int argc, char *argv[])
 {
-    showversion("fusion_pointcloud_bus");
+    showversion("fusion_pointcloud_shenlan");
     QCoreApplication a(argc, argv);
 
-    dec_yaml("fusion_pointcloud_bus.yaml");
+    InitLidarData();
+
+    dec_yaml("fusion_pointcloud_shenlan.yaml");
+
+    iv::modulecomm::RegisterRecv(glidar_data_left.strmemname,ListenPointCloud);
+    iv::modulecomm::RegisterRecv(glidar_data_right.strmemname,ListenPointCloud);
+    iv::modulecomm::RegisterRecv(gstrcentermemename,ListenPointCloud);
 
-    void * pa;
-    int i;
-    for(i=0;i<gvectorlidar.size();i++)
-    {
-        iv::modulecomm::RegisterRecv(gvectorlidar[i].strmemname,ListenPointCloud);
-    }
     gpaout = iv::modulecomm::RegisterSend(gstroutmemname,20000000,1);
 
     std::thread xthread(mergethread);