21-12-7 现场 车高约束调整，仅计算车身附近点云整体高度，排除栏杆拖影影响

locate/locater.cpp

@@ -350,16 +350,6 @@ Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src,
         return code;
     }
 
-    //第三步,计算车辆高度
-    // changed by yct, 传入完整点云而非仅车身点云
-    float height_car=0;
-    code=measure_height(cloud_in,height_car);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    locate_information.locate_hight=height_car;
-
     //第四步,识别轮胎,并根据轮胎数量计算旋转矩形
     float center_x;
     float center_y;
@@ -372,6 +362,55 @@ Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src,
         return code;
     }
 
+    //第三步,计算车辆高度
+    // changed by yct, 传入完整点云而非仅车身点云
+    // 20211207 changed by yct, 点云除去栏杆附近点计算高度
+    float height_car=0;
+    float height_car_filtered=0;
+    // 计算x范围
+    std::vector<Eigen::Vector2d> t_wheel_centers;
+    float height_calc_offsetx = 100;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car_height(new pcl::PointCloud<pcl::PointXYZ>);
+    t_wheel_centers.push_back(Eigen::Vector2d(-(width+height_calc_offsetx)/2.0, wheel_base/2.0));
+    t_wheel_centers.push_back(Eigen::Vector2d((width+height_calc_offsetx)/2.0, wheel_base/2.0));
+    t_wheel_centers.push_back(Eigen::Vector2d(-(width+height_calc_offsetx)/2.0, -wheel_base/2.0));
+    t_wheel_centers.push_back(Eigen::Vector2d((width+height_calc_offsetx)/2.0, -wheel_base/2.0));
+    Eigen::Rotation2Dd t_rot((angle-90.0)*M_PI/180.0);
+    double min_x=INT_MAX, max_x = 0.0;
+    for (int i = 0; i < t_wheel_centers.size(); ++i) {
+        t_wheel_centers[i] = t_rot.toRotationMatrix() * t_wheel_centers[i];
+        if(min_x>t_wheel_centers[i].x())
+        {
+            min_x = t_wheel_centers[i].x();
+        }
+        if(max_x<t_wheel_centers[i].x())
+        {
+            max_x = t_wheel_centers[i].x();
+        }
+    }
+    if(min_x<-width || max_x>width)
+    {
+        LOG(WARNING) << "min max x calc for height filter error, minx maxx: "<<min_x<<", "<<max_x;
+        return Error_manager(LOCATER_MEASURE_HEIGHT_OUT_RANGE,NORMAL,"min max x calc error");
+    }
+    LOG(WARNING) << "min max x calc, minx maxx: "<<min_x+center_x<<", "<<max_x+center_x<<",,width: "<<width;
+    for (int j = 0; j < cloud_in->size(); ++j) {
+        if(cloud_in->points[j].x > (min_x+center_x) && cloud_in->points[j].x < (max_x+center_x))
+        {
+            cloud_car_height->push_back(cloud_in->points[j]);
+        }
+    }
+
+    // 采用切除栏杆后点云计算车高
+    code=measure_height(cloud_car_height,height_car_filtered);
+    if(code!=SUCCESS)
+    {
+        return code;
+    }
+    locate_information.locate_hight=height_car_filtered;
+    measure_height(cloud_in,height_car);
+    LOG(WARNING)<< "height compare, origin---filtered: "<<height_car<<", "<<height_car_filtered<<"..cloud size:"<<cloud_car_height->size();
+
     //第六步,结果赋值
     //角度以度为单位
     locate_information.locate_x=center_x;

terminor/terminal_command_executor.cpp

@@ -540,9 +540,10 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     // 20210527 added by yct
     // 6号位标定地面偏低，高度增加21mm
     // 20210603 2号位加高10mm, 4号位加高5mm
+    // 20210925 6号位加高改为13
     if(m_terminor_parameter.id()==5)
     {
-        m_measure_information.locate_hight += 21;
+        m_measure_information.locate_hight += 13;
     }
     else if(m_terminor_parameter.id()==3)
     {