20221208 huli

plc调度节点/dispatch/dispatch_ground_lidar.cpp

@@ -105,7 +105,11 @@ void Dispatch_ground_lidar::execute_thread_fun()
 				std::unique_lock<std::mutex> t_lock(Dispatch_communication::get_instance_references().m_data_lock);
 				std::unique_lock<std::mutex> t_lock2(m_lock);
 
-				//将nnxx的protobuf 转化为 snap7的DB块,  把店面雷达数据转发给plc
+
+
+
+
+                //将nnxx的protobuf 转化为 snap7的DB块,  把店面雷达数据转发给plc
 //				int t_inlet_id = m_ground_status_msg.mutable_id_struct()->terminal_id() % 2;
 				int t_inlet_id = m_ground_lidar_id;
 				Dispatch_communication::Ground_lidar_response_from_manager_to_plc_for_data *p_response_data = &Dispatch_communication::get_instance_references().m_ground_lidar_response_from_manager_to_plc_for_data[t_inlet_id];
@@ -132,22 +136,50 @@ void Dispatch_ground_lidar::execute_thread_fun()
 //					p_response_data->m_response_uniformed_car_y = m_ground_status_msg.locate_information_realtime().uniformed_car_y();
 //					p_response_data->m_response_locate_correct = m_ground_status_msg.locate_information_realtime().locate_correct();
 
-					p_response_data->m_response_car_center_x = m_measure_info.cx();
-					p_response_data->m_response_car_center_y = m_measure_info.cy();
-					p_response_data->m_response_car_angle = m_measure_info.theta();
-					p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
-					p_response_data->m_response_car_length = m_measure_info.length();
-					p_response_data->m_response_car_width = m_measure_info.width();
-					p_response_data->m_response_car_height = m_measure_info.height();
-					p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
-					p_response_data->m_response_car_wheel_width = m_measure_info.width();
-					p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
-					p_response_data->m_response_uniformed_car_y = m_measure_info.cy();
+
 					if ( m_measure_info.ground_status() == 0 ) //ground_status  :ok 1,nothing 2,noise  3,border
 					{
+                        p_response_data->m_response_car_center_x = m_measure_info.cx();
+                        p_response_data->m_response_car_center_y = m_measure_info.cy();
+                        p_response_data->m_response_car_angle = m_measure_info.theta();
+                        p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
+                        p_response_data->m_response_car_length = m_measure_info.length();
+                        p_response_data->m_response_car_width = m_measure_info.width();
+                        p_response_data->m_response_car_height = m_measure_info.height();
+                        p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
+                        p_response_data->m_response_car_wheel_width = m_measure_info.width();
+                        p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
+                        p_response_data->m_response_uniformed_car_y = m_measure_info.cy();
+
 						p_response_data->m_response_locate_correct = 1;	//locate_correct  0=无效, 1=有效,
 					}
-				}
+					else
+                    {
+                        p_response_data->m_response_car_center_x = 0;
+                        p_response_data->m_response_car_center_y = 0;
+                        p_response_data->m_response_car_angle = 0;
+                        p_response_data->m_response_car_front_theta = 0;
+                        p_response_data->m_response_car_length = 0;
+                        p_response_data->m_response_car_width = 0;
+                        p_response_data->m_response_car_height = 0;
+                        p_response_data->m_response_car_wheel_base = 0;
+                        p_response_data->m_response_car_wheel_width = 0;
+                        p_response_data->m_response_uniformed_car_x = 0;
+                        p_response_data->m_response_uniformed_car_y = 0;
+
+                        p_response_data->m_response_locate_correct = 0;
+                    }
+
+//                    LOG(WARNING) << "------------------------------------------------------------ "<<std::endl;
+//                    LOG(WARNING) << "m_measure_info = " << m_measure_info.ground_status() <<std::endl;
+//                    LOG(WARNING) << "m_measure_info = " << m_measure_info.DebugString() <<std::endl;
+//                    LOG(WARNING) << "m_measure_info.wheelbase() = " << m_measure_info.wheelbase() <<std::endl;
+//                    LOG(WARNING) << "m_measure_info.ground_status() = " << m_measure_info.ground_status() <<std::endl;
+//                    LOG(WARNING) << "p_response_data->m_response_locate_correct = " << p_response_data->m_response_locate_correct <<std::endl;
+//                    printf("p_response_data->m_response_locate_correct = %d, \n", p_response_data->m_response_locate_correct);
+//                    LOG(WARNING) << "-++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ "<<std::endl;
+
+                }
 				else if (p_response_data->m_response_refresh_command == p_request->m_request_refresh_command)
 				{
 					p_response_data->m_response_heartbeat = 1+p_response_data->m_response_heartbeat;
@@ -173,21 +205,38 @@ void Dispatch_ground_lidar::execute_thread_fun()
 //					p_response_data->m_response_uniformed_car_y = m_ground_status_msg.locate_information_realtime().uniformed_car_y();
 //					p_response_data->m_response_locate_correct = m_ground_status_msg.locate_information_realtime().locate_correct();
 
-					p_response_data->m_response_car_center_x = m_measure_info.cx();
-					p_response_data->m_response_car_center_y = m_measure_info.cy();
-					p_response_data->m_response_car_angle = m_measure_info.theta();
-					p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
-					p_response_data->m_response_car_length = m_measure_info.length();
-					p_response_data->m_response_car_width = m_measure_info.width();
-					p_response_data->m_response_car_height = m_measure_info.height();
-					p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
-					p_response_data->m_response_car_wheel_width = m_measure_info.width();
-					p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
-					p_response_data->m_response_uniformed_car_y = m_measure_info.cy();
-					if ( m_measure_info.ground_status() == 0 ) //ground_status  :ok 1,nothing 2,noise  3,border
-					{
-						p_response_data->m_response_locate_correct = 1;	//locate_correct  0=无效, 1=有效,
-					}
+                    if ( m_measure_info.ground_status() == 0 ) //ground_status  :ok 1,nothing 2,noise  3,border
+                    {
+                        p_response_data->m_response_car_center_x = m_measure_info.cx();
+                        p_response_data->m_response_car_center_y = m_measure_info.cy();
+                        p_response_data->m_response_car_angle = m_measure_info.theta();
+                        p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
+                        p_response_data->m_response_car_length = m_measure_info.length();
+                        p_response_data->m_response_car_width = m_measure_info.width();
+                        p_response_data->m_response_car_height = m_measure_info.height();
+                        p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
+                        p_response_data->m_response_car_wheel_width = m_measure_info.width();
+                        p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
+                        p_response_data->m_response_uniformed_car_y = m_measure_info.cy();
+
+                        p_response_data->m_response_locate_correct = 1;	//locate_correct  0=无效, 1=有效,
+                    }
+                    else
+                    {
+                        p_response_data->m_response_car_center_x = 0;
+                        p_response_data->m_response_car_center_y = 0;
+                        p_response_data->m_response_car_angle = 0;
+                        p_response_data->m_response_car_front_theta = 0;
+                        p_response_data->m_response_car_length = 0;
+                        p_response_data->m_response_car_width = 0;
+                        p_response_data->m_response_car_height = 0;
+                        p_response_data->m_response_car_wheel_base = 0;
+                        p_response_data->m_response_car_wheel_width = 0;
+                        p_response_data->m_response_uniformed_car_x = 0;
+                        p_response_data->m_response_uniformed_car_y = 0;
+
+                        p_response_data->m_response_locate_correct = 0;
+                    }
 				}
 
 				m_ground_status_msg_updata_flag = false;

plc调度节点/main.cpp

@@ -72,11 +72,27 @@ int main(int argc,char* argv[])
 //
 //	std::chrono::system_clock::time_point a = Time_tool::get_instance_references().transform_time_string_seconds("2022-11-16 11:12:34");
 //	std::chrono::system_clock::time_point b = Time_tool::get_instance_references().transform_time_string_seconds("2022-11-16 11:34:56");
+//    double k = (b-a).count();
+//    std::cout << " huli test :::: " << " k = " << k << std::endl;
 //	double d = (b-a).count()/1000000000;
 //	std::cout << " huli test :::: " << " d = " << d << std::endl;
 //
+//    std::cout << " huli test :::: " << " a = " << Time_tool::get_instance_references().get_time_string_seconds(a) << std::endl;
+//    std::cout << " huli test :::: " << " b = " << Time_tool::get_instance_references().get_time_string_seconds(b) << std::endl;
+//
+//
+//    int parking_time = d;
+//    char parking_time_string[256] = "";
+//    int hour = parking_time / 3600;
+//    int min = parking_time / 60;
+//    int sec = parking_time % 60;
+//    sprintf(parking_time_string, " %d:%02d:%02d", hour, min, sec);
+//    std::cout << " huli test :::: " << " parking_time_string = " << parking_time_string << std::endl;
+//
 //	return 0;
 
+
+
 	Error_manager t_error;
 
 	const char* logPath = "./";

plc调度节点/tool/time_tool.cpp

@@ -91,16 +91,29 @@ std::chrono::system_clock::time_point Time_tool::transform_time_string_seconds(s
 	std::chrono::system_clock::time_point t_time_point;
 	time_t tt;
 	tm time_tm;
-	sscanf(time_string.c_str(), "%d-%02d-%02d %02d:%02d:%02d", &time_tm.tm_year,
-		   &time_tm.tm_mon, &time_tm.tm_mday, &time_tm.tm_hour,
+	int t_year = 0;
+	int t_mon = 0;
+	sscanf(time_string.c_str(), "%d-%02d-%02d %02d:%02d:%02d", &t_year,
+		   &t_mon, &time_tm.tm_mday, &time_tm.tm_hour,
 		   &time_tm.tm_min, &time_tm.tm_sec);
-	time_tm.tm_year = time_tm.tm_year -1900;
-	time_tm.tm_mon = time_tm.tm_mon -1;
 
+	time_tm.tm_year = t_year -1900;
+	time_tm.tm_mon = t_mon -1;
+    time_tm.tm_isdst = 0;
+//    std::cout << " huli test :::: " << " t_year = " << t_year << std::endl;
+//    std::cout << " huli test :::: " << " t_mon = " << t_mon << std::endl;
+//    std::cout << " huli test :::: " << " time_tm.tm_year = " << time_tm.tm_year << std::endl;
+//    std::cout << " huli test :::: " << " time_tm.tm_mon = " << time_tm.tm_mon << std::endl;
+//    std::cout << " huli test :::: " << " time_tm.tm_mday = " << time_tm.tm_mday << std::endl;
+//    std::cout << " huli test :::: " << " time_tm.tm_hour = " << time_tm.tm_hour << std::endl;
+//    std::cout << " huli test :::: " << " time_tm.tm_min = " << time_tm.tm_min << std::endl;
+//    std::cout << " huli test :::: " << " time_tm.tm_sec = " << time_tm.tm_sec << std::endl;
 
 	tt = mktime(&time_tm);
 	t_time_point = std::chrono::system_clock::from_time_t(tt);
-	return t_time_point;
+//    std::cout << " huli test :::: " << " time_tm.tt = " << tt << std::endl;
+//    std::cout << " huli test :::: " << " t_time_point = " << Time_tool::get_instance_references().get_time_string_seconds(t_time_point) << std::endl;
+    return t_time_point;
 }
 
 std::string Time_tool::get_time_string_millisecond(std::chrono::system_clock::time_point time_point)

测量节点/system/system_executor.cpp

@@ -640,7 +640,8 @@ Error_manager System_executor::encapsulate_send_mq_status()
 			t_multi_measure_info_msg.set_wheelbase(t_car_wheel_information.car_wheel_base);
 			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);
-			
+
+
 			auto t_ground_status = t_multi_status_msg.ground_status();
 			if(t_ground_status == message::Ground_statu::Car_correct)
 			{
@@ -660,10 +661,20 @@ Error_manager System_executor::encapsulate_send_mq_status()
 				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;
+
+
+            // 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());
+
+            if (t_multi_status_msg.id_struct().terminal_id() == 4)
+            {
+                LOG(WARNING) << "-------------------------------------------------------" <<std::endl;
+                LOG(WARNING) << "t_msg = " << t_msg <<std::endl;
+                LOG(WARNING) << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++" <<std::endl;
+                LOG(WARNING) << "t_multi_status_msg.ground_status() = " << t_ground_status <<std::endl;
+                LOG(WARNING) << "t_multi_measure_info_msg.set_ground_status(0) = " << t_multi_measure_info_msg.ground_status() <<std::endl;
+            }
 		}
 		if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
 		{

测量节点/velodyne_lidar/ground_region.cpp

@@ -657,28 +657,29 @@ 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.005)
+    if(min_p.x < m_region.border_minx()-0.000)
     {
         m_range_status_last |= Range_status::Range_left;
     }
-    else if(min_p.x > m_region.border_minx()+0.005)
+    else if(min_p.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(max_p.x > m_region.border_maxx()+0.005)
+    if(max_p.x > m_region.border_maxx()+0.000)
     {
         m_range_status_last |= Range_status::Range_right;
     }
-    else if(max_p.x < m_region.border_maxx()-0.005)
+    else if(max_p.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
 
+
     return m_range_status_last;
 #endif //ANTI_SHAKE
 }
@@ -765,14 +766,14 @@ 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.005    &&
-        t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_border_miny()-0.005 )
+    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 )
     {
         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.005    ||
-             t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) > m_region.plc_border_miny()+0.005 )
+    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 )
     {
         m_range_status_last |= Range_status::Range_back;
     }
@@ -780,13 +781,13 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     // LOG(WARNING) << m_range_status_last;
 
     // 判断车辆宽度超界情况
-    if (measure_result.car_wheel_width < m_region.car_min_width()-0.005 ||
-        measure_result.car_wheel_width > m_region.car_max_width()+0.005)
+    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)
     {
         m_range_status_last |= Range_status::Range_car_width;
     }
-    else if (measure_result.car_wheel_width > m_region.car_min_width()+0.005 &&
-             measure_result.car_wheel_width < m_region.car_max_width()-0.005)
+    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)
     {
         int t = Range_status::Range_car_width;
         m_range_status_last &= (~t);
@@ -795,13 +796,13 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     // LOG(WARNING) << m_range_status_last;
 
     // 判断车辆轴距超界情况
-    if (measure_result.car_wheel_base < m_region.car_min_wheelbase()-0.005 ||
-        measure_result.car_wheel_base > m_region.car_max_wheelbase()+0.005)
+    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;
     }
-    else if (measure_result.car_wheel_base > m_region.car_min_wheelbase()+0.005 ||
-             measure_result.car_wheel_base < m_region.car_max_wheelbase()-0.005)
+    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)
     {
         int t = Range_status::Range_car_wheelbase;
         m_range_status_last &= (~t);
@@ -815,11 +816,11 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     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
     //turnplate_angle_min = -5, turnplate_angle_max = 5,
-    if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()+0.05)
+    if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()+0.00)
     {
         m_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.05)
+    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;
         m_range_status_last &= (~t);
@@ -827,11 +828,11 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     // LOG(WARNING) << 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.05)
+    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;
     }
-    else if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()+0.05)
+    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);
@@ -840,6 +841,7 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     // 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
 
+
     m_range_status_last |= outOfRangeDetection(cloud, plate_cx, plate_cy, theta);
     return m_range_status_last;
 #endif