2022/12/10 huli lidar bord

指令检查节点/CheckEntrance.py

@@ -62,7 +62,10 @@ class EntranceChecker(threading.Thread):
             return tf.MessageToString(park, as_utf8=True)
         else:
             park.statu.execute_statu = message.eError
-            park.statu.statu_description = self.error_str
+            if self.error_str == 'OK':
+            	park.statu.statu_description = "测量异常，请重新点击存车按钮!"
+            else:
+            	park.statu.statu_description = self.error_str
             return tf.MessageToString(park, as_utf8=True)
 
     def run(self):
@@ -128,18 +131,18 @@ class EntranceChecker(threading.Thread):
             #先判断光电
             if is_moving:
                 if icpu_statu.back_io==1:
-                    self.error_str='请按提示调整'
+                    self.error_str='请按提示调整（后超界）'
                     self.last_show="调整"
                     continue
 
             #光电正常
             if lidar_statu==MeasureStatu["无数据"]:
-                self.error_str='请按提示调整'
+                self.error_str='请按提示调整(雷达无数据)'
                 self.last_show="空闲"
                 continue
             elif lidar_statu==MeasureStatu["噪声"]:
                 if self.last_show=="超时":
-                    self.error_str='请按提示调整'
+                    self.error_str='请按提示调整(雷达噪声)'
                     self.last_show="空闲"
                 continue
             elif lidar_statu==MeasureStatu["ok"]:
@@ -151,19 +154,19 @@ class EntranceChecker(threading.Thread):
                     self.last_show="轴距超差"
                     continue
                 if (border_statu>>9)&0x01==1:
-                    self.error_str='请按提示调整'
+                    self.error_str='请按提示调整(向左)'
                     self.last_show="请调整"
                     continue
                 if (border_statu>>8)&0x01==1:
-                    self.error_str='请按提示调整'
+                    self.error_str='请按提示调整(向右)'
                     self.last_show="请调整"
                     continue
                 if (border_statu>>10)&0x01==1:
-                    self.error_str='请按提示调整'
+                    self.error_str='请按提示调整(方向盘)'
                     self.last_show="请调整"
                     continue
                 if (border_statu>>6)&0x01==1:
-                    self.error_str='请按提示调整'
+                    self.error_str='超宽车辆'
                     self.last_show="超宽"
                     continue
                 border=border_statu&0x0f

测量节点/setting/velodyne_manager.prototxt

@@ -184,15 +184,15 @@ region
     region_id:5
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.2
-    border_maxx:1.2
+    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.2
     plc_border_maxy:1.28
     car_min_width:1.55
-    car_max_width:1.92
+    car_max_width:1.920
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -237,8 +237,8 @@ region
     region_id:4
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.2
-    border_maxx:1.2
+    border_minx:-2.220
+    border_maxx:-1.630
     # -1.8683
     plc_offsetx:-1.865
     plc_offsety:-6.11489
@@ -246,7 +246,7 @@ region
     plc_border_miny:1.2
     plc_border_maxy:1.28
     car_min_width:1.55
-    car_max_width:1.92
+    car_max_width:1.920
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -285,15 +285,15 @@ region
     region_id:3
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.2
-    border_maxx:1.2
+    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.2
     plc_border_maxy:1.28
     car_min_width:1.55
-    car_max_width:1.92
+    car_max_width:1.920
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -333,8 +333,8 @@ region
     region_id:2
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.2
-    border_maxx:1.2
+    border_minx:-2.190
+    border_maxx:-1.635
     # 0.02
     plc_offsetx:-1.871
     # -0.34
@@ -343,7 +343,7 @@ region
     plc_border_miny:1.2
     plc_border_maxy:1.28
     car_min_width:1.55
-    car_max_width:1.92
+    car_max_width:1.920
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -387,15 +387,15 @@ region
     region_id:1
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.2
-    border_maxx:1.2
+    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.2
     plc_border_maxy:1.29
     car_min_width:1.55
-    car_max_width:1.92
+    car_max_width:1.920
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -441,15 +441,15 @@ region
     region_id:0
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.2
-    border_maxx:1.2
+    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.2
     plc_border_maxy:1.29
     car_min_width:1.55
-    car_max_width:1.92
+    car_max_width:1.920
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3

测量节点/system/system_executor.cpp

@@ -667,7 +667,7 @@ Error_manager System_executor::encapsulate_send_mq_status()
 			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());
 
-            if (t_multi_status_msg.id_struct().terminal_id() == 3)
+            if (t_multi_status_msg.id_struct().terminal_id() == 1)
             {
                 LOG(WARNING) << "-------------------------------------------------------" <<std::endl;
                 LOG(WARNING) << "t_msg = " << t_msg <<std::endl;

测量节点/velodyne_lidar/ground_region.cpp

@@ -657,6 +657,7 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     // LOG_IF(WARNING, m_region.region_id() == 4)<< "border minmax x: "<< min_p.x<<", "<<max_p.x<<", res: "<<res;
     return res;
 #else
+    /*
     if(min_p.x < m_region.border_minx()-0.000)
     {
         m_range_status_last |= Range_status::Range_left;
@@ -678,7 +679,38 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
         m_range_status_last &= (~t);
     }
     //else      m_range_status_last no change
+*/
+    //huli 不再使用汽车左右边界来判断，直接使用中心点x值判断左右超界。
+    //plc坐标，1 3 5入口x为-1.8左右， 2 4 6入口为1.8左右
+    //要求x取值范围，1650～2150左右
+    float t_center_x = (min_p.x + max_p.x) / 2 + m_region.plc_offsetx();
+    if(t_center_x < m_region.border_minx()-0.000)
+    {
+        m_range_status_last |= Range_status::Range_left;
+    }
+    else if(t_center_x > m_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)
+    {
+        m_range_status_last |= Range_status::Range_right;
+    }
+    else if(t_center_x < m_region.border_maxx()-0.010)
+    {
+        int t = Range_status::Range_right;
+        m_range_status_last &= (~t);
+    }
+    //else      m_range_status_last no change
 
+//    LOG(WARNING) << "-------------------------------------------------------" <<std::endl;
+//    LOG(WARNING) << "min_p.x = " << min_p.x <<std::endl;
+//    LOG(WARNING) << "max_p.x = " << max_p.x <<std::endl;
+//    LOG(WARNING) << "t_center_x = " << t_center_x <<std::endl;
+//    LOG(WARNING) << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++" <<std::endl;
 
     return m_range_status_last;
 #endif //ANTI_SHAKE
@@ -1008,8 +1040,11 @@ void Ground_region::thread_measure_func()
                 if (ec == SUCCESS)
                 {
                     // 20221208 added by yct, use avg car angle and width data.
-                    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;
+                    // 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;