Merge remote-tracking branch 'origin/master'

plc调度节点/dispatch/dispatch_command.cpp

@@ -924,17 +924,17 @@ Error_manager Dispatch_command::updata_record_for_park_end()
 	int in_time_difference = Time_tool::get_instance_references().get_time_seconds(PARK_TIME_FLAG);
 
 	//t_index_id是指定单元的车位id, 1~78
-	int t_index_id = (m_dispatch_command_map[m_car_number_optimal].m_parkspace_info_msg.id() - 1 ) / 78;
-	t_index_id = t_index_id + 1;
+	int t_table_id = (m_dispatch_command_map[m_car_number_optimal].m_parkspace_info_msg.id() - 1 ) % 78;
+	t_table_id = t_table_id + 1;
 	//执行sql操作
 	char update_record_sql[1024];
 	memset(update_record_sql, 0, 1024);
 //	sprintf(update_record_sql, "update record set in_time_end = '%s', in_time_difference = %d, space_id = %d, space_info = '%s' where primary_key = '%s' ",
-	sprintf(update_record_sql, "update record set in_time_end = '%s', in_time_difference = %d, space_id = %d, index_id = %d, unit_id = %d, floor_id = %d, room_id = %d  where primary_key = '%s' ",
+	sprintf(update_record_sql, "update record set in_time_end = '%s', in_time_difference = %d, space_id = %d, table_id = %d, unit_id = %d, floor_id = %d, room_id = %d  where primary_key = '%s' ",
 			Time_tool::get_instance_references().get_current_time_seconds().c_str(),
 			in_time_difference,
 			m_dispatch_command_map[m_car_number_optimal].m_parkspace_info_msg.id(),
-			t_index_id,
+			t_table_id,
 			m_dispatch_command_map[m_car_number_optimal].m_parkspace_info_msg.unit_id(),
 			m_dispatch_command_map[m_car_number_optimal].m_parkspace_info_msg.floor(),
 			m_dispatch_command_map[m_car_number_optimal].m_parkspace_info_msg.room_id(),
@@ -972,7 +972,7 @@ Error_manager Dispatch_command::updata_record_for_pick_end()
 	get_parking_time(m_dispatch_command_map[m_car_number_optimal].m_primary_key, parking_time);
 	char parking_time_string[256] = "";
 	int hour = parking_time / 3600;
-	int min = parking_time / 60;
+	int min = (parking_time / 60)%60;
 	int sec = parking_time % 60;
 	sprintf(parking_time_string, " %d:%02d:%02d", hour, min, sec);
 
@@ -996,7 +996,7 @@ Error_manager Dispatch_command::updata_record_for_pick_end_ex()
 	get_parking_time(m_dispatch_command_map[m_car_number_optimal].m_primary_key, parking_time);
 	char parking_time_string[256] = "";
 	int hour = parking_time / 3600;
-	int min = parking_time / 60;
+    int min = (parking_time / 60)%60;
 	int sec = parking_time % 60;
 	sprintf(parking_time_string, " %d:%02d:%02d", hour, min, sec);
 

plc调度节点/dispatch/dispatch_plc.cpp

@@ -622,8 +622,8 @@ Error_manager Dispatch_plc::encapsulate_dispatch_request_to_plc_new()
 		//车位信息
 		m_request_parkingspace_index_id = m_park_table_msg.allocated_space_info().id();
 		m_request_parkingspace_unit_id = m_park_table_msg.allocated_space_info().unit_id();
-		int t_parkingspace_label_id = m_park_table_msg.allocated_space_info().id()%78;
-		m_request_parkingspace_label_id = t_parkingspace_label_id;
+		int t_parkingspace_label_id = (m_park_table_msg.allocated_space_info().id()-1)%78;
+		m_request_parkingspace_label_id = t_parkingspace_label_id+1;
 		m_request_parkingspace_floor_id = m_park_table_msg.allocated_space_info().floor();
 		m_request_parkingspace_room_id = m_park_table_msg.allocated_space_info().room_id();
 
@@ -665,8 +665,8 @@ Error_manager Dispatch_plc::encapsulate_dispatch_request_to_plc_new()
 		//车位信息
 		m_request_parkingspace_index_id = m_pick_table_msg.actually_space_info().id();
 		m_request_parkingspace_unit_id = m_pick_table_msg.actually_space_info().unit_id();
-		int t_parkingspace_label_id = m_pick_table_msg.actually_space_info().id()%78;
-		m_request_parkingspace_label_id = t_parkingspace_label_id;
+		int t_parkingspace_label_id = (m_pick_table_msg.actually_space_info().id()-1)%78;
+		m_request_parkingspace_label_id = t_parkingspace_label_id+1;
 		m_request_parkingspace_floor_id = m_pick_table_msg.actually_space_info().floor();
 		m_request_parkingspace_room_id = m_pick_table_msg.actually_space_info().room_id();
 		//车位朝向, 奇数朝南, 偶数朝北

出口单片机节点/ExportIO.py

@@ -31,11 +31,15 @@ class ExportIO(threading.Thread):
         self._rabbit_mq=ASC.RabbitAsyncCommunicator(rabbitmq_parameter["ip"],rabbitmq_parameter["port"],
                                                     rabbitmq_parameter['user'],rabbitmq_parameter['password'])
 
-
         #消费关门消息
         self._rabbit_mq.Init(None, None)
         self._rabbit_mq.start()
 
+        #数据库重试的次数, 超过3次就重启数据库梁
+        self._retry_count = 0
+
+
+
     def SetUserLeaveCallback(self,callback):
         self._user_leave_callback=callback
 
@@ -54,6 +58,7 @@ class ExportIO(threading.Thread):
     #open_door 函数被node_py的主函数调用，不要做太多的事，剩余的开门控制和数据库清理，交给open_door_loop
     def open_door(self,table):
         self._open_door_flag = True
+        
         return
 
     async def open_door_loop(self):
@@ -74,7 +79,9 @@ class ExportIO(threading.Thread):
                         if not self._user_leave_callback == None:
                             self._user_leave_callback(self._id)
                             self._open_door_flag = False
-                    #else  无限等待，什么也不做，  等待汽车离开
+                            self._retry_count = self._retry_count + 1
+                    else:#else  无限等待，什么也不做，  等待汽车离开
+                        self._retry_count = 0
 
             except:
                 pass

出口单片机节点/node.py

@@ -62,7 +62,12 @@ if __name__=="__main__":
                     export=g_exports.get(table.export_id)
                     if not export==None:
                         # if export.export_idle()==False:
-                        export.open_door(table)
+                        if export._retry_count < 5:
+                            export.open_door(table)
+                        else :
+                            g_db.close()
+                            g_db.__init__(db_parameter["ip"],db_parameter["port"],db_parameter["dbname"],
+                                          db_parameter["user"],db_parameter["password"])
 
         time.sleep(1)
 

指令检查节点/CheckEntrance.py

@@ -1,4 +1,3 @@
-import datetime
 import threading
 import time
 
@@ -40,7 +39,12 @@ class EntranceChecker(threading.Thread):
         park = message.park_table()
         park.CopyFrom(park_table)
         measure_info = message.measure_info()
-        tf.Parse(self.measure_statu.statu, measure_info)
+        if self.measure_statu.statu is not None:
+            tf.Parse(self.measure_statu.statu, measure_info)
+        else:
+            park.statu.execute_statu = message.eError
+            park.statu.statu_description = "设备故障，请联系管理员(雷达)"
+            return tf.MessageToString(park, as_utf8=True)     
         tm = time.time()
         if self.error_str == 'OK':
             while time.time() - tm < 0.5:

测量节点/setting/velodyne_manager.prototxt

@@ -260,6 +260,7 @@ region
         {
             cx:1.9095
             cy:-0.019388
+            r:0.1
         }
     }
     lidar_exts

测量节点/system/system_executor.h

@@ -20,7 +20,7 @@
 #include "../wanji_lidar/wanji_manager.h"
 #include "../velodyne_lidar/velodyne_manager.h"
 
-#define DISP_TERM_ID 2
+#define DISP_TERM_ID -1
 // 0wj  1velo	2both
 #define WJ_VELO 2
 

测量节点/tool/region_status_checker.h

@@ -309,8 +309,8 @@ public:
         return ERROR;
     }
 
-    // 显著性判断，平移x方向速度0.02m/s,y方向速度0.03m/s误差，角度3deg/s误差
-    bool is_significant(Eigen::Matrix4f trans, double dtime, double vx_eps = 0.03, double vy_eps = 0.03, double omega_eps = 0.051)
+    // 显著性判断，平移x方向速度0.02m/s,y方向速度0.06m/s误差，角度3deg/s误差
+    bool is_significant(Eigen::Matrix4f trans, double dtime, double vx_eps = 0.06, double vy_eps = 0.06, double omega_eps = 0.051)
     {
         // 间隔时间为负数，或间隔超过0.5s则手动赋值
         if(dtime<0 || dtime>0.5)

测量节点/velodyne_lidar/ground_region.cpp

@@ -402,7 +402,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_filtered); //保存滤波结果到cloud_filter
 
-    if (cloud_filtered->size() == 0){
+    // 20221211 changed by yct, 10 point threshold
+    if (cloud_filtered->size() <= 10){
         // 更新过滤点
         m_filtered_cloud_mutex.lock();
         mp_cloud_filtered->clear();
@@ -586,12 +587,33 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 
     // 车宽精度优化
     {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_rough_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
         pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_extract_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        t_width_rough_cloud->operator+=(*mp_cloud_filtered);
         t_width_extract_cloud->operator+=(m_detector->m_left_front_cloud);
         t_width_extract_cloud->operator+=(m_detector->m_right_front_cloud);
         t_width_extract_cloud->operator+=(m_detector->m_left_rear_cloud);
         t_width_extract_cloud->operator+=(m_detector->m_right_rear_cloud);
 
+        pcl::PointXYZ t_rough_min_p, t_rough_max_p;
+        // rough width for human detect
+        {
+            //离群点过滤
+            sor.setInputCloud(t_width_rough_cloud);
+            sor.setMeanK(5);                    //K近邻搜索点个数
+            sor.setStddevMulThresh(1.0);        //标准差倍数
+            sor.setNegative(false);             //保留未滤波点（内点）
+            sor.filter(*t_width_rough_cloud); //保存滤波结果到cloud_filter
+
+            Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
+            t_affine.prerotate(Eigen::AngleAxisd((90 - last_result.theta) * M_PI / 180.0, Eigen::Vector3d::UnitZ()));
+            pcl::transformPointCloud(*t_width_rough_cloud, *t_width_rough_cloud, t_affine.matrix());
+
+            // 车宽重计算，并赋值到当前车宽
+            pcl::getMinMax3D(*t_width_rough_cloud, t_rough_min_p, t_rough_max_p);
+        }
+        double rough_width = t_rough_max_p.x - t_rough_min_p.x;
+
         //离群点过滤
         sor.setInputCloud(t_width_extract_cloud);
         sor.setMeanK(5);             //K近邻搜索点个数
@@ -610,6 +632,13 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         double accurate_width = t_max_p.x - t_min_p.x;
         last_result.width = accurate_width;
 
+        if(fabs(rough_width - accurate_width) > 0.08)
+        {
+            std::string disp_str = std::string("width difference too large, assume noise situation, ")+std::to_string(rough_width)+", "+std::to_string(accurate_width);
+            LOG(WARNING) << disp_str;
+            return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, MINOR_ERROR, disp_str.c_str());
+        }
+
         // !!!暂时不限制宽度数据
         // char valid_info[255];
         // sprintf(valid_info, "validation for car width, origin/accurate: (%.3f, %.3f)", last_result.width, accurate_width);
@@ -723,9 +752,9 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
 
 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;
+    // int res = Range_status::Range_correct;
     if(cloud->size() <= 0)
-        return res;
+        return Range_status::Range_correct;
 
     // 计算转盘旋转后车辆中心点坐标
     Eigen::Vector2d car_center(measure_result.car_center_x, measure_result.car_center_y);
@@ -836,7 +865,7 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     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)
     {
-        res |= Range_status::Range_car_wheelbase;
+        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)
@@ -845,7 +874,7 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
         m_range_status_last &= (~t);
     }
     //else      m_range_status_last no change
-    // LOG(WARNING) << m_range_status_last;
+    // LOG_IF(INFO, m_region.region_id() == 4) << m_range_status_last;
 
     // 判断车辆旋转角超界情况
     //huli new program 20221103, 使用 plc  dispatch_1_statu_port 里面的  来做边界判断
@@ -862,19 +891,19 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
         int t = Range_status::Range_angle_anti_clock;
         m_range_status_last &= (~t);
     }
-    // LOG(WARNING) << m_range_status_last;
+    // LOG_IF(INFO, m_region.region_id() == 4) << m_range_status_last;
    
     //else      m_range_status_last no change
     if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()-0.00)
     {
-        res |= Range_status::Range_angle_clock;
+        m_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;
         m_range_status_last &= (~t);
     }
-    // LOG(WARNING) << m_range_status_last;
+    // LOG_IF(INFO, m_region.region_id() == 4) << m_range_status_last <<", t_dtheta "<<t_dtheta<<", minmax: "<<System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()<<", "<<System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max();
     // LOG(INFO) << t_dtheta<<", "<<System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()<<std::endl;
     //else      m_range_status_last no change
 
@@ -1001,6 +1030,7 @@ 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;
                 }
                 // 添加plc偏移
                 m_car_wheel_information.car_center_x += m_region.plc_offsetx();
@@ -1062,7 +1092,7 @@ void Ground_region::thread_measure_func()
                 // else{
                 //     std::cout<<ec.to_string()<<std::endl;
                 // }
-                //  LOG_IF(INFO, m_region.region_id() == 1) << m_car_wheel_information.to_string();
+                //  LOG_IF(INFO, m_region.region_id() == 4) << m_car_wheel_information.to_string();
             }
             else
             {