add gedian wj test

laser/LivoxLaser.cpp

@@ -155,11 +155,11 @@ bool CLivoxLaser::is_ready()
 	if ( m_laser_statu == LASER_READY &&
 		 g_devices[m_handle].device_state != kDeviceStateDisconnect  )
 	{
-		true;
+		return true;
 	}
 	else
 	{
-	    false;
+	    return false;
 	}
 }
 

test/wj_lidar_test.cpp

@@ -29,6 +29,12 @@
 typedef std::chrono::duration<int, std::ratio<1, 1000>> milli_type;
 
 std::atomic<bool> g_measure_cmd, g_measure_completed;
+// 范围: 
+// x -2400 0
+// b -20 20
+// plc(0,0)->laser(4.315, 84.46)
+// x 0 2400
+// ->laser(1.915, 84.46)
 
 class Temp_plc
 {
@@ -98,6 +104,16 @@ public:
         return m_data[CURRB_];
     }
 
+    short get_last_x()
+    {
+        return m_last_data[CURRX_];
+    }
+
+    short get_last_b()
+    {
+        return m_last_data[CURRB_];
+    }
+
     bool get_data(short *data)
     {
         if(mb_exit || ! mb_ready)
@@ -149,6 +165,7 @@ private:
                 {
                     std::lock_guard<std::mutex> lck(p->m_mutex);
                     p->mb_ready = p->m_plc.ReadShorts(DB, 0, BLOCK_SIZE, p->m_data);
+                    memcpy(p->m_last_data, p->m_data, BLOCK_SIZE * sizeof(short));
                 }
                 p->disp_data();
                 // 查询状态
@@ -161,7 +178,12 @@ private:
                         // 等待测量结束
                         while (!g_measure_completed)
                         {
-                            usleep(1000 * 200);
+                            usleep(1000 * 100);
+                            {
+                                std::lock_guard<std::mutex> lck(p->m_mutex);
+                                p->mb_ready = p->m_plc.ReadShorts(DB, 0, BLOCK_SIZE, p->m_data);
+                                memcpy(p->m_last_data, p->m_data, BLOCK_SIZE * sizeof(short));
+                            }
                             std::cout << "wating for measurement." << std::endl;
                         }
                         g_measure_completed = false;
@@ -169,7 +191,14 @@ private:
 
                         // 写指令
                         p->mb_device_responsing = true;
-                        p->mb_ready = p->write_cmd(rnd.uniform(1, 100), rnd.uniform(1, 30));
+                        short rndx, rndb;
+                        do{
+                            rndx = rnd.uniform(0, 8);
+                            rndb = 0;
+                            // rnd.uniform(-20, 20);
+                        } while (abs(rndx*300 - p->get_curr_x()) < 300);// || abs(rndb - p->get_curr_b()) < 3);
+                        std::cout << "write cmd: "<< rndx*300<<", "<<rndb << std::endl;
+                        p->mb_ready = p->write_cmd(rndx*300, rndb);
                     }
                     else{
                         std::cout << "waiting for status switch." << std::endl;
@@ -185,8 +214,6 @@ private:
                     // p->write_data(1, 3);
                 }
 
-
-                memcpy(p->m_last_data, p->m_data, BLOCK_SIZE * sizeof(short));
             }else{
                 std::lock_guard<std::mutex> lck(p->m_mutex);
                 p->m_plc.disconnect();
@@ -220,6 +247,7 @@ private:
     short m_data[BLOCK_SIZE];
     short m_last_data[BLOCK_SIZE];
     bool mb_device_responsing; // 设备状态切换中
+
     bool mb_device_working; // 设备工作中
 };
 
@@ -489,6 +517,7 @@ void plc_test_only()
     }
 }
 
+#include "pcl/io/pcd_io.h"
 void accuracy_validation()
 {
     g_measure_cmd = false;
@@ -498,11 +527,11 @@ void accuracy_validation()
     struct tm *tc = localtime(&tt);
     char buf[255] = {0};
     memset(buf, 0, 255);
-    sprintf(buf, "./result_%d-%02d-%02d_%02d:%02d:%02d.txt", tc->tm_year + 1900,
-            tc->tm_mon + 1, tc->tm_mday, tc->tm_hour, tc->tm_min, tc->tm_sec);
+    sprintf(buf, "./result_%d-%02d-%02d.txt", tc->tm_year + 1900,
+            tc->tm_mon + 1, tc->tm_mday);
 
     std::ofstream out;
-    out.open(buf, std::ios::ate);
+    out.open(buf, std::ios::app);
     if(!out.is_open())
     {
         std::cout << "open file failed, "<< buf << std::endl;
@@ -542,55 +571,84 @@ void accuracy_validation()
     std::cout << "init detector: "<< t_detectors.size() << std::endl;
 
     // 初始化一个plc类用于读写
-    Temp_plc t_plc("192.168.0.1");
+    Temp_plc t_plc("192.168.0.100");
     short t_plc_data[BLOCK_SIZE];
-
+    // g_measure_cmd = true;
     char ch = 'x';
-    while(ch != 'q')
+    int mcount = 0;
+    while (ch != 'q')
     {
         std::cout << "please input cmd (m for measure, q for quit):" << std::endl;
         std::cin >> ch;
 
         if (ch == 'm')
         {
-            // 获取点云
-            pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-            for (int i = 0; i < t_lidars.size(); ++i)
-            {
-                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-                if (t_lidars[i]->get_cloud(cloud, std::chrono::steady_clock::now()) == SUCCESS)
-                {
-                    total_cloud->operator+=(*cloud);
-                }
-            }
+            std::cout << "start measure process." << std::endl;
 
-            // 从plc获取编码值
-            detect_wheel_ceres::Detect_result t_plc_values;
-            t_plc.get_data(t_plc_data);
-
-            // 测量获得结果,测试时通常只存在一个区域测量块
-            detect_wheel_ceres::Detect_result t_result;
-            std::string t_info;
-            Error_manager ec = t_detectors[0]->detect(total_cloud, t_result.cx, t_result.cy, t_result.theta,t_result.front_theta, t_result.wheel_base, t_result.width, t_info, false);
-            
-            // 测量结果与plc读取数据写入文件
-            char line_buf[512];
-            memset(line_buf, 0, 512);
-            if(ec == SUCCESS)
+            while (true)
             {
-                sprintf(line_buf, "success,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,plc,%.3f,%.3f,%.3f\n", 
-                                            t_result.cx, t_result.cy, t_result.theta,t_result.front_theta, t_result.wheel_base, t_result.width,
-                                            t_plc_values.cx, t_plc_values.cy, t_plc_values.theta);
-            }
-            else
-            {
-                sprintf(line_buf, "failed,0.0,0.0,0.0,0.0,0.0,0.0,plc,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n", 
-                                            t_plc_values.cx, t_plc_values.cy, t_plc_values.theta);
-                std::cout << "detect failed" << std::endl;
+                if(g_measure_cmd){
+                    mcount++;
+                    g_measure_cmd = false;
+
+                    int repeated_count = 6;
+                    while(repeated_count-->0){
+                        // 获取点云
+                        pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+                        for (int i = 0; i < t_lidars.size(); ++i)
+                        {
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+                            if (t_lidars[i]->get_cloud(cloud, std::chrono::steady_clock::now()) == SUCCESS)
+                            {
+                                total_cloud->operator+=(*cloud);
+                            }
+                        }
+                        std::cout << "get cloud size: "
+                                << total_cloud->size() << std::endl;
+                        // pcl::PCDWriter t_writer;
+                        // t_writer.write(std::string("./cloud/")+std::to_string(mcount)+std::to_string(repeated_count) + ".pcd", *total_cloud);
+
+                        // 测量获得结果,测试时通常只存在一个区域测量块
+                        detect_wheel_ceres::Detect_result t_result;
+                        std::string t_info;
+                        Error_manager ec = t_detectors[0]->detect(total_cloud, t_result.cx, t_result.cy, t_result.theta, t_result.front_theta, t_result.wheel_base, t_result.width, t_info, false);
+                        // Error_manager ec = t_detectors[0]->find_circle(total_cloud, t_result.cx, t_result.cy);
+
+                        // 从plc获取编码值
+                        short cx, cy, theta;
+                        cx = t_plc.get_last_x();
+                        cy = 0;
+                        theta = t_plc.get_last_b();
+
+                        // 测量结果与plc读取数据写入文件
+                        char line_buf[512];
+                        memset(line_buf, 0, 512);
+                        if (ec == SUCCESS)
+                        {
+                            sprintf(line_buf, "success,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,plc,%d,%d,%d",
+                                    t_result.cx, t_result.cy, t_result.theta, t_result.front_theta, t_result.wheel_base, t_result.width,
+                                    cx, cy, theta);
+                        }
+                        else
+                        {
+                            sprintf(line_buf, "failed,0.0,0.0,0.0,0.0,0.0,0.0,plc,%d,%d,%d",
+                                    cx, cy, theta);
+                            std::cout << "detect failed: "<< ec.to_string()<<" "<<t_info.c_str() << std::endl;
+                        }
+                        out << line_buf << std::endl;
+                        std::cout << line_buf << ", " << strlen(line_buf) << std::endl;
+                    }
+                    out << "seperation line" << std::endl;
+                    g_measure_completed = true;
+                    usleep(1000 * 50);
+                }
             }
         }
     }
-
+    if(out.is_open())
+    {
+        out.close();
+    }
 }
 
 int main(int argc, char *argv[])
@@ -605,11 +663,23 @@ int main(int argc, char *argv[])
     // lidar_test();
     // usleep(1000 * 1000);
 
+    // std::chrono::steady_clock::time_point current_time = std::chrono::steady_clock::now();
+    // std::chrono::time_point<std::chrono::steady_clock, milli_type> current_time_in_milliseconds = std::chrono::time_point_cast<milli_type>(current_time);
+    // int now_in_milliseconds = current_time_in_milliseconds.time_since_epoch().count();
+    // uint64 seed = uint64(now_in_milliseconds);
+    // cv::RNG rnd = cv::RNG(seed);
+    // for (size_t i = 0; i < 100; i++)
+    // {
+    //     short ttx = (short)rnd.uniform(-2400, 0);
+    //     std::cout << ttx<< std::endl;
+    // }
 
     // wj test
     // sample_detect("/home/youchen/Documents/measure/MainStructure/wj_data_records");
-    plc_test_only();
-    // accuracy_validation();
+    // plc_test_only();
+
+    accuracy_validation();
+
 
     // LOG(INFO) << "end";
     getchar();

wj_lidar/detect_wheel_ceres.cpp

@@ -413,7 +413,7 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
             }
             auto t1=std::chrono::system_clock::now();
 	    std::string t_error_info;
-            bool current_result = Solve(t_detect_result, t_loss_result, t_error_info, true);
+            bool current_result = Solve(t_detect_result, t_loss_result, t_error_info, false);
     	    error_info = t_error_info;
             auto t2=std::chrono::system_clock::now();
             auto duration=t2-t1;

wj_lidar/region_detect.cpp

@@ -42,7 +42,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
     pcl::copyPointCloud(*cloud_in, *cloud_out);
     if (cloud_out->size() <= 0)
         return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
-
+    // std::cout << "000 "<<cloud_out->size() << std::endl;
     // 2.直通滤波, 筛选xy
     pcl::PassThrough<pcl::PointXYZ> pass;
     pass.setInputCloud(cloud_out);
@@ -50,7 +50,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
     pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx()); //将x轴的0到1范围内
     pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
     pass.filter(*cloud_out);                                            //输出到结果指针
-
+// std::cout << "111 "<<cloud_out->size() << std::endl;
     if (cloud_out->size() <= 0)
         return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
 
@@ -59,7 +59,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
     pass.setFilterLimits(m_region_param.miny(), m_region_param.maxy()); //将x轴的0到1范围内
     pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
     pass.filter(*cloud_out);                                            //输出到结果指针
-
+// std::cout << "222 "<<cloud_out->size() << std::endl;
     if (cloud_out->size() <= 0)
         return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
 
@@ -67,10 +67,10 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
     sor.setInputCloud(cloud_out);
     sor.setMeanK(10);            //K近邻搜索点个数
-    sor.setStddevMulThresh(1.0); //标准差倍数
+    sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_out);      //保存滤波结果到cloud_filter
-
+// std::cout << "333" << std::endl;
     if (cloud_out->size() <= 0)
         return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
     else
@@ -476,6 +476,28 @@ Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud
     return Error_manager(Error_code::SUCCESS);
 }
 
+Error_manager Region_detector::find_circle(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y)
+{
+    Error_manager result = Error_manager(Error_code::SUCCESS);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    // 1.预处理
+    result = preprocess(cloud_in, cloud_filtered);
+    if (result != SUCCESS) {
+        return result;
+    }
+
+    std::vector<cv::Point2f> t_point_vec;
+    for (size_t i = 0; i < cloud_filtered->size(); i++)
+    {
+        t_point_vec.push_back(cv::Point2f(cloud_filtered->points[i].x, cloud_filtered->points[i].y));
+    }
+
+    cv::RotatedRect t_rec = cv::fitEllipse(t_point_vec);
+    x = t_rec.center.x;
+    y = t_rec.center.y;
+
+    return SUCCESS;
+}
 
 /**
  * 输出中心点、角度、轮距与宽度的四轮点云检测函数

wj_lidar/region_detect.h

@@ -62,6 +62,9 @@ public:
     //通过ceres检测中心,旋转,轴距,宽度,前轮旋转,增加异常信息传出
     Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,double& front_wheel_theta,
                          double &wheelbase, double &width, std::string &error_info, bool print=true);
+
+    // 临时添加拟合圆
+    Error_manager find_circle(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y);
     // 获得区域id
     int get_region_id();
 

wj_lidar/wj_716_lidar_protocol.cpp

@@ -1,6 +1,6 @@
 #include "wj_716_lidar_protocol.h"
 #include <iostream>
-
+#include "pcl/io/pcd_io.h"
 namespace wj_lidar
 {
 
@@ -20,6 +20,7 @@ Error_manager scan_transform(wj::wjLidarParams params, pcl::PointCloud<pcl::Poin
   float angle = params.angle_min();
   cloud_out->clear();
   // LOG(INFO) << "cloud size before transform: " <<cloud_in->size();
+  // LOG(WARNING) << params.transform().DebugString() << std::endl;
   for (size_t i = 0; i < cloud_in->size(); ++i)
   {
     const float first_echo = cloud_in->points[i].x;
@@ -32,7 +33,7 @@ Error_manager scan_transform(wj::wjLidarParams params, pcl::PointCloud<pcl::Poin
             e_transform<<mat,1.0;
             Eigen::MatrixXf  new_pos=tf_btol*e_transform;*/
       float x = mat(0, 0);
-      float y = mat(1, 0);
+      float y = -mat(1, 0); // changed by yct 210430
 
       if (x < params.scan_limit().minx() || x > params.scan_limit().maxx() || y < params.scan_limit().miny() || y > params.scan_limit().maxy())
       {
@@ -45,9 +46,12 @@ Error_manager scan_transform(wj::wjLidarParams params, pcl::PointCloud<pcl::Poin
       cloud_out->push_back(point);
       // cloud_in->points[i].x = newx;
       // cloud_in->points[i].y = newy;
+      // LOG(WARNING) << x << ", " << y << "---" << newx << ", " << newy <<", " << std::endl;
     }
     angle += params.angle_increment();
   }
+  // pcl::PCDWriter t_writer;
+  // t_writer.write(params.net_config().ip_address() + ".pcd", *cloud_out);
   return Error_manager(Error_code::SUCCESS);
 }