lidar0 region4 region0 config changes.

add steering wheel range check

message/measure_message.proto

@@ -143,7 +143,7 @@ message Ground_status_msg
     required Locate_information         locate_information_realtime = 6;//地面雷达的 实时定位信息
     required Ground_statu               ground_status=7; // 电子围栏状态
     // 超界状态,为0表示正常，从末尾开始123456位分别代表前、后、左、右、底盘、车高超界
-    // 增加超宽、超轴距、左(逆时针)旋转超界，右(顺时针)旋转超界判断，分别处于7 8 9 10四个位
+    // 增加超宽、超轴距、左(逆时针)旋转超界，右(顺时针)旋转超界，前轮角超界判断，分别处于7 8 9 10 11四个位
     required int32               	border_status=8; 
 
     required Error_manager              error_manager = 9;

setting/velodyne_manager.prototxt

@@ -127,6 +127,7 @@ velodyne_lidars
 }
 
 # 0 lidar 192.168.1.207
+# p:0.68354 - 0.4092
 velodyne_lidars
 {
     ip:""
@@ -142,7 +143,8 @@ velodyne_lidars
     calib
     {
         r:0.03026
-        p:0.68354
+        # p:0.68354
+        p:0
         y:90.6538
         cz:0.07697
     }
@@ -202,6 +204,7 @@ region
 # plc_4 = plc_5 + calib5in5 - calib5in4
 # plc_4x = 1.913 + (-1.9018) - (1.9095) = -1.8983
 # plc_4y = 5.998 + (0.0175) - (-0.019388) = 6.03489
+# 20211209 x+(1cm)
 region
 {
     minx:-1.5
@@ -215,7 +218,7 @@ region
     turnplate_cy:0.0
     border_minx:-1.3
     border_maxx:1.3
-    plc_offsetx:-1.8783
+    plc_offsetx:-1.8683
     plc_offsety:-6.11489
     plc_offset_degree:-89.5
     plc_border_miny:-7.51
@@ -360,7 +363,7 @@ region
     border_maxx:1.3
     plc_offsetx:-1.86
     plc_offsety:-6.1368
-    plc_offset_degree:-90
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.95

system/system_executor.cpp

@@ -359,6 +359,27 @@ Error_manager System_executor::encapsulate_send_status()
 			{
 				t_error_str.append("顶超界 ");
 			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_car_width != 0)
+			{
+				t_error_str.append("车宽超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_car_wheelbase != 0)
+			{
+				t_error_str.append("轴距超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_angle_anti_clock != 0)
+			{
+				t_error_str.append("车辆角度左超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_angle_clock != 0)
+			{
+				t_error_str.append("车辆角度右超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_steering_wheel_nozero != 0)
+			{
+				t_error_str.append("车辆前轮角超界 ");
+			}
+			
 			t_error.set_error_description(t_error_str);
 		}
 

system/system_executor.h

@@ -10,7 +10,7 @@
 #include "../error_code/error_code.h"
 #include "../communication/communication_message.h"
 
-#define DISP_TERM_ID 1
+#define DISP_TERM_ID -1
 
 class System_executor:public Singleton<System_executor>
 {

velodyne_lidar/ground_region.cpp

@@ -516,18 +516,17 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 
     if (results.size() == 0)
     {
-        // // 20201010, may lead to problem in chutian, uncomment in debug only
-        // // changed by yct, save 3d wheel detect result. 
-        // static int save_debug = 0;
-        // if (save_debug++ == 5)
-        //     m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
-
         // std::cout << "\n-------- no result: " << std::endl;
         //LOG_IF(INFO, m_region.region_id() == 4) << "failed with midz, currz: " << mid_z << ", " << chassis_z;
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "3d wheel detect failed.");
     }
     else
     {
+        // // 20201010, may lead to problem in chutian, uncomment in debug only
+        // // changed by yct, save 3d wheel detect result. 
+        // static int save_debug = 0;
+        // if (m_region.region_id() == 0 && save_debug++ == 5)
+        //     m_detector->save_debug_data("/home/zx/yct/chutian_measure_2021/build");
         // LOG_IF(INFO, m_region.region_id() == 4) << "detected with suitable z value: " << chassis_z;
     }
     ///  to be
@@ -641,6 +640,11 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     {
         res |= Range_status::Range_angle_anti_clock;
     }
+    // // 判断车辆前轮角回正情况
+    // if (fabs(measure_result.car_front_theta) > 8.0)
+    // {
+    //     res |= Range_status::Range_steering_wheel_nozero;
+    // }
     res |= outOfRangeDetection(cloud, plate_cx, plate_cy, theta);
     return res;
 }
@@ -771,6 +775,12 @@ void Ground_region::thread_measure_func()
                 if (ec == SUCCESS)
                 {
                     m_car_wheel_information.car_wheel_base = t_wheel_info_stamped.wheel_data.car_wheel_base;
+                    m_car_wheel_information.car_front_theta = t_wheel_info_stamped.wheel_data.car_front_theta;
+                    // 临时添加，滤波后前轮超界
+                    if(fabs(m_car_wheel_information.car_front_theta)>8.0)
+                    {
+                        m_car_wheel_information.range_status |= Range_status::Range_steering_wheel_nozero;
+                    }
                 }
                 // else{
                 //     std::cout<<ec.to_string()<<std::endl;

velodyne_lidar/ground_region.h

@@ -58,6 +58,7 @@ public:
       Range_car_wheelbase = 0x0080, // 轴距超界
       Range_angle_anti_clock = 0x0100, // 左(逆时针)旋转超界
       Range_angle_clock = 0x0200, // 右(顺时针)旋转超界
+      Range_steering_wheel_nozero = 0x0400, // 方向盘未回正
    };
 
 #define GROUND_REGION_DETECT_CYCLE_MS 150