change driver_can_nvidia_agx_new. change recv notify.

src/detection/detection_localization_global_hdl/hdl_global_localization/engines/global_localization_bbs.cpp

@@ -98,6 +98,8 @@ GlobalLocalizationResults GlobalLocalizationBBS::query(pcl::PointCloud<pcl::Poin
   results.resize(1);
   results[0].reset(new GlobalLocalizationResult(best_score, best_score, trans_3d));
 
+  std::cout<<trans_3d.matrix()<<std::endl;
+
   return GlobalLocalizationResults(results);
 }
 

src/detection/detection_localization_global_hdl/hdl_global_localization/engines/global_localization_fpfh_teaser.cpp

@@ -17,7 +17,9 @@ GlobalLocalizationEngineFPFH_Teaser::~GlobalLocalizationEngineFPFH_Teaser() {}
 
 void GlobalLocalizationEngineFPFH_Teaser::set_global_map(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud) {
   this->global_map = cloud;
+  std::cout<<" extract fpfh"<<std::endl;
   this->global_map_features = extract_fpfh(cloud);
+  std::cout<<" end fpfh."<<std::endl;
 
   evaluater.reset(new MatchingCostEvaluaterFlann());
   evaluater->set_target(this->global_map,1.0);// private_nh.param<double>("ransac/max_correspondence_distance", 1.0));
@@ -26,6 +28,7 @@ void GlobalLocalizationEngineFPFH_Teaser::set_global_map(pcl::PointCloud<pcl::Po
 GlobalLocalizationResults GlobalLocalizationEngineFPFH_Teaser::query(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud, int max_num_candidates) {
   auto cloud_features = extract_fpfh(cloud);
 
+  std::cout<<"query teaser"<<std::endl;
   teaser::PointCloud target_cloud, source_cloud;
   teaser::FPFHCloud target_features, source_features;
   target_cloud.reserve(global_map->size());
@@ -78,6 +81,8 @@ GlobalLocalizationResults GlobalLocalizationEngineFPFH_Teaser::query(pcl::PointC
   transformation.linear() = solution.rotation.cast<float>();
   transformation.translation() = solution.translation.cast<float>();
 
+  std::cout<<transformation.matrix()<<std::endl;
+
   double inlier_fraction = 0.0;
   double error = evaluater->calc_matching_error(*cloud, transformation.matrix(), &inlier_fraction);
 

src/detection/detection_localization_global_hdl/hdl_global_localization/ransac/ransac_pose_estimation.cpp

@@ -117,7 +117,7 @@ GlobalLocalizationResults RansacPoseEstimation<FeatureT>::estimate() {
   std::atomic_int iterations(0);
 //  std::vector<GlobalLocalizationResult::Ptr> results(private_nh.param<int>("ransac/max_iterations", 100000));
   std::vector<GlobalLocalizationResult::Ptr> results(100000);
-  int matching_budget =10000;// private_nh.param<int>("ransac/matching_budget", 10000);
+  int matching_budget =100;// private_nh.param<int>("ransac/matching_budget", 10000);
   double min_inlier_fraction = 0.25;// private_nh.param<double>("ransac/inlier_fraction", 0.25);
 
 #pragma omp parallel for
@@ -143,6 +143,7 @@ GlobalLocalizationResults RansacPoseEstimation<FeatureT>::estimate() {
     //   continue;
     // }
 
+    std::cout<<transformation<<std::endl;
     matching_count++;
     double inlier_fraction = 0.0;
     double matching_error = evaluater->calc_matching_error(*source, transformation, &inlier_fraction);

src/detection/detection_localization_global_hdl/main.cpp

@@ -14,13 +14,13 @@ int main(int argc, char *argv[])
 //    hdl_global_localization::GlobalLocalizationBBS gglobal;
 
 //    pcl::PointCloud<pcl::PointXYZ>::Ptr mapx = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);;
-//    if(0 == pcl::io::loadPCDFile("/home/nvidia/map/xingxialicangfang.pcd",*mapx))
+//    if(0 == pcl::io::loadPCDFile("/home/nvidia/map/mapcangfangnei.pcd",*mapx))
 //    {
 //        std::cout<<" Load Map success."<<std::endl;
 //        gglobal.set_global_map(mapx);
 
 //        pcl::PointCloud<pcl::PointXYZ>::Ptr pc1 = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
-//        if(0 == pcl::io::loadPCDFile("/home/nvidia/pcd/xiali2.pcd",*pc1))
+//        if(0 == pcl::io::loadPCDFile("/home/nvidia/pcd/xiali9.pcd",*pc1))
 //        {
 //            std::cout<<"Load point cloud success."<<std::endl;
 //             gglobal.query(pc1,10);
@@ -30,13 +30,13 @@ int main(int argc, char *argv[])
     hdl_global_localization::GlobalLocalizationEngineFPFH_RANSAC gglobal;
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr mapx = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);;
-    if(0 == pcl::io::loadPCDFile("/home/nvidia/map/xingxialicangfang.pcd",*mapx))
+    if(0 == pcl::io::loadPCDFile("/home/nvidia/map/mapcangfangnei.pcd",*mapx))
     {
         std::cout<<" Load Map success."<<std::endl;
         gglobal.set_global_map(mapx);
 
         pcl::PointCloud<pcl::PointXYZ>::Ptr pc1 = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
-        if(0 == pcl::io::loadPCDFile("/home/nvidia/pcd/xiali2.pcd",*pc1))
+        if(0 == pcl::io::loadPCDFile("/home/nvidia/pcd/xiali10.pcd",*pc1))
         {
             std::cout<<"Load point cloud success."<<std::endl;
              gglobal.query(pc1,10);

src/driver/driver_can_nvidia_agx_new/canctrl.cpp

@@ -47,7 +47,7 @@ canctrl::canctrl(const char * strmemsend0,const char * strmemrecv0,const char *
     mpasend0 = iv::modulecomm::RegisterRecv(strmemsend0,Listencansend0);
     mpasend1 = iv::modulecomm::RegisterRecv(strmemsend1,Listencansend1);
 
-    mpcan = new nvcan(strcan0name,strcan1name);
+    mpcan = new nvcan(strcan0name,strcan1name,this);
     mspcan.reset(mpcan);
     connect(mpcan,SIGNAL(SIG_CANOPENSTATE(bool,int,const char*)),this,SLOT(onCANState(bool,int,const char*)));
     mpcan->startdev();
@@ -105,6 +105,9 @@ void canctrl::run()
     {
         if(mbCANOpen)
         {
+            mMutexWait.lock();
+            mwc.wait(&mMutexWait,5);
+            mMutexWait.unlock();
             basecan_msg xmsg[2500];
             int nRec1,nRec2,nSend1,nSend2;
             if((nRec1 =mpcan->GetMessage(0,xmsg,2500))>0)
@@ -112,12 +115,16 @@ void canctrl::run()
                 sharecanmsg(mparecv0,xmsg,nRec1,0);
             }
 
+            if((nRec2 =mpcan->GetMessage(1,xmsg,2500))>0)
+            {
+                sharecanmsg(mparecv1,xmsg,nRec1,1);
+            }
+
 
 
             nSend1 = 0;
             nSend2 = 0;
 
-            msleep(5);
         }
         else
         {
@@ -257,3 +264,8 @@ void canctrl::sharecanmsg(void *xpa, basecan_msg * pxmsg,int ncount,int nch)
     }
     delete strdata;
 }
+
+void canctrl::NotifyRecv()
+{
+    mwc.wakeAll();
+}

src/driver/driver_can_nvidia_agx_new/canctrl.h

@@ -46,6 +46,7 @@ private:
 public:
     void sendmsg(int index,iv::can::canmsg xmsg);
     void sharecanmsg(void * xpa,basecan_msg * pxmsg,int ncount,int nch);
+    void NotifyRecv();
 
 
 private:
@@ -53,6 +54,9 @@ private:
     std::thread * mptheadstate;
     bool mbstaterun = true;
 
+    QWaitCondition mwc;
+    QMutex mMutexWait;
+
 
 
 

src/driver/driver_can_nvidia_agx_new/nvcan.cpp

@@ -27,6 +27,8 @@
 #include <iostream>
 #include  <thread>
 
+#include "canctrl.h"
+
 /* for hardware timestamps - since Linux 2.6.30 */
 #ifndef SO_TIMESTAMPING
 #define SO_TIMESTAMPING 37
@@ -54,8 +56,9 @@
 
 std::string CANNAME[] = {"can0","can1"};
 
-nvcan::nvcan(const char * strcan0name,const char * strcan1name)
+nvcan::nvcan(const char * strcan0name,const char * strcan1name,void * pcanctrl)
 {
+    mpcanctrl = pcanctrl;
 //    qDebug("nvcan");
 //    connect(this,SIGNAL(SIG_CANOPENSTATE(bool,int,const char*)),this,SLOT(onMsg(bool,int,const char*)));
 
@@ -196,6 +199,9 @@ int nvcan::ExecRecv(int & s,fd_set & rdfs,int nch)
 
         mMutex.unlock();
 
+        canctrl * pcantrl = (canctrl * )mpcanctrl;
+        pcantrl->NotifyRecv();
+
 
 
     }

src/driver/driver_can_nvidia_agx_new/nvcan.h

@@ -11,11 +11,13 @@
 #include <QMutex>
 #include <QTimer>
 
+
+
 class nvcan : public basecan
 {
     Q_OBJECT
 public:
-    nvcan(const char * strcan0name,const char * strcan1name);
+    nvcan(const char * strcan0name,const char * strcan1name,void * pcanctrl);
 public:
     void startdev();
     void stopdev();
@@ -69,6 +71,9 @@ private:
     int ExecRecv(int & s,fd_set & rdfs,int nch);
     int ExecSend(int * s ,struct canfd_frame & framesend,int nch,qint64 xMsgSetTime);
 
+private:
+    void * mpcanctrl;
+
 
 };
 

src/tool/bqev_pcdview/main.cpp

@@ -79,7 +79,8 @@ int main(int argc, char *argv[])
     showversion("bqev_pcdview");
     QApplication a(argc, argv);
 
-    snprintf(gstr_memname,255,"lidar_pc");
+//    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_memname,255,"lidarpc_center");
 
     int nRtn = GetOptLong(argc,argv);
     if(nRtn == 1)  //show help,so exit.

src/tool/pointcloudviewer/main.cpp

@@ -186,6 +186,8 @@ int main(int argc, char *argv[])
     QCoreApplication a(argc, argv);
 
     snprintf(gstr_memname,255,"lidar_pc");
+//    snprintf(gstr_memname,255,"lidarpc_center");
+
 
     int nRtn = GetOptLong(argc,argv);
     if(nRtn == 1)  //show help,so exit.

src/tool/pointcloudviewer/pointcloudviewer.pro

@@ -48,6 +48,11 @@ unix:LIBS +=  -lpcl_common\
 INCLUDEPATH += $$PWD/../../../include/
 LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault
 
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
 LIBS += -lboost_system
 #LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
 #        -lvtkFiltersSources-6.3