empty car movement queue debug

setting/velodyne_manager.prototxt

@@ -171,7 +171,7 @@ region
     plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
-    car_max_width:1.95
+    car_max_width:1.92
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -223,7 +223,7 @@ region
     plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
-    car_max_width:1.95
+    car_max_width:1.92
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -271,7 +271,7 @@ region
 #     plc_offset_degree:0.0
 #     plc_border_miny:-7.51
 #     car_min_width:1.55
-#     car_max_width:1.95
+#     car_max_width:1.92
 #     car_min_wheelbase:2.3
 #     car_max_wheelbase:3.15
 #     turnplate_angle_limit_anti_clockwise:5.3
@@ -318,7 +318,7 @@ region
     plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
-    car_max_width:1.95
+    car_max_width:1.92
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -366,7 +366,7 @@ region
     plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
-    car_max_width:1.95
+    car_max_width:1.92
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3

velodyne_lidar/region_status_checker.h

@@ -30,7 +30,7 @@
 // #include "opencv2/opencv.hpp"
 #include "../tool/point_tool.h"
 
-#define DEFAULT_REGION_STATUS_TIMEOUT_MILLI 30000
+#define DEFAULT_REGION_STATUS_TIMEOUT_MILLI 600000
 #define DEFAULT_REGION_STATUS_MAX_QUEUE_SIZE 600
 // 检测到静止后稳定时间
 #define MIN_STABLE_TIME_MILLI 1000
@@ -45,7 +45,16 @@ class Region_status_checker : public Singleton<Region_status_checker>
 struct Region_measure_info_stamped
 {
     Common_data::Car_wheel_information_stamped measure_result;
-    pcl::PointCloud<pcl::PointXYZ> cloud;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud;
+    Region_measure_info_stamped()
+    {
+        p_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    }
+
+    // ~Region_measure_info_stamped()
+    // {
+    //     p_cloud->clear();
+    // }
 };
 
 public:
@@ -93,7 +102,7 @@ public:
         std::vector<int> index;
         // cloud.is_dense = false;
         // pcl::removeNaNFromPointCloud(cloud, data.cloud, index);
-        data.cloud = cloud;
+        data.p_cloud->operator+=(cloud);
         if (!data.measure_result.wheel_data.correctness)
             return;
 //        LOG(INFO) << data.wheel_data.to_string();
@@ -126,7 +135,7 @@ public:
             return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("当前icp空指针 终端") + std::to_string(terminal_id)).c_str());
         }
 
-        if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end())
+        if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end() || m_measure_results_map[terminal_id].size() == 0)
         {
             return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("暂无最近历史数据用于推测当前状态 终端") + std::to_string(terminal_id)).c_str());
         }
@@ -138,7 +147,7 @@ public:
         {
             return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("最近历史数据超时 终端") + std::to_string(terminal_id)).c_str());
         }
-
+// std::cout<<"000000"<<std::endl;
         // 截取当前帧框内与框外点云
         Eigen::Vector2d t_last_center(
             last_data_ref.measure_result.wheel_data.car_center_x,
@@ -154,17 +163,17 @@ public:
         // 计算历史(模板)框内点云与框外点云
         pcl::PointCloud<pcl::PointXYZ>::Ptr t_prev_cloud_inside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
         pcl::PointCloud<pcl::PointXYZ>::Ptr t_prev_cloud_outside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-        for (size_t i = 0; i < last_data_ref.cloud.size(); i++)
+        for (size_t i = 0; i < last_data_ref.p_cloud->size(); i++)
         {
-            Eigen::Vector2d t_point(last_data_ref.cloud[i].x - t_last_center.x(), last_data_ref.cloud[i].y - t_last_center.y());
+            Eigen::Vector2d t_point(last_data_ref.p_cloud->points[i].x - t_last_center.x(), last_data_ref.p_cloud->points[i].y - t_last_center.y());
             Eigen::Vector2d t_rot_point = t_rot_mat * t_point;
             if (t_rot_point.x() > -t_width_height.x() / 2.0 && t_rot_point.x() < t_width_height.x() / 2.0 && t_rot_point.y() > -t_width_height.y() / 2.0 && t_rot_point.y() < t_width_height.y() / 2.0)
             {
-                t_prev_cloud_inside->push_back(last_data_ref.cloud[i]);
+                t_prev_cloud_inside->push_back(last_data_ref.p_cloud->points[i]);
             }
             else
             {
-                t_prev_cloud_outside->push_back(last_data_ref.cloud[i]);
+                t_prev_cloud_outside->push_back(last_data_ref.p_cloud->points[i]);
             }
 
             // if(std::isfinite(last_data_ref.cloud[i].x) && std::isfinite(last_data_ref.cloud[i].y) && std::isfinite(last_data_ref.cloud[i].z))
@@ -175,7 +184,7 @@ public:
 
             // }
         }
-
+// std::cout<<"111111"<<std::endl;
         // 计算当前帧在框内与框外点云
         pcl::PointCloud<pcl::PointXYZ>::Ptr t_curr_cloud_inside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
         pcl::PointCloud<pcl::PointXYZ>::Ptr t_curr_cloud_outside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
@@ -203,9 +212,11 @@ public:
 
             // }
         }
+// std::cout<<"222222"<<std::endl;
         if(t_prev_cloud_inside->size() <= 30 || t_curr_cloud_inside->size() <= 30)
         {
             char buf[255];
+            memset(buf, 0, 255);
             sprintf(buf, ",, prev %d, curr %d;; last center(%.3f, %.3f)", t_prev_cloud_inside->size(), t_curr_cloud_inside->size(), t_last_center.x(), t_last_center.y());
             return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("区域剪切后无点可用于匹配 终端") + std::to_string(terminal_id)+std::string(buf)).c_str());
         }
@@ -220,34 +231,37 @@ public:
         
         // icp计算前后内点匹配情况
         //下采样
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_prev_cloud_inside_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_curr_cloud_inside_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
         pcl::VoxelGrid<pcl::PointXYZ> vox;    //创建滤波对象
         vox.setInputCloud(t_prev_cloud_inside);         //设置需要过滤的点云给滤波对象
         vox.setLeafSize(0.03f, 0.03f, 0.03f); //设置滤波时创建的体素体积为1cm的立方体
-        vox.filter(*t_prev_cloud_inside);             //执行滤波处理，存储输出
+        vox.filter(*t_prev_cloud_inside_filtered);             //执行滤波处理，存储输出
 
         vox.setInputCloud(t_curr_cloud_inside);         //设置需要过滤的点云给滤波对象
         vox.setLeafSize(0.03f, 0.03f, 0.03f); //设置滤波时创建的体素体积为1cm的立方体
-        vox.filter(*t_curr_cloud_inside);             //执行滤波处理，存储输出
+        vox.filter(*t_curr_cloud_inside_filtered);             //执行滤波处理，存储输出
 
-        mp_icp_svd->SetInputTarget(t_prev_cloud_inside);
+        mp_icp_svd->SetInputTarget(t_prev_cloud_inside_filtered);
         
 
         // static int count = 0;
         // if(count++ == 20)
         // {
-        //     save_cloud_txt(t_prev_cloud_inside, "./t_prev_cloud.txt");
-        //     save_cloud_txt(t_curr_cloud_inside, "./t_curr_cloud.txt");
+        //     save_cloud_txt(t_prev_cloud_inside_filtered, "./t_prev_cloud.txt");
+        //     save_cloud_txt(t_curr_cloud_inside_filtered, "./t_curr_cloud.txt");
         // }
 
         // std::cout << "111" << std::endl;
         pcl::PointCloud<pcl::PointXYZ>::Ptr t_result_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
         Eigen::Matrix4f t_pred_pose, t_res_pose;
         t_pred_pose.setIdentity();
-        if (!mp_icp_svd->ScanMatch(t_curr_cloud_inside, t_pred_pose, t_result_cloud, t_res_pose))
+        if (!mp_icp_svd->ScanMatch(t_curr_cloud_inside_filtered, t_pred_pose, t_result_cloud, t_res_pose))
         {
             return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("icp匹配失败，终端") + std::to_string(terminal_id)).c_str());
         }
 
+        // std::cout << "222" << std::endl;
         if(is_significant(t_res_pose, dtime / 1000.0))
         {
             mb_is_stable = false;