// // Created by huli on 2020/7/2. // #include "system_executor.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; PathCreator pc; pc.Mkdir("./log/"); 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.mutable_id_struct()->set_terminal_id(t_ground_detect_request_msg.id_struct().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()) && t_ground_detect_request_msg.id_struct().has_terminal_id()) { //往线程池添加执行任务, 之后会唤醒一个线程去执行他. m_thread_pool.enqueue(&System_executor::execute_for_measure, this, t_ground_detect_request_msg.command_key(), t_ground_detect_request_msg.id_struct().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 "); } } // 更新消息中关于定位结果与超界信息的内容 bool System_executor::update_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.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.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.range_status & Ground_region::Range_status::Range_front != 0) { t_error_str.append("前超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_back != 0) { t_error_str.append("后超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_left != 0) { t_error_str.append("左超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_right != 0) { t_error_str.append("右超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_bottom != 0) { t_error_str.append("底盘超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_top != 0) { t_error_str.append("顶超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_car_width != 0) { t_error_str.append("车宽超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_car_wheelbase != 0) { t_error_str.append("轴距超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_angle_anti_clock != 0) { t_error_str.append("车辆角度左超界 "); } if (measure_info.range_status & Ground_region::Range_status::Range_angle_clock != 0) { t_error_str.append("车辆角度右超界 "); } if (measure_info.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() { 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); t_ground_status_msg.mutable_id_struct()->set_terminal_id(m_terminal_id); // 创建各区域状态消息, // 注意！！！目前公共消息名字依旧使用wj，不做修改 // manager #if WJ_VELO == 1 { 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.mutable_id_struct()->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 < t_param.lidar_exts_size(); j++) { std::map::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 " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error"; } else { t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->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); // } update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size()); std::string t_msg = t_multi_status_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID) std::cout << t_multi_status_msg.DebugString() << std::endl << std::endl; } } #elif WJ_VELO == 0 { // wj_support 20220718 Wanji_manager::Wanji_manager_status t_wj_manager_status = Wanji_manager::get_instance_references().get_status(); t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_wj_manager_status); // lidar std::map t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map(); std::map t_wj_lidar_status_map; for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter) { t_wj_lidar_status_map.emplace(std::pair(iter->second->get_lidar_id(), iter->second->get_status())); } // region 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) { // 以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.mutable_id_struct()->set_terminal_id(iter->second->get_terminal_id()); t_multi_status_msg.set_region_worker_status((message::Region_worker_status)(iter->second->get_status())); wj::Region t_param = iter->second->get_param(); for (size_t j = 0; j < t_param.lidar_exts_size(); j++) { std::map::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id()); if (t_status_iter == t_wj_lidar_status_map.end()) { LOG(WARNING) << "lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error"; } else { t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->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); 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); // } update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size()); std::string t_msg = t_multi_status_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID) LOG(INFO) << t_multi_status_msg.DebugString(); } } // std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << std::endl; // std::cout << t_ground_status_msg.DebugString() << std::endl; // std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << t_ground_status_msg.DebugString() << std::endl; #else { // mix vlp16 and wj716 // 以多线雷达为主 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); // vlp16 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())); } // wj lidar #if LIDAR_TYPE == 1 std::map t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map(); std::map t_wj_lidar_status_map; for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter) { t_wj_lidar_status_map.emplace(std::pair(iter->second->get_lidar_id(), iter->second->get_status())); } #else std::map t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map(); std::map t_wj_lidar_status_map; for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter) { t_wj_lidar_status_map.emplace(std::pair(iter->second->get_lidar_id(), iter->second->get_status())); } #endif // wj region std::map t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map(); // vlp16 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.mutable_id_struct()->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 < t_param.lidar_exts_size(); j++) { std::map::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) << "vlp16 lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error"; } else { t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->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 == 10) { std::string t_filename = std::string("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt"; save_cloud_txt(t_region_cloud, t_filename); LOG(INFO) << " vlp16 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); // } // 检查vlp16区域与wj区域对应关系，wj存在对应区域则填充设备状态，并融合测量结果 auto t_wj_region_iter = t_region_worker_map.find(iter->first); if(t_wj_region_iter != t_region_worker_map.end()) { wj::Region t_param = t_wj_region_iter->second->get_param(); // for (size_t j = 0; j < t_param.lidar_exts_size(); j++) { #if LIDAR_TYPE == 1 std::map::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id()); #else std::map::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id()); #endif if (t_status_iter == t_wj_lidar_status_map.end()) { LOG(WARNING) << "wj lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error"; } else { t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second); } } //wj雷达的自动定位信息 Common_data::Car_wheel_information t_wj_car_wheel_information; t_error = t_wj_region_iter->second->get_last_wheel_information(&t_wj_car_wheel_information, std::chrono::system_clock::now()); // 获取区域点云填入信息 pcl::PointCloud::Ptr t_wj_region_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); t_wj_region_iter->second->get_region_cloud(t_wj_region_cloud); if (cloud_count == 8) { std::string t_filename = std::string("wj_region_") + std::to_string(t_wj_region_iter->first) + "_cloud.txt"; save_cloud_txt(t_wj_region_cloud, t_filename); LOG(INFO) << "wj region " << t_wj_region_iter->first << " cloud has been saved in " + t_filename; } // for (size_t j = 0; j < t_wj_region_cloud->size(); j++) // { // message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud(); // tp_cloud->set_x(t_wj_region_cloud->points[j].x); // tp_cloud->set_y(t_wj_region_cloud->points[j].y); // tp_cloud->set_z(t_wj_region_cloud->points[j].z); // } // LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<"\n"<second->get_terminal_id() == DISP_TERM_ID)<< "detect mismatch, region "<first<<", dx dy dang dwb: "<second->get_terminal_id() == DISP_TERM_ID)<<"region mismatch, vlp16 "<first<<", wj doesn't have corresponding region id"; } update_measure_info(t_multi_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|| t_multi_status_msg.id_struct().terminal_id() == 5|| t_multi_status_msg.id_struct().terminal_id() == 1|| t_multi_status_msg.id_struct().terminal_id() == 3|| t_multi_status_msg.id_struct().terminal_id() == 2) { // rabbitmq new measure info 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); t_multi_measure_info_msg.set_theta(t_car_wheel_information.car_angle); t_multi_measure_info_msg.set_length(0.0f); t_multi_measure_info_msg.set_width(t_car_wheel_information.car_wheel_width); t_multi_measure_info_msg.set_height(0.0f); 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) { 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 "<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); t_ground_status_msg.mutable_id_struct()->set_terminal_id(m_terminal_id); // 创建各区域状态消息, // 注意！！！目前公共消息名字依旧使用wj，不做修改 // manager #if WJ_VELO == 1 { 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.mutable_id_struct()->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 < t_param.lidar_exts_size(); j++) { std::map::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 " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error"; } else { t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->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); // } update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size()); std::string t_msg = t_multi_status_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID) std::cout << t_multi_status_msg.DebugString() << std::endl << std::endl; } } #elif WJ_VELO == 0 { // wj_support 20220718 Wanji_manager::Wanji_manager_status t_wj_manager_status = Wanji_manager::get_instance_references().get_status(); t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_wj_manager_status); // lidar std::map t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map(); std::map t_wj_lidar_status_map; for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter) { t_wj_lidar_status_map.emplace(std::pair(iter->second->get_lidar_id(), iter->second->get_status())); } // region 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) { // 以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.mutable_id_struct()->set_terminal_id(iter->second->get_terminal_id()); t_multi_status_msg.set_region_worker_status((message::Region_worker_status)(iter->second->get_status())); wj::Region t_param = iter->second->get_param(); for (size_t j = 0; j < t_param.lidar_exts_size(); j++) { std::map::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id()); if (t_status_iter == t_wj_lidar_status_map.end()) { LOG(WARNING) << "lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error"; } else { t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->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); // 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); // } update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size()); std::string t_msg = t_multi_status_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID) LOG(INFO) << t_multi_status_msg.DebugString(); } } // std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << std::endl; // std::cout << t_ground_status_msg.DebugString() << std::endl; // std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << t_ground_status_msg.DebugString() << std::endl; #else { // mix vlp16 and wj716 // 以多线雷达为主 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); // vlp16 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())); } // wj lidar #if LIDAR_TYPE == 1 std::map t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map(); std::map t_wj_lidar_status_map; for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter) { t_wj_lidar_status_map.emplace(std::pair(iter->second->get_lidar_id(), iter->second->get_status())); } #else std::map t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map(); std::map t_wj_lidar_status_map; for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter) { t_wj_lidar_status_map.emplace(std::pair(iter->second->get_lidar_id(), iter->second->get_status())); } #endif // wj region std::map t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map(); // vlp16 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.mutable_id_struct()->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 < t_param.lidar_exts_size(); j++) { std::map::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) << "vlp16 lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error"; } else { t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->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()); // 获取区域点云填入信息 // 20221211 changed by yct, nanomsg dont save cloud 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("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt"; // save_cloud_txt(t_region_cloud, t_filename); // LOG(INFO) << "vlp16 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); // } // 检查vlp16区域与wj区域对应关系，wj存在对应区域则填充设备状态，并融合测量结果 auto t_wj_region_iter = t_region_worker_map.find(iter->first); if(t_wj_region_iter != t_region_worker_map.end()) { wj::Region t_param = t_wj_region_iter->second->get_param(); // for (size_t j = 0; j < t_param.lidar_exts_size(); j++) { #if LIDAR_TYPE == 1 std::map::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id()); #else std::map::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id()); #endif if (t_status_iter == t_wj_lidar_status_map.end()) { LOG(WARNING) << "wj lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error"; } else { t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second); } } //wj雷达的自动定位信息 Common_data::Car_wheel_information t_wj_car_wheel_information; t_error = t_wj_region_iter->second->get_last_wheel_information(&t_wj_car_wheel_information, std::chrono::system_clock::now()); // 获取区域点云填入信息 pcl::PointCloud::Ptr t_wj_region_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); t_wj_region_iter->second->get_region_cloud(t_wj_region_cloud); // if (cloud_count == 8) // { // std::string t_filename = std::string("wj_region_") + std::to_string(t_wj_region_iter->first) + "_cloud.txt"; // save_cloud_txt(t_wj_region_cloud, t_filename); // LOG(INFO) << "wj region " << t_wj_region_iter->first << " cloud has been saved in " + t_filename; // } // for (size_t j = 0; j < t_wj_region_cloud->size(); j++) // { // message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud(); // tp_cloud->set_x(t_wj_region_cloud->points[j].x); // tp_cloud->set_y(t_wj_region_cloud->points[j].y); // tp_cloud->set_z(t_wj_region_cloud->points[j].z); // } // LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<"\n"<second->get_terminal_id() == DISP_TERM_ID)<< // "detect mismatch, region "<first<<", dx dy dang dwb: "<second->get_terminal_id() == DISP_TERM_ID)<<"region mismatch, vlp16 "<first<<", wj doesn't have corresponding region id"; } update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size()); std::string t_msg = t_multi_status_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); // if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID) // std::cout << t_multi_status_msg.DebugString() << std::endl // << std::endl; } } #endif return Error_code::SUCCESS; } //判断是否为待机，如果已经准备好，则可以执行任务。 bool System_executor::is_ready() { if (m_system_executor_status == SYSTEM_EXECUTOR_READY && m_thread_pool.thread_is_full_load() == false) { return true; } else { return false; } } System_executor::System_executor_status System_executor::get_system_executor_status() { return m_system_executor_status; } int System_executor::get_terminal_id() { return m_terminal_id; } //雷达感测定位 的处理函数 //input::command_id, 消息指令id, 由主控制系统生成的唯一码 //input::command_id, 终端id, 对应具体的某个车位 //return::void, 没有返回, 执行结果直接生成一条答复消息, 然后通过通信返回 void System_executor::execute_for_measure(std::string command_info, int terminal_id, std::chrono::system_clock::time_point receive_time) { Error_manager t_error; Error_manager t_result; LOG(INFO) << " System_executor::execute_for_measure run " << this; //第1步, 按照时间生成中间文件的保存路径 time_t nowTime; nowTime = time(NULL); struct tm *sysTime = localtime(&nowTime); char t_save_path[256] = {0}; sprintf(t_save_path, "../data/%04d%02d%02d_%02d%02d%02d", sysTime->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.mutable_id_struct()->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_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; }