// // Created by huli on 2020/7/2. // #include "system_executor.h" #include "../tool/common_data.h" #include "../tool/measure_filter.h" #include "../message/measure_message.pb.h" #include "../system/system_communication.h" #include "../wanji_lidar/wanji_manager.h" #include "../velodyne_lidar/velodyne_manager.h" // std::ofstream g_debug_file; // 用于测试滤波功能 System_executor::System_executor() { } System_executor::~System_executor() { system_executor_uninit(); } //初始化 Error_manager System_executor::system_executor_init(int threads_size, int terminal_id) { m_thread_pool.thread_pool_init(threads_size); m_system_executor_status = SYSTEM_EXECUTOR_READY; m_terminal_id = terminal_id; return Error_code::SUCCESS; } //反初始化 Error_manager System_executor::system_executor_uninit() { // if(g_debug_file.is_open()) // g_debug_file.close(); m_thread_pool.thread_pool_uninit(); m_system_executor_status = SYSTEM_EXECUTOR_UNKNOW; return Error_code::SUCCESS; } //检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的. Error_manager System_executor::check_msg(Communication_message* p_msg) { if ( p_msg == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } //检查消息类型 switch ( p_msg->get_message_type() ) { case Communication_message::Message_type::eGround_detect_request_msg: { //检查接受人 if ( p_msg->get_receiver() == Communication_message::Communicator::eGround_measurer) { message::Ground_detect_request_msg t_ground_detect_request_msg; //针对消息类型, 对消息进行二次解析 if (t_ground_detect_request_msg.ParseFromString(p_msg->get_message_buf())) { // //检查终端id // 普爱不检查终端id // 楚天检查终端id // if ( t_ground_detect_request_msg.terminal_id() == m_terminal_id ) // { return Error_code::SUCCESS; // } } } break; } default : ; break; } //无效的消息, return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR, " INVALID_MESSAGE error "); } //检查执行者的状态, 判断能否处理这条消息, Error_manager System_executor::check_executer(Communication_message* p_msg) { //huli DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDD 记得删除 return Error_code::SUCCESS; if ( p_msg == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } Error_manager t_error; //通过 p_msg->get_message_type() 和 p_msg->get_receiver() 找到处理模块的实例对象, 查询执行人是否可以处理这条消息 switch ( p_msg->get_message_type() ) { case Communication_message::Message_type::eGround_detect_request_msg: { Error_manager t_executor_result = System_executor::get_instance_references().check_status(); if (t_executor_result == SUCCESS) { return Error_code::SUCCESS; } else { //整合所有的错误码 t_error.compare_and_cover_error(t_executor_result); if (t_error.get_error_level() == NEGLIGIBLE_ERROR)//一级故障,轻微故障, { LOG(INFO) << "executer_is_busy , "; //返回繁忙之后, 通信模块1秒后再次调用check return Error_code::COMMUNICATION_EXCUTER_IS_BUSY; } else//返回二级故障,可以封装一条答复信息, 返回错误码 { message::Ground_detect_request_msg t_ground_detect_request_msg; //针对消息类型, 对消息进行二次解析 if (t_ground_detect_request_msg.ParseFromString(p_msg->get_message_buf())) { //创建一条答复消息 message::Ground_detect_response_msg t_ground_detect_response_msg; t_ground_detect_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eGround_detect_response_msg); t_ground_detect_response_msg.mutable_base_info()->set_timeout_ms(5000); t_ground_detect_response_msg.mutable_base_info()->set_sender(message::Communicator::eGround_measurer); t_ground_detect_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain); t_ground_detect_response_msg.set_command_key(t_ground_detect_request_msg.command_key()); t_ground_detect_response_msg.set_terminal_id(t_ground_detect_request_msg.terminal_id()); t_ground_detect_response_msg.mutable_error_manager()->set_error_code(t_error.get_error_code()); t_ground_detect_response_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level()); t_ground_detect_response_msg.mutable_error_manager()->set_error_description(t_error.get_error_description()); std::string t_msg = t_ground_detect_response_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); LOG(INFO) << " System_executor::check_executer executer status error "<< this; return t_error; } else { LOG(INFO) << " System_executor::check_executer second PARSE_ERROR "<< this; return Error_manager(Error_code::SYSTEM_EXECUTOR_PARSE_ERROR, Error_level::MINOR_ERROR, " message::Measure_request_msg ParseFromString error "); } } } break; } default : ; break; } return t_error; } //处理消息的执行函数 Error_manager System_executor::execute_msg(Communication_message* p_msg) { if ( p_msg == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } // std::cout << " huli test ::333333333333333333333333333333:: " << " p_msg->get_message_type() = " << p_msg->get_message_type() << std::endl; switch ( p_msg->get_message_type() ) { case Communication_message::eGround_detect_request_msg: { message::Ground_detect_request_msg t_ground_detect_request_msg; //针对消息类型, 对消息进行二次解析 if (t_ground_detect_request_msg.ParseFromString(p_msg->get_message_buf())) { //往线程池添加执行任务, 之后会唤醒一个线程去执行他. m_thread_pool.enqueue(&System_executor::execute_for_measure, this, t_ground_detect_request_msg.command_key(), t_ground_detect_request_msg.terminal_id(), p_msg->get_receive_time()); } else { return Error_manager(Error_code::SYSTEM_EXECUTOR_PARSE_ERROR, Error_level::MINOR_ERROR, " message::Measure_request_msg ParseFromString error "); } break; } default: { } } return Error_code::SUCCESS; } //检查状态 Error_manager System_executor::check_status() { if ( m_system_executor_status == SYSTEM_EXECUTOR_READY ) { if ( m_thread_pool.thread_is_full_load() == false ) { return Error_code::SUCCESS; } else { return Error_manager(Error_code::SYSTEM_EXECUTOR_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR, " System_executor::check_status error "); } } else { return Error_manager(Error_code::SYSTEM_EXECUTOR_STATUS_ERROR, Error_level::MINOR_ERROR, " System_executor::check_status error "); } } //定时发送状态信息 Error_manager System_executor::encapsulate_send_status() { static int cloud_count=-1; cloud_count++; // if(!g_debug_file.is_open()) // { // g_debug_file.open("./filter_debug_result.txt", std::ios::app); // } Error_manager t_error; //创建一条状态消息 message::Ground_status_msg t_ground_status_msg; t_ground_status_msg.mutable_base_info()->set_msg_type(message::Message_type::eGround_status_msg); t_ground_status_msg.mutable_base_info()->set_timeout_ms(5000); t_ground_status_msg.mutable_base_info()->set_sender(message::Communicator::eGround_measurer); t_ground_status_msg.mutable_base_info()->set_receiver(message::Communicator::eEmpty); t_ground_status_msg.set_terminal_id(m_terminal_id); // 创建各区域状态消息, // 注意！！！目前公共消息名字依旧使用wj，不做修改 // manager Velodyne_manager::Velodyne_manager_status t_velodyne_manager_status = Velodyne_manager::get_instance_references().get_status(); t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_velodyne_manager_status); // lidar std::map t_velodyne_lidar_device_map = Velodyne_manager::get_instance_references().get_velodyne_lidar_device_map(); std::map t_velodyne_lidar_status_map; for (auto iter = t_velodyne_lidar_device_map.begin(); iter != t_velodyne_lidar_device_map.end(); ++iter) { t_velodyne_lidar_status_map.emplace(std::pair(iter->second->get_lidar_id(), iter->second->get_status())); } // region std::map t_ground_region_map = Velodyne_manager::get_instance_references().get_ground_region_map(); int region_index = 0; for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter) { // 以t_ground_status_msg为模板创建各区域心跳消息 message::Ground_status_msg t_multi_status_msg; t_multi_status_msg.CopyFrom(t_ground_status_msg); t_multi_status_msg.set_terminal_id(iter->second->get_terminal_id()); t_multi_status_msg.set_region_worker_status((message::Region_worker_status)(iter->second->get_status())); velodyne::Region t_param = iter->second->get_param(); for (size_t j = 0; j ::iterator t_status_iter = t_velodyne_lidar_status_map.find(t_param.lidar_exts(j).lidar_id()); if(t_status_iter== t_velodyne_lidar_status_map.end()) { LOG(WARNING) << "lidar status "<second); } } //velodyne雷达的自动定位信息 Common_data::Car_wheel_information t_car_wheel_information; t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now()); // 获取区域点云填入信息 pcl::PointCloud::Ptr t_region_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered); if(cloud_count == 5) { std::string t_filename = std::string("region_")+std::to_string(iter->first)+"_cloud.txt"; save_cloud_txt(t_region_cloud, t_filename); LOG(INFO) << "region "<< iter->first <<" cloud has been saved in " + t_filename; } for (size_t j = 0; j < t_region_cloud->size(); j++) { message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud(); tp_cloud->set_x(t_region_cloud->points[j].x); tp_cloud->set_y(t_region_cloud->points[j].y); tp_cloud->set_z(t_region_cloud->points[j].z); } //无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确. message::Locate_information t_locate_information; //= t_multi_status_msg.add_locate_information_realtime(); t_locate_information.set_locate_x(t_car_wheel_information.car_center_x); t_locate_information.set_locate_y(t_car_wheel_information.car_center_y); t_locate_information.set_locate_angle(t_car_wheel_information.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(t_car_wheel_information.car_wheel_base); t_locate_information.set_locate_wheel_width(t_car_wheel_information.car_wheel_width); t_locate_information.set_locate_front_theta(t_car_wheel_information.car_front_theta); t_locate_information.set_locate_correct(t_car_wheel_information.correctness); t_locate_information.set_uniformed_car_x(t_car_wheel_information.uniform_car_x); 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 == int(Ground_region::Range_status::Range_correct)) { 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()); t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level()); t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length()); std::string t_msg = t_multi_status_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); // std::cout< & t_wanji_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map(); for (auto iter = t_wanji_lidar_device_map.begin(); iter != t_wanji_lidar_device_map.end(); ++iter) { Wanji_lidar_device::Wanji_lidar_device_status t_wanji_lidar_device_status = (*iter).second->get_status(); t_ground_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_wanji_lidar_device_status); } std::map & t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map(); int region_index = 0; for (auto iter = t_region_worker_map.begin(); iter != t_region_worker_map.end(); ++iter) { message::Ground_status_msg t_multi_status_msg; t_multi_status_msg.CopyFrom(t_ground_status_msg); t_multi_status_msg.set_terminal_id(region_index++); Region_worker::Region_worker_status t_region_worker_status = (*iter).second->get_status(); // t_multi_status_msg.add_region_worker_status((message::Region_worker_status)t_region_worker_status); t_multi_status_msg.set_region_worker_status((message::Region_worker_status)t_region_worker_status); //万集雷达的自动定位信息 Common_data::Car_wheel_information t_car_wheel_information; t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now()); //无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确. message::Locate_information t_locate_information; //= t_multi_status_msg.add_locate_information_realtime(); t_locate_information.set_locate_x(t_car_wheel_information.center_x); t_locate_information.set_locate_y(t_car_wheel_information.center_y); t_locate_information.set_locate_angle(t_car_wheel_information.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(t_car_wheel_information.wheel_base); t_locate_information.set_locate_wheel_width(t_car_wheel_information.wheel_width); t_locate_information.set_locate_front_theta(t_car_wheel_information.front_theta); t_locate_information.set_locate_correct(t_car_wheel_information.correctness); t_multi_status_msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information); t_multi_status_msg.set_ground_status(t_car_wheel_information.correctness?message::Ground_statu::Car_correct:message::Ground_statu::Noise); t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code()); t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level()); t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length()); std::string t_msg = t_multi_status_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); // // 记录滤波前后测量数据，用于判断测量精度变化 // static int t_line_count = 0; // if(region_index == 6 && ++t_line_count < 120) { // if (g_debug_file.is_open() && t_car_wheel_information.correctness) { // g_debug_file << "basic:"<tm_year + 1900, sysTime->tm_mon + 1, sysTime->tm_mday, sysTime->tm_hour, sysTime->tm_min, sysTime->tm_sec); //第2步, 创建万集管理模块的任务单, 并发送到 wanji_manager // Wanji_manager_task t_wanji_manager_task ; // Common_data::Car_wheel_information t_car_information_by_wanji; // t_wanji_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(3000), // terminal_id, receive_time); // t_error = Task_command_manager::get_instance_references().execute_task(&t_wanji_manager_task); // 或者创建velodyne管理模块任务单，并发送到velodyne_manager Velodyne_manager_task t_velodyne_manager_task; Common_data::Car_wheel_information t_car_information_by_velodyne; t_velodyne_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(3000), terminal_id, receive_time); t_error = Task_command_manager::get_instance_references().execute_task(&t_velodyne_manager_task); //第3步, 等待任务单完成 if ( t_error != Error_code::SUCCESS ) { LOG(INFO) << " Velodyne_manager/Velodyne_manager execute_task error "<< this; } else { // //判断任务是否结束, 阻塞 等待, 直到任务完成或者超时 // while ( t_wanji_manager_task.is_task_end() == false) // { // if ( t_wanji_manager_task.is_over_time() ) // { // //超时处理。取消任务。 // Wanji_manager::get_instance_pointer()->cancel_task(&t_wanji_manager_task); // t_error.error_manager_reset(Error_code::WJ_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR, // " t_wanjestaui_manager_task is_over_time "); // t_wanji_manager_task.set_task_error_manager(t_error); // } // else // { // //继续等待 // std::this_thread::sleep_for(std::chrono::microseconds(1)); // std::this_thread::yield(); // } // } // //提取任务单 的错误码 // t_error = t_wanji_manager_task.get_task_error_manager(); // if ( t_error == Error_code::SUCCESS ) // { // t_car_information_by_wanji = t_wanji_manager_task.get_car_wheel_information(); // } // else // { // LOG(INFO) << " wanji_manager_task error :::::::" << t_wanji_manager_task.get_task_error_manager().to_string() << this; // } // ------------------- 切换为velodyne ------------------ //判断任务是否结束, 阻塞 等待, 直到任务完成或者超时 while ( t_velodyne_manager_task.is_task_end() == false) { if ( t_velodyne_manager_task.is_over_time() ) { //超时处理。取消任务。 Velodyne_manager::get_instance_pointer()->cancel_task(&t_velodyne_manager_task); t_error.error_manager_reset(Error_code::VELODYNE_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR, " t_velodyne_manager_task is_over_time "); t_velodyne_manager_task.set_task_error_manager(t_error); } else { //继续等待 std::this_thread::sleep_for(std::chrono::microseconds(1)); std::this_thread::yield(); } } //提取任务单 的错误码 t_error = t_velodyne_manager_task.get_task_error_manager(); if ( t_error == Error_code::SUCCESS ) { t_car_information_by_velodyne = t_velodyne_manager_task.get_car_wheel_information(); } else { LOG(INFO) << " velodyne_manager_task error :::::::" << t_velodyne_manager_task.get_task_error_manager().to_string() << this; } } t_result.compare_and_cover_error(t_error); //measure 测量模块的最终结果 Common_data::Car_measure_information t_car_information_result; //第4步, 生成反馈消息 // if(t_car_information_by_wanji.correctness == true ) // { // t_car_information_result.center_x = t_car_information_by_wanji.center_x; // t_car_information_result.center_y = t_car_information_by_wanji.center_y; // t_car_information_result.car_angle = t_car_information_by_wanji.car_angle; // t_car_information_result.wheel_base = t_car_information_by_wanji.wheel_base; // t_car_information_result.wheel_width = t_car_information_by_wanji.wheel_width; // t_car_information_result.correctness = true; // }else{ // t_result.set_error_level_down(NEGLIGIBLE_ERROR); // } // ------------------- 切换为velodyne ------------------ if(t_car_information_by_velodyne.correctness == true ) { t_car_information_result.car_center_x = t_car_information_by_velodyne.car_center_x; t_car_information_result.car_center_y = t_car_information_by_velodyne.car_center_y; t_car_information_result.car_angle = t_car_information_by_velodyne.car_angle; t_car_information_result.car_wheel_base = t_car_information_by_velodyne.car_wheel_base; t_car_information_result.car_wheel_width = t_car_information_by_velodyne.car_wheel_width; t_car_information_result.correctness = true; }else{ t_result.set_error_level_down(NEGLIGIBLE_ERROR); } //第七步, 创建一条答复消息 message::Ground_detect_response_msg t_ground_detect_response_msg; t_ground_detect_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eGround_detect_response_msg); t_ground_detect_response_msg.mutable_base_info()->set_timeout_ms(5000); t_ground_detect_response_msg.mutable_base_info()->set_sender(message::Communicator::eGround_measurer); t_ground_detect_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain); t_ground_detect_response_msg.set_command_key(command_info); t_ground_detect_response_msg.set_terminal_id(terminal_id); t_ground_detect_response_msg.mutable_error_manager()->set_error_code(t_result.get_error_code()); t_ground_detect_response_msg.mutable_error_manager()->set_error_level((message::Error_level)t_result.get_error_level()); t_ground_detect_response_msg.mutable_error_manager()->set_error_description(t_result.get_error_description(), t_result.get_description_length()); t_ground_detect_response_msg.mutable_locate_information()->set_locate_x(t_car_information_result.car_center_x); t_ground_detect_response_msg.mutable_locate_information()->set_locate_y(t_car_information_result.car_center_y); t_ground_detect_response_msg.mutable_locate_information()->set_locate_angle(t_car_information_result.car_angle); t_ground_detect_response_msg.mutable_locate_information()->set_locate_length(t_car_information_result.car_length); t_ground_detect_response_msg.mutable_locate_information()->set_locate_width(t_car_information_result.car_width); t_ground_detect_response_msg.mutable_locate_information()->set_locate_height(t_car_information_result.car_height); t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_base(t_car_information_result.car_wheel_base); t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_width(t_car_information_result.car_wheel_width); t_ground_detect_response_msg.mutable_locate_information()->set_locate_front_theta(t_car_information_result.car_front_theta); t_ground_detect_response_msg.mutable_locate_information()->set_locate_correct(t_car_information_result.correctness); std::string t_msg = t_ground_detect_response_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); std::cout << "huli t_measure_response_msg = " << t_ground_detect_response_msg.DebugString() << std::endl; return ; }