add back border detection,

re-organize border msg and error string

message/measure_message.proto

@@ -129,10 +129,7 @@ enum Ground_statu
     Nothing=0;
     Noise=1;
     Car_correct=2;
-    Car_left_out=3;
-    Car_right_out=4;
-    Car_top_out=5;
-    Car_bottom_out=6;
+    Car_border_reached=3;
 }
 
 message Ground_status_msg
@@ -144,10 +141,11 @@ message Ground_status_msg
     repeated Wanji_lidar_device_status  wanji_lidar_device_status = 4;  //万集设备身状态
     required Region_worker_status       region_worker_status = 5;       //万集区域功能的状态
     required Locate_information         locate_information_realtime = 6;//地面雷达的 实时定位信息
-    required Ground_statu               ground_status=7; // 电子围栏状态
+    required int32               ground_status=7; // 电子围栏状态
+    required int32               border_status=8; // 超界状态,为0表示正常，从末尾开始123456位分别代表前、后、左、右、底盘、车高超界
 
-    required Error_manager              error_manager = 8;
-    repeated Cloud_coordinate           cloud=9;     //点云坐标
+    required Error_manager              error_manager = 9;
+    repeated Cloud_coordinate           cloud=10;     //点云坐标
 }
 
 

setting/velodyne_manager.prototxt

@@ -94,6 +94,7 @@ region
     border_maxx:1.3
     plc_offsetx:1.913
     plc_offsety:-6.078
+    plc_border_miny:-8.0
     lidar_exts
     {
         lidar_id:6
@@ -136,6 +137,7 @@ region
     border_maxx:1.3
     plc_offsetx:-1.8783
     plc_offsety:-6.11489
+    plc_border_miny:-8.0
     # 4-->5: -3.81895 0.038776
     lidar_exts
     {

setting/velodyne_manager_chutian.prototxt

@@ -86,14 +86,15 @@ region
 	miny:-2.6
 	maxy:2.6
 	minz:0.025
-	maxz:1.0
+	maxz:0.5
     region_id:5
     turnplate_cx:0.0
     turnplate_cy:0.0
     border_minx:-1.3
     border_maxx:1.3
     plc_offsetx:1.913
-    plc_offsety:5.998
+    plc_offsety:-6.078
+    plc_border_miny:-8.0
     lidar_exts
     {
         lidar_id:6
@@ -126,16 +127,17 @@ region
     minx:-1.5
 	maxx:1.5
 	miny:-2.6
-	maxy:2.0
+	maxy:2.3
 	minz:0.03
-	maxz:1.0
+	maxz:0.5
     region_id:4
     turnplate_cx:0.0
     turnplate_cy:0.0
     border_minx:-1.3
     border_maxx:1.3
-    plc_offsetx:-1.8983
-    plc_offsety:6.03489
+    plc_offsetx:-1.8783
+    plc_offsety:-6.11489
+    plc_border_miny:-8.0
     # 4-->5: -3.81895 0.038776
     lidar_exts
     {
@@ -155,4 +157,4 @@ region
             cy:0.019388
         }
     }
-}
+}

system/system_executor.cpp

@@ -318,20 +318,46 @@ Error_manager System_executor::encapsulate_send_status()
 		t_locate_information.set_uniformed_car_y(t_car_wheel_information.uniform_car_y);
 		t_multi_status_msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information);
 		// 当前超界提示仅保留一项
+		// 20211026已修改，分离为电子围栏状态与超界状态，只有测量正确时超界状态才有意义
+		t_multi_status_msg.set_border_status(t_car_wheel_information.range_status);
 		if(!t_car_wheel_information.correctness)
 		{
 			if(t_region_cloud->size() > 0)
 				t_multi_status_msg.set_ground_status(message::Ground_statu::Noise);
 			else
 				t_multi_status_msg.set_ground_status(message::Ground_statu::Nothing);
-		}else if(t_car_wheel_information.range_status & Ground_region::Range_status::Range_left != 0)
+		}else if(t_car_wheel_information.range_status == int(Ground_region::Range_status::Range_correct))
 		{
-			t_multi_status_msg.set_ground_status(message::Ground_statu::Car_left_out);
-		}else if(t_car_wheel_information.range_status & Ground_region::Range_status::Range_right != 0)
-		{
-			t_multi_status_msg.set_ground_status(message::Ground_statu::Car_right_out);
-		}else{
 			t_multi_status_msg.set_ground_status(message::Ground_statu::Car_correct);
+		}else {
+			t_multi_status_msg.set_ground_status(message::Ground_statu::Car_border_reached);
+			// 更新待提示错误信息
+			std::string t_error_str;
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_front != 0)
+			{
+				t_error_str.append("前超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_back != 0)
+			{
+				t_error_str.append("后超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_left != 0)
+			{
+				t_error_str.append("左超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_right != 0)
+			{
+				t_error_str.append("右超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_bottom != 0)
+			{
+				t_error_str.append("底盘超界 ");
+			}
+			if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_top != 0)
+			{
+				t_error_str.append("顶超界 ");
+			}
+			t_error.set_error_description(t_error_str);
 		}
 
 		t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());

velodyne_lidar/ground_region.cpp

@@ -571,6 +571,27 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     return res;
 }
 
+int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Common_data::Car_wheel_information measure_result, double plate_cx, double plate_cy, double theta)
+{
+    int res = Range_status::Range_correct;
+    if(cloud->size() <= 0)
+        return res;
+
+    Eigen::Vector2d car_center(measure_result.car_center_x, measure_result.car_center_y);
+    Eigen::Vector2d car_uniform_center;
+    double theta_rad = theta * M_PI / 180.0;
+    car_uniform_center.x() = car_center.x() * cos(theta_rad) - car_center.y() * sin(theta_rad);
+    car_uniform_center.y() = car_center.x() * sin(theta_rad) + car_center.y() * cos(theta_rad);
+    // 车位位于y负方向
+    double rear_wheel_y = car_uniform_center.y() + m_region.plc_offsety() - 0.5 * measure_result.car_wheel_base;
+    if(rear_wheel_y < m_region.plc_border_miny())
+    {
+        res |= Range_status::Range_back;
+    }
+    res |= outOfRangeDetection(cloud, plate_cx, plate_cy, theta);
+    return res;
+}
+
 //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
 Error_manager Ground_region::get_current_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information, std::chrono::system_clock::time_point command_time)
 {
@@ -677,7 +698,7 @@ void Ground_region::thread_measure_func()
                 // 超界校验
                 {
                     std::lock_guard<std::mutex> lck(m_filtered_cloud_mutex);
-                    int res = outOfRangeDetection(mp_cloud_filtered, m_region.turnplate_cx(), m_region.turnplate_cy(), 90.0 - t_result.theta);
+                    int res = outOfRangeDetection(mp_cloud_filtered, m_car_wheel_information, m_region.turnplate_cx(), m_region.turnplate_cy(), 90.0 - t_result.theta);
                     m_car_wheel_information.range_status = res;
                 }
                 // 添加plc偏移

velodyne_lidar/ground_region.h

@@ -48,8 +48,12 @@ public:
    enum Range_status
    {
       Range_correct = 0x0000, // 未超界
-      Range_left = 0x0001,    // 左超界
-      Range_right = 0x0002,   // 右超界
+      Range_front = 0x0001, //前超界
+      Range_back = 0x0002, //后超界
+      Range_left = 0x0004,    // 左超界
+      Range_right = 0x0008,   // 右超界
+      Range_bottom = 0x0010, //底盘超界
+      Range_top = 0x0020, // 车顶超界
    };
 
 #define GROUND_REGION_DETECT_CYCLE_MS 150
@@ -147,6 +151,9 @@ private:
    // 超界判断，旋转单位(deg)，默认无旋转
    int outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0);
 
+   // 超界判断，增加对后轮的判断
+   int outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Common_data::Car_wheel_information measure_result, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0);
+
 private:
    velodyne::Region m_region; // 区域配置
    detect_wheel_ceres3d *m_detector; // 检测器

velodyne_lidar/velodyne_config.proto

@@ -59,4 +59,5 @@ message Region
     required float border_maxx=12; // 最大边界x，右超界提示
     required float plc_offsetx=13; // plc偏移x
     required float plc_offsety=14; // plc偏移y
+    required float plc_border_miny=15;// plc后夹持y方向极限值
 }