1、测量添加终端相关状态，并通过rabbitmq对外发送；

2、rabbitmq配置文件添加对外发送队列；
3、添加.gitignore文件，排除protobuf生成文件；
4、删除git上protobuf生成文件；

测量节点/.gitignore

@@ -0,0 +1,7 @@
+# -----> protobuf
+*.pb.cc
+*.pb.h
+
+# -----> build
+*build*
+

测量节点/proto.sh

@@ -8,6 +8,7 @@ protoc -I=./message message_base.proto --cpp_out=./message
  # @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 ### 
 protoc -I=./message measure_message.proto --cpp_out=./message
+protoc -I=./message message.proto --cpp_out=./message
 
 #protoc -I=./locate locate_parameter.proto --cpp_out=./locate
 #protoc -I=./laser laser_parameter.proto --cpp_out=./laser

测量节点/setting/rabbitmq.prototxt

@@ -1,12 +1,12 @@
-
-
 rabbitmq_parameters
 {
-    ip:"192.168.1.233"
+    # ip:"192.168.1.233"
+    ip:"127.0.0.1"
     port:5672
     user:"zx"
     password:"zx123456"
 
+    # 调度节点1-用于接收plc变更旋转角度的最大最小值
     rabbitmq_reciever_vector
     {
        channel:401
@@ -25,6 +25,7 @@ rabbitmq_parameters
        consume_exclusive:0
     }
 
+    # 调度节点2-用于接收plc变更旋转角度的最大最小值
     rabbitmq_reciever_vector
     {
        channel:402
@@ -43,6 +44,7 @@ rabbitmq_parameters
        consume_exclusive:0
     }
 
+    # 调度节点3-用于接收plc变更旋转角度的最大最小值
     rabbitmq_reciever_vector
     {
        channel:403
@@ -61,7 +63,7 @@ rabbitmq_parameters
        consume_exclusive:0
     }
 
-    #终端点击存车时, 发送存车表单给检查节点, 这里顺便发送给感测节点.
+    # 终端点击存车时, 发送存车表单给检查节点, 这里顺便发送给感测节点, 用于存储点云
     rabbitmq_reciever_vector
      {
          channel:401
@@ -79,8 +81,7 @@ rabbitmq_parameters
          consume_exclusive:0
      }
 
-
-
+    # 发送plc的测量结果
     rabbitmq_sender_status_vector
     {
         channel:411
@@ -123,5 +124,49 @@ rabbitmq_parameters
         routing_key:"measure_6_statu_port"
         timeout_ms:0
     }
+
+    # 发送终端的测量结果
+    rabbitmq_sender_status_vector
+    {
+        channel:411
+        exchange_name:"statu_ex"
+        routing_key:"measure_1_terminal_statu_port"
+        timeout_ms:0
+    }
+    rabbitmq_sender_status_vector
+    {
+        channel:411
+        exchange_name:"statu_ex"
+        routing_key:"measure_2_terminal_statu_port"
+        timeout_ms:0
+    }
+    rabbitmq_sender_status_vector
+    {
+        channel:411
+        exchange_name:"statu_ex"
+        routing_key:"measure_3_terminal_statu_port"
+        timeout_ms:0
+    }
+    rabbitmq_sender_status_vector
+    {
+        channel:411
+        exchange_name:"statu_ex"
+        routing_key:"measure_4_terminal_statu_port"
+        timeout_ms:0
+    }
+    rabbitmq_sender_status_vector
+    {
+        channel:411
+        exchange_name:"statu_ex"
+        routing_key:"measure_5_terminal_statu_port"
+        timeout_ms:0
+    }
+    rabbitmq_sender_status_vector
+    {
+        channel:411
+        exchange_name:"statu_ex"
+        routing_key:"measure_6_terminal_statu_port"
+        timeout_ms:0
+    }
 }
 

测量节点/setting/velodyne_manager.prototxt

@@ -187,19 +187,35 @@ region
     region_id:5
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:1.645
-    border_maxx:2.090
     plc_offsetx:1.913
     plc_offsety:-6.078
     plc_offset_degree:-89.5
-    plc_border_miny:1.175
-    plc_border_maxy:1.255
-    car_min_width:1.55
-    car_max_width:1.920
-    car_min_wheelbase:2.3
-    car_max_wheelbase:3.15
-    turnplate_angle_limit_anti_clockwise:5.3
-    turnplate_angle_limit_clockwise:5.3
+
+    plc_out_of_region {
+        border_minx:1.645
+        border_maxx:2.090
+        plc_border_miny:1.175
+        plc_border_maxy:1.255
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
+
+    terminal_out_of_region {
+        border_minx:1.645
+        border_maxx:2.090
+        plc_border_miny:1.175
+        plc_border_maxy:1.255
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
 
     lidar_exts
     {
@@ -244,20 +260,36 @@ region
     region_id:4
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-2.220
-    border_maxx:-1.630
     # -1.8683
     plc_offsetx:-1.865
     plc_offsety:-6.11489
     plc_offset_degree:-89.5
-    plc_border_miny:1.175
-    plc_border_maxy:1.255
-    car_min_width:1.55
-    car_max_width:1.920
-    car_min_wheelbase:2.3
-    car_max_wheelbase:3.15
-    turnplate_angle_limit_anti_clockwise:5.3
-    turnplate_angle_limit_clockwise:5.3
+
+    plc_out_of_region {
+        border_minx:-2.220
+        border_maxx:-1.630
+        plc_border_miny:1.175
+        plc_border_maxy:1.255
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
+
+    terminal_out_of_region {
+        border_minx:-2.220
+        border_maxx:-1.630
+        plc_border_miny:1.175
+        plc_border_maxy:1.255
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
 
     # 4-->5: -3.81895 0.038776
     lidar_exts
@@ -293,19 +325,35 @@ region
     region_id:3
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:1.615
-    border_maxx:2.120
     plc_offsetx:1.926
     plc_offsety:-6.135
     plc_offset_degree:-89.5
-    plc_border_miny:1.175
-    plc_border_maxy:1.255
-    car_min_width:1.55
-    car_max_width:1.920
-    car_min_wheelbase:2.3
-    car_max_wheelbase:3.15
-    turnplate_angle_limit_anti_clockwise:5.3
-    turnplate_angle_limit_clockwise:5.3
+
+    plc_out_of_region {
+        border_minx:1.615
+        border_maxx:2.120
+        plc_border_miny:1.175
+        plc_border_maxy:1.255
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
+
+    terminal_out_of_region {
+        border_minx:1.615
+        border_maxx:2.120
+        plc_border_miny:1.175
+        plc_border_maxy:1.255
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
 
     # 3-->4: -3.80394 (-0.01385+0.02)
     lidar_exts
@@ -341,21 +389,37 @@ region
     region_id:2
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-2.190
-    border_maxx:-1.635
     # 0.02
     plc_offsetx:-1.871
     # -0.34
     plc_offsety:-6.135
     plc_offset_degree:-89.1
-    plc_border_miny:1.175
-    plc_border_maxy:1.255
-    car_min_width:1.55
-    car_max_width:1.920
-    car_min_wheelbase:2.3
-    car_max_wheelbase:3.15
-    turnplate_angle_limit_anti_clockwise:5.3
-    turnplate_angle_limit_clockwise:5.3
+
+    plc_out_of_region {
+        border_minx:-2.190
+        border_maxx:-1.635
+        plc_border_miny:1.175
+        plc_border_maxy:1.255
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
+
+    terminal_out_of_region {
+        border_minx:-2.190
+        border_maxx:-1.635
+        plc_border_miny:1.175
+        plc_border_maxy:1.255
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
 
     # 2-->3: -3.87778 (-0.052905-0.025)
     # 20220215 宽度调整 x-0.02
@@ -395,19 +459,35 @@ region
     region_id:1
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:1.640
-    border_maxx:2.155
     plc_offsetx:1.93
     plc_offsety:-6.1368
     plc_offset_degree:-89.5
-    plc_border_miny:1.175
-    plc_border_maxy:1.265
-    car_min_width:1.55
-    car_max_width:1.920
-    car_min_wheelbase:2.3
-    car_max_wheelbase:3.15
-    turnplate_angle_limit_anti_clockwise:5.3
-    turnplate_angle_limit_clockwise:5.3
+
+    plc_out_of_region {
+        border_minx:1.640
+        border_maxx:2.155
+        plc_border_miny:1.175
+        plc_border_maxy:1.265
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
+
+    terminal_out_of_region {
+        border_minx:1.640
+        border_maxx:2.155
+        plc_border_miny:1.175
+        plc_border_maxy:1.265
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
 
     # 2-->1: 3.82222438  -0.106852
     # 20220215 宽度调整 x-0.04
@@ -451,19 +531,35 @@ region
     region_id:0
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-2.155
-    border_maxx:-1.655
     plc_offsetx:-1.86
     plc_offsety:-6.1368
     plc_offset_degree:-89.5
-    plc_border_miny:1.175
-    plc_border_maxy:1.265
-    car_min_width:1.55
-    car_max_width:1.920
-    car_min_wheelbase:2.3
-    car_max_wheelbase:3.15
-    turnplate_angle_limit_anti_clockwise:5.3
-    turnplate_angle_limit_clockwise:5.3
+
+    plc_out_of_region {
+        border_minx:-2.155
+        border_maxx:-1.655
+        plc_border_miny:1.175
+        plc_border_maxy:1.265
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
+
+    terminal_out_of_region {
+        border_minx:-2.155
+        border_maxx:-1.655
+        plc_border_miny:1.175
+        plc_border_maxy:1.265
+        car_min_width:1.55
+        car_max_width:1.920
+        car_min_wheelbase:2.3
+        car_max_wheelbase:3.15
+        turnplate_angle_limit_anti_clockwise:5.3
+        turnplate_angle_limit_clockwise:5.3
+    }
 
     # 1-->0: 3.8173811 -0.0273465
     # 20220215 宽度调整 x-0.015

测量节点/system/system_executor.cpp

@@ -220,6 +220,20 @@ Error_manager System_executor::check_status()
 							 " System_executor::check_status error ");
 	}
 }
+int System_executor::transMeasureStatus(const int &measure_status) {
+    switch (measure_status) {
+        case message::Ground_statu::Car_correct:
+            return 0;
+        case message::Ground_statu::Nothing:
+            return 1;
+        case message::Ground_statu::Noise:
+            return 2;
+        case message::Ground_statu::Car_border_reached:
+            return 3;
+        default:
+            return 2;
+    }
+}
 
 // 更新消息中关于定位结果与超界信息的内容
 bool System_executor::update_measure_info(message::Ground_status_msg &msg, Common_data::Car_wheel_information &measure_info, int cloud_size)
@@ -313,6 +327,98 @@ bool System_executor::update_measure_info(message::Ground_status_msg &msg, Commo
 	msg.mutable_error_manager()->set_error_description(t_error.get_error_description());
 	return true;
 }
+// 更新消息中关于定位结果与超界信息的内容
+bool System_executor::update_terminal_measure_info(message::Ground_status_msg &msg, Common_data::Car_wheel_information &measure_info, int cloud_size)
+{
+    Error_manager t_error;
+    //无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确.
+    message::Locate_information t_locate_information;
+    //= t_multi_status_msg.add_locate_information_realtime();
+    t_locate_information.set_locate_x(measure_info.car_center_x);
+    t_locate_information.set_locate_y(measure_info.car_center_y);
+    t_locate_information.set_locate_angle(measure_info.car_angle);
+    t_locate_information.set_locate_length(0);
+    t_locate_information.set_locate_width(0);
+    t_locate_information.set_locate_height(0);
+    t_locate_information.set_locate_wheel_base(measure_info.car_wheel_base);
+    t_locate_information.set_locate_wheel_width(measure_info.car_wheel_width);
+    t_locate_information.set_locate_front_theta(measure_info.car_front_theta);
+    t_locate_information.set_locate_correct(measure_info.correctness);
+    t_locate_information.set_uniformed_car_x(measure_info.uniform_car_x);
+    t_locate_information.set_uniformed_car_y(measure_info.uniform_car_y);
+    msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information);
+    // 当前超界提示仅保留一项
+    // 20211026已修改，分离为电子围栏状态与超界状态，只有测量正确时超界状态才有意义
+    msg.set_border_status(measure_info.terminal_range_status);
+    if (!measure_info.correctness)
+    {
+        if (cloud_size > 0)
+            msg.set_ground_status(message::Ground_statu::Noise);
+        else
+            msg.set_ground_status(message::Ground_statu::Nothing);
+    }
+    else if (measure_info.terminal_range_status == int(Ground_region::Range_status::Range_correct))
+    {
+        msg.set_ground_status(message::Ground_statu::Car_correct);
+    }
+    else
+    {
+        msg.set_ground_status(message::Ground_statu::Car_border_reached);
+        // 更新待提示错误信息
+        std::string t_error_str;
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_front != 0)
+        {
+            t_error_str.append("前超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_back != 0)
+        {
+            t_error_str.append("后超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_left != 0)
+        {
+            t_error_str.append("左超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_right != 0)
+        {
+            t_error_str.append("右超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_bottom != 0)
+        {
+            t_error_str.append("底盘超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_top != 0)
+        {
+            t_error_str.append("顶超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_car_width != 0)
+        {
+            t_error_str.append("车宽超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_car_wheelbase != 0)
+        {
+            t_error_str.append("轴距超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_angle_anti_clock != 0)
+        {
+            t_error_str.append("车辆角度左超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_angle_clock != 0)
+        {
+            t_error_str.append("车辆角度右超界 ");
+        }
+        if (measure_info.terminal_range_status & Ground_region::Range_status::Range_steering_wheel_nozero != 0)
+        {
+            t_error_str.append("车辆前轮角超界 ");
+        }
+
+        t_error.set_error_description(t_error_str);
+    }
+
+    msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
+    msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
+    msg.mutable_error_manager()->set_error_description(t_error.get_error_description());
+    return true;
+}
 
 // ***************************** rabbitmq *********************************
 Error_manager System_executor::encapsulate_send_mq_status()
@@ -624,7 +730,12 @@ Error_manager System_executor::encapsulate_send_mq_status()
 			LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<"region mismatch, vlp16 "<<iter->first<<", wj doesn't have corresponding region id";
 		}
 
+        int rabbit_send_status_msg_id = t_multi_status_msg.id_struct().terminal_id();
+        int rabbit_send_terminal_status_msg_id = t_multi_status_msg.id_struct().terminal_id() + 6;  // 6是因为目前楚天plc有六个队列
+
+        message::Ground_status_msg t_multi_terminal_status_msg = t_multi_status_msg;
 		update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size());
+        update_terminal_measure_info(t_multi_terminal_status_msg, t_car_wheel_information, t_region_cloud->size());
 
 		if(t_multi_status_msg.id_struct().terminal_id() == 4 ||
 			t_multi_status_msg.id_struct().terminal_id() == 0||
@@ -633,7 +744,7 @@ Error_manager System_executor::encapsulate_send_mq_status()
 			t_multi_status_msg.id_struct().terminal_id() == 3||
 			t_multi_status_msg.id_struct().terminal_id() == 2)
 		{
-			// rabbitmq new measure info
+			// plc测量结果转换
 			measure_info t_multi_measure_info_msg;
 			t_multi_measure_info_msg.set_cx(t_car_wheel_information.uniform_car_x);
 			t_multi_measure_info_msg.set_cy(t_car_wheel_information.uniform_car_y);
@@ -645,32 +756,18 @@ Error_manager System_executor::encapsulate_send_mq_status()
 			t_multi_measure_info_msg.set_front_theta(t_car_wheel_information.car_front_theta);
 			t_multi_measure_info_msg.set_border_statu(t_car_wheel_information.range_status);
 
+            // 终端测量结果copy，只改变超界状态
+            measure_info t_multi_terminal_measure_info_msg;
+            t_multi_terminal_measure_info_msg = t_multi_measure_info_msg;
+            t_multi_terminal_measure_info_msg.set_border_statu(t_car_wheel_information.terminal_range_status);
 
-			auto t_ground_status = t_multi_status_msg.ground_status();
-			if(t_ground_status == message::Ground_statu::Car_correct)
-			{
-				t_multi_measure_info_msg.set_ground_status(0);
-			}else if(t_ground_status == message::Ground_statu::Nothing)
-			{
-				t_multi_measure_info_msg.set_ground_status(1);
-			}else if(t_ground_status == message::Ground_statu::Noise)
-			{
-				t_multi_measure_info_msg.set_ground_status(2);
-			}
-			else if(t_ground_status == message::Ground_statu::Car_border_reached)
-			{
-				t_multi_measure_info_msg.set_ground_status(3);
-			}else
-			{
-				t_multi_measure_info_msg.set_ground_status(2);
-				std::cout<<"system executor 661, original status "<<t_ground_status<<std::endl;
-			}
-
-
-            // LOG(WARNING) << t_multi_status_msg.error_manager().error_description()<<std::endl;
-			std::string t_msg = t_multi_measure_info_msg.DebugString();
-			System_communication_mq::get_instance_references().encapsulate_status_msg(t_msg, t_multi_status_msg.id_struct().terminal_id());
+            // 变更超界状态，因为发出去的和旧的状态不太一样
+            t_multi_measure_info_msg.set_ground_status(transMeasureStatus(t_multi_status_msg.ground_status()));
+            t_multi_terminal_measure_info_msg.set_ground_status(transMeasureStatus(t_multi_terminal_status_msg.ground_status()));
 
+            // 分别发送到两个队列
+			System_communication_mq::get_instance_references().encapsulate_status_msg(t_multi_measure_info_msg.DebugString(), rabbit_send_status_msg_id);
+            System_communication_mq::get_instance_references().encapsulate_status_msg(t_multi_terminal_measure_info_msg.DebugString(), rabbit_send_terminal_status_msg_id);
             // if (t_multi_status_msg.id_struct().terminal_id() == 1)
             // {
             //     LOG(WARNING) << "-------------------------------------------------------" <<std::endl;

测量节点/system/system_executor.h

@@ -75,8 +75,13 @@ public://API functions
 	//判断是否为待机，如果已经准备好，则可以执行任务。
 	bool is_ready();
 
+    int transMeasureStatus(const int &statu);
+
 	// 更新消息中关于定位结果与超界信息的内容
 	bool update_measure_info(message::Ground_status_msg &msg, Common_data::Car_wheel_information &measure_info, int cloud_size);
+
+    // 更新消息中关于终端的定位结果与超界信息的内容
+    bool update_terminal_measure_info(message::Ground_status_msg &msg, Common_data::Car_wheel_information &measure_info, int cloud_size);
 public://get or set member variable
 	System_executor_status get_system_executor_status();
 	int get_terminal_id();

测量节点/tool/common_data.h

@@ -68,6 +68,7 @@ public:
 		float single_wheel_length = 0;		// 单轮长
 
 		int range_status = 0;
+        int terminal_range_status = 0;
 
 	public:
 		Car_wheel_information& operator+=(const Car_wheel_information& other)
@@ -127,6 +128,7 @@ public:
 			this->uniform_car_x = other.uniform_car_x;
 			this->uniform_car_y = other.uniform_car_y;
 			this->range_status = other.range_status;
+            this->terminal_range_status = other.terminal_range_status;
 			return *this;
 		}
 		// 定义评分规则, 

测量节点/tool/region_status_checker.h

@@ -229,7 +229,7 @@ public:
         {
             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());
+            sprintf(buf, ",, prev %ld, curr %ld;; 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());
         }
         // std::cout << "000" << std::endl;

测量节点/tool/rotate_center_caliber.cpp

@@ -137,6 +137,7 @@ bool rotate_center_caliber::push_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
         // updata_points();
     }
 
+    return true;
 }
 
 

测量节点/velodyne_lidar/ground_region.cpp

@@ -371,7 +371,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
             cloud_cut->push_back(pt);
         }
     }
-    if(cloud_cut->size() <= 0)
+    if(cloud_cut->empty())
     {
         // 更新过滤点
         m_filtered_cloud_mutex.lock();
@@ -386,7 +386,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
     vox.filter(*cloud_clean);         //执行滤波处理，存储输出
     // cloud_filtered = cloud_clean;
-    if (cloud_clean->size() == 0){
+    if (cloud_clean->empty()){
         // 更新过滤点
         m_filtered_cloud_mutex.lock();
         mp_cloud_filtered->clear();
@@ -737,22 +737,22 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     //plc坐标，1 3 5入口x为-1.8左右， 2 4 6入口为1.8左右
     //要求x取值范围，1650～2150左右
     float t_center_x = cx + m_region.plc_offsetx();
-    if(t_center_x < m_region.border_minx()-0.000)
+    if(t_center_x < m_region.plc_out_of_region().border_minx()-0.000)
     {
         m_range_status_last |= Range_status::Range_left;
     }
-    else if(t_center_x > m_region.border_minx()+0.010)
+    else if(t_center_x > m_region.plc_out_of_region().border_minx()+0.010)
     {
         int t = Range_status::Range_left;
         m_range_status_last &= (~t);
     }
     //else      m_range_status_last no change
 
-    if(t_center_x > m_region.border_maxx()+0.000)
+    if(t_center_x > m_region.plc_out_of_region().border_maxx()+0.000)
     {
         m_range_status_last |= Range_status::Range_right;
     }
-    else if(t_center_x < m_region.border_maxx()-0.010)
+    else if(t_center_x < m_region.plc_out_of_region().border_maxx()-0.010)
     {
         int t = Range_status::Range_right;
         m_range_status_last &= (~t);
@@ -851,55 +851,39 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     // LOG(WARNING) << m_range_status_last;
     // 车位位于y负方向，判断后轮超界情况
     double t_center_y = 0.0 - (car_uniform_center.y() + m_region.plc_offsety()); // 汽车中心y， 参照plc坐标，并且进行偏移， 绝对值。结果大约为 5～6左右
-    if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_border_maxy()-0.000    &&
-        t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_border_miny()-0.000 )
+    if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_out_of_region().plc_border_maxy()-0.000    &&
+        t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_out_of_region().plc_border_miny()-0.000 )
     {
         int t = Range_status::Range_back;
         m_range_status_last &= (~t);
     }
-    else if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) > m_region.plc_border_maxy()+0.010    ||
-             t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) > m_region.plc_border_miny()+0.010 )
+    else if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) > m_region.plc_out_of_region().plc_border_maxy()+0.010    ||
+             t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) > m_region.plc_out_of_region().plc_border_miny()+0.010 )
     {
         m_range_status_last |= Range_status::Range_back;
     }
 
-//    LOG(WARNING) << "********************************************************************************************************";
-//    LOG(WARNING) << "t_center_y" << " " << t_center_y;
-//    LOG(WARNING) << "measure_result.uniform_car_y" << " " << measure_result.uniform_car_y;
-//    LOG(WARNING) << "car_uniform_center.y()" << " " << car_uniform_center.y();
-//    LOG(WARNING) << "m_region.plc_offsety()" << " " << m_region.plc_offsety();
-//    LOG(WARNING) << "measure_result.car_wheel_base" << " " << measure_result.car_wheel_base;
-//    LOG(WARNING) << "m_region.plc_border_maxy()" << " " << m_region.plc_border_maxy();
-//    LOG(WARNING) << "m_region.plc_border_miny()" << " " << m_region.plc_border_miny();
-//    LOG(WARNING) << "m_range_status_last" << " " << m_range_status_last;
-//    LOG(WARNING) << "--------------------------------------------------------------------------------------------------";
-
-    //else      m_range_status_last no change
-    // LOG(WARNING) << m_range_status_last;
-
     // 判断车辆宽度超界情况
-    if (measure_result.car_wheel_width < m_region.car_min_width()-0.000 ||
-        measure_result.car_wheel_width > m_region.car_max_width()+0.000)
+    if (measure_result.car_wheel_width < m_region.plc_out_of_region().car_min_width()-0.000 ||
+        measure_result.car_wheel_width > m_region.plc_out_of_region().car_max_width()+0.000)
     {
         m_range_status_last |= Range_status::Range_car_width;
     }
-    else if (measure_result.car_wheel_width > m_region.car_min_width()+0.010 &&
-             measure_result.car_wheel_width < m_region.car_max_width()-0.010)
+    else if (measure_result.car_wheel_width > m_region.plc_out_of_region().car_min_width()+0.010 &&
+             measure_result.car_wheel_width < m_region.plc_out_of_region().car_max_width()-0.010)
     {
         int t = Range_status::Range_car_width;
         m_range_status_last &= (~t);
     }
-    //else      m_range_status_last no change
-    // LOG(WARNING) << m_range_status_last;
 
     // 判断车辆轴距超界情况
-    if (measure_result.car_wheel_base < m_region.car_min_wheelbase()-0.000 ||
-        measure_result.car_wheel_base > m_region.car_max_wheelbase()+0.000)
+    if (measure_result.car_wheel_base < m_region.plc_out_of_region().car_min_wheelbase()-0.000 ||
+        measure_result.car_wheel_base > m_region.plc_out_of_region().car_max_wheelbase()+0.000)
     {
         m_range_status_last |= Range_status::Range_car_wheelbase;
     }
-    else if (measure_result.car_wheel_base > m_region.car_min_wheelbase()+0.010 ||
-             measure_result.car_wheel_base < m_region.car_max_wheelbase()-0.010)
+    else if (measure_result.car_wheel_base > m_region.plc_out_of_region().car_min_wheelbase()+0.010 ||
+             measure_result.car_wheel_base < m_region.plc_out_of_region().car_max_wheelbase()-0.010)
     {
         int t = Range_status::Range_car_wheelbase;
         m_range_status_last &= (~t);
@@ -944,6 +928,101 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
 #endif
 }
 
+int Ground_region::outOfTerminalRangeDetection(Common_data::Car_wheel_information &measure_result, const velodyne::OutOfRegion &outOfRegion) {
+    Eigen::Vector2d car_uniform_center(measure_result.uniform_car_x, measure_result.uniform_car_y);
+
+    int t_range_status_last = Range_status::Range_correct;
+    // LOG(WARNING) << m_range_status_last;
+    // 车位位于y负方向，判断后轮超界情况
+    double t_center_y = 0.0 - (car_uniform_center.y() + m_region.plc_offsety()); // 汽车中心y， 参照plc坐标，并且进行偏移， 绝对值。结果大约为 5～6左右
+    if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) < outOfRegion.plc_border_maxy()-0.000    &&
+        t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) < outOfRegion.plc_border_miny()-0.000 )
+    {
+        int t = Range_status::Range_back;
+        t_range_status_last &= (~t);
+    }
+    else if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) > outOfRegion.plc_border_maxy()+0.010    ||
+             t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) > outOfRegion.plc_border_miny()+0.010 )
+    {
+        t_range_status_last |= Range_status::Range_back;
+    }
+
+    // 判断车辆宽度超界情况
+    if (measure_result.car_wheel_width < outOfRegion.car_min_width()-0.000 ||
+        measure_result.car_wheel_width > outOfRegion.car_max_width()+0.000)
+    {
+        t_range_status_last |= Range_status::Range_car_width;
+    }
+    else if (measure_result.car_wheel_width > outOfRegion.car_min_width()+0.010 &&
+             measure_result.car_wheel_width < outOfRegion.car_max_width()-0.010)
+    {
+        int t = Range_status::Range_car_width;
+        t_range_status_last &= (~t);
+    }
+
+    // 判断车辆轴距超界情况
+    if (measure_result.car_wheel_base < outOfRegion.car_min_wheelbase()-0.000 ||
+        measure_result.car_wheel_base > outOfRegion.car_max_wheelbase()+0.000)
+    {
+        t_range_status_last |= Range_status::Range_car_wheelbase;
+    }
+    else if (measure_result.car_wheel_base > outOfRegion.car_min_wheelbase()+0.010 ||
+             measure_result.car_wheel_base < outOfRegion.car_max_wheelbase()-0.010)
+    {
+        int t = Range_status::Range_car_wheelbase;
+        t_range_status_last &= (~t);
+    }
+
+    // 判断车辆旋转角超界情况
+    double t_dtheta = measure_result.car_angle + m_region.plc_offset_degree();    // 车身的旋转角， 修正到正5度， (m_region.plc_offset_degree():-89)
+    int t_terminal_id = m_region.region_id() +1;        //region_id:0~5,   terminal_id:1~6
+    if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()+0.00)
+    {
+        t_range_status_last |= Range_status::Range_angle_anti_clock;
+    }
+    else if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()-0.10)
+    {
+        int t = Range_status::Range_angle_anti_clock;
+        t_range_status_last &= (~t);
+    }
+
+    if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()-0.00)
+    {
+        t_range_status_last |= Range_status::Range_angle_clock;
+    }
+    else if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()+0.10)
+    {
+        int t = Range_status::Range_angle_clock;
+        t_range_status_last &= (~t);
+    }
+
+    //huli 不再使用汽车左右边界来判断，直接使用中心点x值判断左右超界。
+    //plc坐标，1 3 5入口x为-1.8左右， 2 4 6入口为1.8左右
+    //要求x取值范围，1650～2150左右
+    if(car_uniform_center.x() < outOfRegion.border_minx()-0.000)
+    {
+        t_range_status_last |= Range_status::Range_left;
+    }
+    else if(car_uniform_center.x() > outOfRegion.border_minx()+0.010)
+    {
+        int t = Range_status::Range_left;
+        t_range_status_last &= (~t);
+    }
+    //else      m_range_status_last no change
+
+    if(car_uniform_center.x() > outOfRegion.border_maxx()+0.000)
+    {
+        t_range_status_last |= Range_status::Range_right;
+    }
+    else if(car_uniform_center.x() < outOfRegion.border_maxx()-0.010)
+    {
+        int t = Range_status::Range_right;
+        t_range_status_last &= (~t);
+    }
+
+    return t_range_status_last;
+}
+
 //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
 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)
 {
@@ -1059,9 +1138,11 @@ void Ground_region::thread_measure_func()
                 // 超界校验
                 {
                     std::lock_guard<std::mutex> lck(m_filtered_cloud_mutex);
-                    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;
-                    // LOG_IF(INFO, m_region.region_id() == 4) << res;
+
+                    m_car_wheel_information.range_status = 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.terminal_range_status = outOfTerminalRangeDetection(m_car_wheel_information, m_region.terminal_out_of_region());
                 }
                 // 添加plc偏移
                 m_car_wheel_information.car_center_x += m_region.plc_offsetx();
@@ -1070,13 +1151,6 @@ void Ground_region::thread_measure_func()
                 m_car_wheel_information.uniform_car_y += m_region.plc_offsety();
                 m_car_wheel_information.car_angle += m_region.plc_offset_degree();
 
-//                LOG(INFO) << "yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy t_result.theta = " << t_result.theta << "  ";
-//                LOG(INFO) << "yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy m_region.plc_offset_degree() = " << m_region.plc_offset_degree() << "  ";
-//                LOG(INFO) << "yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy  m_car_wheel_information.car_angle= " <<   m_car_wheel_information.car_angle << "  ";
-
-
-
-
                 t_wheel_info_stamped.wheel_data = m_car_wheel_information;
                 t_wheel_info_stamped.measure_time = m_detect_update_time;
                 Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped);
@@ -1096,22 +1170,12 @@ void Ground_region::thread_measure_func()
                 }else
                 {
                     m_car_wheel_information.range_status |= Range_status::Range_car_moving;
-                    // if(m_region.region_id()==4){
-                    //     std::cout<<"success: "<<car_status_res.to_string()<<std::endl;
-                    // }
                 }
                 Region_status_checker::get_instance_references().add_measure_data(m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered);
 
                 ec = Measure_filter::get_instance_references().get_filtered_wheel_information(m_region.region_id(), t_wheel_info_stamped.wheel_data);
                 if (ec == SUCCESS)
                 {
-                    // 20221208 added by yct, use avg car angle and width data.
-                    // LOG(WARNING)<<m_region.region_id()<<" angles: "<<m_car_wheel_information.car_angle<<", "<<t_wheel_info_stamped.wheel_data.car_angle;
-                    // LOG(WARNING)<<m_region.region_id()<<" filter res: "<<t_wheel_info_stamped.wheel_data.to_string()<<std::endl;
-
-                    // m_car_wheel_information.car_angle = t_wheel_info_stamped.wheel_data.car_angle;
-                    // m_car_wheel_information.car_wheel_width = t_wheel_info_stamped.wheel_data.car_wheel_width;
-
                     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;
                     // 临时添加，滤波后前轮超界
@@ -1120,10 +1184,9 @@ void Ground_region::thread_measure_func()
                         m_car_wheel_information.range_status |= Range_status::Range_steering_wheel_nozero;
                     }
                 }
-                // else{
-                //     std::cout<<ec.to_string()<<std::endl;
-                // }
-                //  LOG_IF(INFO, m_region.region_id() == 4) << m_car_wheel_information.to_string();
+
+                // 超界判断，这个必须最后判断
+
             }
             // rough angle too large, display for pre-movement
             else if(ec.get_error_code() == VELODYNE_REGION_ANGLE_PRECHECK_FAILED)

测量节点/velodyne_lidar/ground_region.h

@@ -166,6 +166,8 @@ private:
    // 超界判断，增加对后轮的判断，对轴距、车宽、角度超界的判断
    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);
 
+   int outOfTerminalRangeDetection(Common_data::Car_wheel_information &measure_result, const velodyne::OutOfRegion &outOfRegion);
+
 private:
 
 

测量节点/velodyne_lidar/velodyne_config.proto

@@ -44,6 +44,19 @@ message lidarExtrinsic
     optional CalibParameter calib=2;
 }
 
+message OutOfRegion {
+    required float border_minx=1; // 最小边界x，左超界提示
+    required float border_maxx=2; // 最大边界x，右超界提示
+    required float plc_border_miny=3;// plc后夹持y方向极限值
+    required float plc_border_maxy=4;// plc后夹持y方向极限值
+    required float car_min_width=5; // 最小车宽
+    required float car_max_width=6; // 最大车宽
+    required float car_min_wheelbase=7; // 最小轴距
+    required float car_max_wheelbase=8; // 最大轴距
+    required float turnplate_angle_limit_anti_clockwise=9; // 转盘逆时针角度极限
+    required float turnplate_angle_limit_clockwise=10; // 转盘顺时针角度极限
+}
+
 message Region
 {
     required float minx=1;
@@ -56,17 +69,9 @@ message Region
     repeated lidarExtrinsic lidar_exts=8;
     required float turnplate_cx=9;
     required float turnplate_cy=10;
-    required float border_minx=11; // 最小边界x，左超界提示
-    required float border_maxx=12; // 最大边界x，右超界提示
-    required float plc_offsetx=13; // plc偏移x
-    required float plc_offsety=14; // plc偏移y
-    required float plc_offset_degree=15; // plc偏移角度
-    required float plc_border_miny=16;// plc后夹持y方向极限值
-    required float plc_border_maxy=17;// plc后夹持y方向极限值
-    required float car_min_width=18; // 最小车宽
-    required float car_max_width=19; // 最大车宽
-    required float car_min_wheelbase=20; // 最小轴距
-    required float car_max_wheelbase=21; // 最大轴距
-    required float turnplate_angle_limit_anti_clockwise=22; // 转盘逆时针角度极限
-    required float turnplate_angle_limit_clockwise=23; // 转盘顺时针角度极限
+    required float plc_offsetx=11; // plc偏移x
+    required float plc_offsety=12; // plc偏移y
+    required float plc_offset_degree=13; // plc偏移角度
+    required OutOfRegion plc_out_of_region = 14;
+    required OutOfRegion terminal_out_of_region = 15;
 }