// // Created by huli on 2021/8/3. // #include "dispatch_plc.h" #include "../system/system_communication.h" #include "dispatch_manager.h" Dispatch_plc::Dispatch_plc() { m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_UNKNOW; m_plc_id = 0; mp_execute_thread = NULL; } Dispatch_plc::~Dispatch_plc() { dispatch_plc_uninit(); } //调度plc 初始化 Error_manager Dispatch_plc::dispatch_plc_init(int plc_id) { m_plc_id = plc_id; m_status_updata_time = std::chrono::system_clock::now(); m_last_heartbeat = 0; // 线程默认开启 m_execute_condition.reset(false, true, false); mp_execute_thread = new std::thread(&Dispatch_plc::execute_thread_fun, this); m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY; return Error_code::SUCCESS; } //调度plc 反初始化 Error_manager Dispatch_plc::dispatch_plc_uninit() { if (mp_execute_thread) { m_execute_condition.kill_all(); } if (mp_execute_thread) { mp_execute_thread->join(); delete mp_execute_thread; mp_execute_thread = NULL; } m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_UNKNOW; return Error_code::SUCCESS; } //调度plc 执行请求 Error_manager Dispatch_plc::execute_for_dispatch_request_msg(message::Dispatch_request_msg& dispatch_request_msg) { if ( m_dispatch_plc_status == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY ) { std::unique_lock t_lock(m_lock); //设定超时时间, 默认比任务指令里面的时间少10秒, if ( dispatch_request_msg.base_info().has_timeout_ms() ) { m_timeout_ms = dispatch_request_msg.base_info().timeout_ms() - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS; } else { m_timeout_ms = DISPATCH_PROCESS_TIMEOUT_MS - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS; } m_command_key = dispatch_request_msg.command_key(); m_start_time = std::chrono::system_clock::now(); m_dispatch_request_msg = dispatch_request_msg; // if ( dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR ) // { // m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE; // } // else if ( dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR ) // { // m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP; // } // else // { // return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR, // " dispatch_request_msg.dispatch_motion_direction() PARAMRTER ERROR "); // } m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW; m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_REQUEST; } else { return Error_manager(Error_code::DISPATCH_PLC_STATUS_ERROR, Error_level::MINOR_ERROR, " m_dispatch_plc_status error "); } return Error_code::SUCCESS; } Dispatch_plc::Dispatch_plc_status Dispatch_plc::get_dispatch_plc_status() { return m_dispatch_plc_status; } message::Dispatch_request_msg Dispatch_plc::get_dispatch_request_msg() { std::unique_lock t_lock(m_lock); return m_dispatch_request_msg; } void Dispatch_plc::clear_request_msg() { m_request_key.clear(); //请求唯一码, 用作识别 m_request_status = 0; //请求确认标志 // 调度指令的起点,终点,方向 m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_UNKNOWN; //调度方向 0=未知，1=存车，2=取车 m_request_passageway_id = 0; //出入口id 6个入口和6个出口 m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_UNKNOWN; //出入口方向 0=未知，1=入口，2=出口 m_request_parkingspace_index_id = 0; //楼上车位索引id 1~180(3*6*13) m_request_parkingspace_unit_id = 0; //楼上车位单元号 1~3 m_request_parkingspace_label_id = 0; //楼上车位标签号 1~78 m_request_parkingspace_floor_id = 0; //楼上车位楼层号 2~11 m_request_parkingspace_room_id = 0; //楼上车位房间号 1~6 m_request_parkingspace_direction = Common_data::Parkingspace_direction::PARKINGSPACE_DIRECTION_UNKNOWN; //楼上车位方向 0=未知，1=朝南，2=朝北 //汽车的定位信息(地面雷达) m_request_car_center_x = 0; //整车的中心点x值, 四轮的中心, 单位:米 m m_request_car_center_y = 0; //整车的中心点y值, 四轮的中心, 单位:米 m m_request_car_angle = 0; //整车的车身旋转角, 单位:度 (-90~90) m_request_car_front_theta = 0; //整车的前轮的旋转角, 单位:度 (-90~90) m_request_car_length = 0; //整车的长度, 用于规避碰撞, 单位:米 m m_request_car_width = 0; //整车的宽度, 用于规避碰撞, 单位:米 m m_request_car_height = 0; //整车的高度, 用于规避碰撞, 单位:米 m m_request_car_wheel_base = 0; //整车的轮距, 前后轮的距离, 用于机器人或agv的抓车, 单位:米 m m_request_car_wheel_width = 0; //整车的轮距, 左右轮的距离, 用于机器人或agv的抓车, 单位:米 m m_request_car_license.clear(); //车牌号(汽车唯一标示) 例如: 鄂A12345 m_request_car_type = Common_data::Car_type::UNKNOW_CAR_TYPE; //车的大小 m_request_uniformed_car_x = 0; //转角复位后,车辆中心点x m_request_uniformed_car_y = 0; //转角复位后,车辆中心点y //防撞雷达 m_request_anticollision_lidar_flag = Common_data::Anticollision_lidar_flag::ANTICOLLISION_LIDAR_UNKNOWN; //汽车是否停到正确的位置, 防撞雷达 预留, 楚天暂时不用,0=未知，1=位置正常，2=位置异常 //轮距雷达 m_request_car_wheel_base_exact_value = 0; //汽车前后轮距,轮距雷达的精确值 预留, 楚天暂时不用,单位:米 m } //jiancha qingqiu xiaoxi Error_manager Dispatch_plc::check_dispatch_request_msg(message::Dispatch_request_msg & dispatch_request_msg) { Error_manager t_error; if(dispatch_request_msg.has_locate_information() && dispatch_request_msg.has_dispatch_motion_direction() && dispatch_request_msg.has_car_type() && dispatch_request_msg.has_id_struct() && dispatch_request_msg.has_command_key()) { if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR ) { if ( dispatch_request_msg.id_struct().has_unit_id() ) { } } else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR ) return Error_code::SUCCESS; } else { return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR, " m_dispatch_plc_status error "); } return Error_code::SUCCESS; } //执行外界任务的执行函数 void Dispatch_plc::execute_thread_fun() { LOG(INFO) << " Dispatch_plc::execute_thread_fun() start " << this; Error_manager t_error; while (m_execute_condition.is_alive()) { m_execute_condition.wait(); if (m_execute_condition.is_alive()) { std::this_thread::sleep_for(std::chrono::microseconds(1)); std::this_thread::sleep_for(std::chrono::seconds(1)); std::this_thread::yield(); // std::unique_lock t_lock(m_lock); static int t_count = 0; if (t_count % 10 == 0 ) { std::cout << " huli test :::: " << " m_plc_id = " << m_plc_id << std::endl; std::cout << " huli test :::: " << " m_dispatch_plc_status = " << m_dispatch_plc_status << std::endl; } t_count++; switch ((Dispatch_plc_status) m_dispatch_plc_status) { case DISPATCH_PLC_READY: { if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW ) { update_dispatch_plc_communication(); } else { m_result = check_dispatch_request_msg(m_dispatch_request_msg); if(m_result == Error_code::SUCCESS) { m_dispatch_plc_status = DISPATCH_PLC_REQUEST; } else { m_dispatch_plc_status = DISPATCH_PLC_RESPONSE; } } break; } case DISPATCH_PLC_REQUEST://给plc发送请求 { #ifdef PLC_OVER_TIME_TO_ERROR if ( std::chrono::system_clock::now() - m_start_time > std::chrono::milliseconds(m_timeout_ms) ) { //超时直接报错 m_result = Error_manager(Error_code::DISPATCH_PLC_TIME_OUT, Error_level::MINOR_ERROR, " DISPATCH_PLC_TIME_OUT fun error "); m_dispatch_plc_status = DISPATCH_PLC_RESPONSE; } else #endif { //封装 给plc的调度请求 m_result = encapsulate_dispatch_request_to_plc(); if ( m_result == Error_code::SUCCESS ) { m_dispatch_plc_status = DISPATCH_PLC_WORKING; } else { m_dispatch_plc_status = DISPATCH_PLC_RESPONSE; } } break; } case DISPATCH_PLC_WORKING: { #ifdef PLC_OVER_TIME_TO_ERROR if ( std::chrono::system_clock::now() - m_start_time > std::chrono::milliseconds(m_timeout_ms) ) { //超时直接报错 m_result = Error_manager(Error_code::DISPATCH_PLC_TIME_OUT, Error_level::MINOR_ERROR, " DISPATCH_PLC_TIME_OUT fun error "); m_dispatch_plc_status = DISPATCH_PLC_RESPONSE; } else #endif { update_dispatch_plc_communication(); //检查plc答复反馈 m_result = check_response_from_plc(); if ( m_result == Error_code::SUCCESS ) { m_dispatch_plc_status = DISPATCH_PLC_RESPONSE; } else if ( m_result != Error_code::NODATA ) { // m_dispatch_plc_status = DISPATCH_PLC_FAULT; m_dispatch_plc_status = DISPATCH_PLC_RESPONSE; } //else 原地等待 } break; } case DISPATCH_PLC_RESPONSE: { //20211213. huli m_request_status = 0 after over process m_request_status = 0; clear_request_msg(); //发送调度答复信息给主控 send_dispatch_response_to_main_control(); //无论前面的流程是否正确,都要给主控答复, 反馈里面填充错误码即可. if ( m_result == Error_code::SUCCESS ) { //response add to _map if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR ) { std::unique_lock t_lock(Dispatch_manager::get_instance_references().m_lock); std::chrono::system_clock::time_point t_time = std::chrono::system_clock::now(); Dispatch_manager::get_instance_references().m_dispatch_response_store_map[t_time] = m_dispatch_response_msg; Dispatch_manager::get_instance_references().m_store_updata_time = std::chrono::system_clock::now(); } else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR ) { std::unique_lock t_lock(Dispatch_manager::get_instance_references().m_lock); int t_terminal_id = m_dispatch_request_msg.id_struct().terminal_id(); Dispatch_manager::get_instance_references().m_dispatch_response_pickup_map[t_terminal_id] = m_dispatch_response_msg; Dispatch_manager::get_instance_references().m_pickup_updata_time = std::chrono::system_clock::now(); } //流程正常结束, 恢复到待机状态. m_dispatch_plc_status = DISPATCH_PLC_READY; m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW; m_result.error_manager_clear_all(); } else { //流程异常结束, 进入到故障状态. m_dispatch_plc_status = DISPATCH_PLC_FAULT; } break; } case DISPATCH_PLC_FAULT: { //20211213. huli m_request_status = 0 after over process m_request_status = 0; clear_request_msg(); //不在接受新的任务,但是保持基本的通信正常 update_dispatch_plc_communication(); //response add to _map if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR ) { std::unique_lock t_lock(Dispatch_manager::get_instance_references().m_lock); std::chrono::system_clock::time_point t_time = std::chrono::system_clock::now(); Dispatch_manager::get_instance_references().m_dispatch_response_store_map[t_time] = m_dispatch_response_msg; Dispatch_manager::get_instance_references().m_store_updata_time = std::chrono::system_clock::now(); } else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR ) { std::unique_lock t_lock(Dispatch_manager::get_instance_references().m_lock); int t_terminal_id = m_dispatch_request_msg.id_struct().terminal_id(); Dispatch_manager::get_instance_references().m_dispatch_response_pickup_map[t_terminal_id] = m_dispatch_response_msg; Dispatch_manager::get_instance_references().m_pickup_updata_time = std::chrono::system_clock::now(); } //20211209 huli //流程异常结束, 进入到 ready. m_dispatch_plc_status = DISPATCH_PLC_READY; m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW; m_result.error_manager_clear_all(); LOG(ERROR) << " Dispatch_plc::execute_thread_fun() dispatch_plc is fault, now recover to normal " << this; break; } } } } } //封装 给plc的调度请求 Error_manager Dispatch_plc::encapsulate_dispatch_request_to_plc() { //把m_dispatch_request_msg的信息传给本地的数据缓存. std::unique_lock t_lock(m_lock); m_request_key = m_dispatch_request_msg.command_key(); m_request_status = 1; if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR ) { m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_STORE; m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_INLET; m_request_passageway_id = m_dispatch_request_msg.mutable_id_struct()->terminal_id()+1;//0~5入口终端号改为1~6 } else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR ) { m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_PICKUP; m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_OUTLET; m_request_passageway_id = m_dispatch_request_msg.mutable_id_struct()->terminal_id()+1;//0~5入口终端号改为1~6 } else { m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_UNKNOWN; m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_UNKNOWN; m_request_passageway_id = -1; } if ( m_dispatch_request_msg.locate_information().locate_correct() ) { m_request_car_center_x = m_dispatch_request_msg.locate_information().locate_x(); m_request_car_center_y = m_dispatch_request_msg.locate_information().locate_y(); #ifdef MEASURE_TO_PLC_CORRECTION m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle()-90+0.3;//80~100改为-10~+10 #endif #ifndef MEASURE_TO_PLC_CORRECTION m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle(); #endif m_request_car_front_theta = m_dispatch_request_msg.locate_information().locate_front_theta(); m_request_car_length = m_dispatch_request_msg.locate_information().locate_length(); m_request_car_width = m_dispatch_request_msg.locate_information().locate_width(); m_request_car_height = m_dispatch_request_msg.locate_information().locate_height(); m_request_car_wheel_base = m_dispatch_request_msg.locate_information().locate_wheel_base(); m_request_car_wheel_width = m_dispatch_request_msg.locate_information().locate_wheel_width(); m_request_uniformed_car_x = m_dispatch_request_msg.locate_information().uniformed_car_x(); m_request_uniformed_car_y = m_dispatch_request_msg.locate_information().uniformed_car_y(); } else { m_request_car_center_x = m_dispatch_request_msg.locate_information().locate_x(); m_request_car_center_y = m_dispatch_request_msg.locate_information().locate_y(); #ifdef MEASURE_TO_PLC_CORRECTION m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle()-90+0.3;//80~100改为-10~+10 #endif #ifndef MEASURE_TO_PLC_CORRECTION m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle(); #endif m_request_car_front_theta = m_dispatch_request_msg.locate_information().locate_front_theta(); m_request_car_length = m_dispatch_request_msg.locate_information().locate_length(); m_request_car_width = m_dispatch_request_msg.locate_information().locate_width(); m_request_car_height = m_dispatch_request_msg.locate_information().locate_height(); m_request_car_wheel_base = m_dispatch_request_msg.locate_information().locate_wheel_base(); m_request_car_wheel_width = m_dispatch_request_msg.locate_information().locate_wheel_width(); m_request_uniformed_car_x = m_dispatch_request_msg.locate_information().uniformed_car_x(); m_request_uniformed_car_y = m_dispatch_request_msg.locate_information().uniformed_car_y(); } if ( m_dispatch_request_msg.parkspace_info_ex_size() ==1 ) { m_request_parkingspace_index_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_index_id(); m_request_parkingspace_unit_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_unit_id()+1;//0~2单元号改为1~3 m_request_parkingspace_label_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_label_id(); m_request_parkingspace_floor_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_floor_id(); m_request_parkingspace_room_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_room_id(); //车位朝向, 小号朝前朝南, 大号朝后朝北 m_request_parkingspace_direction = (Common_data::Parkingspace_direction)m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_direction(); //20211220, utf-8 -> GBK m_request_car_license = String_convert::utf8_to_gbk( m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_numberplate() ); m_request_car_type = (Common_data::Car_type)m_dispatch_request_msg.parkspace_info_ex(0).car_type(); //20211207, huli add wheel_base for pickup_car if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR ) { m_request_car_wheel_base = m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_wheel_base(); m_request_car_wheel_width = m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_wheel_width(); } } else { return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR, " m_dispatch_request_msg.parkspace_info_ex_size() !=1 error "); } return Error_code::SUCCESS; } //更新plc通信 Error_manager Dispatch_plc::update_dispatch_plc_communication() { std::unique_lock t_lock(m_lock); std::unique_lock t_lock1(Dispatch_communication::get_instance_references().m_data_lock); /* //请求消息, 调度->plc Dispatch_communication::Dispatch_request_from_manager_to_plc_for_data * tp_dispatch_request_from_manager_to_plc_for_data = & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_data; Dispatch_communication::Dispatch_request_from_manager_to_plc_for_key * m_dispatch_request_from_manager_to_plc_for_key = & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_key; memset(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, 0, 50); int t_size1 = m_request_key.size()<=50 ? m_request_key.size() : 50 ; m_request_key = "123456789"; memcpy(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size1); static unsigned char index = 0; index++; tp_dispatch_request_from_manager_to_plc_for_data->m_request_dispatch_motion_direction = index; tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_id = 02; tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_direction = 03; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_index_id = 04; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_unit_id = 05; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_floor_id = 6; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_room_id = 7; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_direction = 8; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_x = 11; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_y = 12; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_angle = 13; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_front_theta = 14; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_length = 15; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_width = 16; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_height = 17; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base = 18; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_width = 19; memset(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, 0, 20); int t_size2 = m_request_key.size()<=20 ? m_request_key.size() : 20 ; m_request_car_license = "ABCDEF"; memcpy(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, m_request_car_license.c_str(), t_size2); tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_type = 33; tp_dispatch_request_from_manager_to_plc_for_data->m_request_anticollision_lidar_flag = 44; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base_exact_value = 55; */ //请求消息, 调度->plc Dispatch_communication::Dispatch_request_from_manager_to_plc_for_data * tp_dispatch_request_from_manager_to_plc_for_data = & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_data; Dispatch_communication::Dispatch_request_from_manager_to_plc_for_key * m_dispatch_request_from_manager_to_plc_for_key = & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_key; memset(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, 0, 50); int t_size1 = m_request_key.size()<=50 ? m_request_key.size() : 50 ; memcpy(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size1); tp_dispatch_request_from_manager_to_plc_for_data->m_request_status = m_request_status; tp_dispatch_request_from_manager_to_plc_for_data->m_request_dispatch_motion_direction = m_request_dispatch_motion_direction; tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_id = m_request_passageway_id; tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_direction = m_request_passageway_direction; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_index_id = m_request_parkingspace_index_id; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_unit_id = m_request_parkingspace_unit_id; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_label_id = m_request_parkingspace_label_id; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_floor_id = m_request_parkingspace_floor_id; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_room_id = m_request_parkingspace_room_id; tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_direction = m_request_parkingspace_direction; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_x = m_request_car_center_x; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_y = m_request_car_center_y; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_angle = m_request_car_angle; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_front_theta = m_request_car_front_theta; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_length = m_request_car_length; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_width = m_request_car_width; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_height = m_request_car_height; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base = m_request_car_wheel_base; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_width = m_request_car_wheel_width; memset(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, 0, 20); tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license[0] = 18; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license[1] = m_request_car_license.length(); unsigned char * p_ch = tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license; memcpy(p_ch+2, m_request_car_license.c_str(), m_request_car_license.length()); tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_type = m_request_car_type; tp_dispatch_request_from_manager_to_plc_for_data->m_request_uniformed_car_x = m_request_uniformed_car_x; tp_dispatch_request_from_manager_to_plc_for_data->m_request_uniformed_car_y = m_request_uniformed_car_y; tp_dispatch_request_from_manager_to_plc_for_data->m_request_anticollision_lidar_flag = m_request_anticollision_lidar_flag; tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base_exact_value = m_request_car_wheel_base_exact_value; //答复消息, plc->调度 Dispatch_communication::Dispatch_response_from_plc_to_manager * tp_dispatch_response_from_plc_to_manager = & Dispatch_communication::get_instance_references().m_dispatch_response_from_plc_to_manager; m_response_key = (char*) tp_dispatch_response_from_plc_to_manager->m_response_key; m_response_status = (Dispatch_plc::Response_status)tp_dispatch_response_from_plc_to_manager->m_response_status; m_response_dispatch_motion_direction = (Common_data::Dispatch_motion_direction)tp_dispatch_response_from_plc_to_manager->m_response_dispatch_motion_direction; m_response_passageway_id = tp_dispatch_response_from_plc_to_manager->m_response_passageway_id; m_response_passageway_direction = (Common_data::Passageway_direction)tp_dispatch_response_from_plc_to_manager->m_response_passageway_direction; m_response_parkingspace_index_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_index_id; m_response_parkingspace_unit_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_unit_id; m_response_parkingspace_label_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_label_id; m_response_parkingspace_floor_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_floor_id; m_response_parkingspace_room_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_room_id; m_response_parkingspace_direction = (Common_data::Parkingspace_direction)tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_direction; m_response_car_center_x = tp_dispatch_response_from_plc_to_manager->m_response_car_center_x; m_response_car_center_y = tp_dispatch_response_from_plc_to_manager->m_response_car_center_y; m_response_car_angle = tp_dispatch_response_from_plc_to_manager->m_response_car_angle; m_response_car_front_theta = tp_dispatch_response_from_plc_to_manager->m_response_car_front_theta; m_response_car_length = tp_dispatch_response_from_plc_to_manager->m_response_car_length; m_response_car_width = tp_dispatch_response_from_plc_to_manager->m_response_car_width; m_response_car_height = tp_dispatch_response_from_plc_to_manager->m_response_car_height; m_response_car_wheel_base = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_base; m_response_car_wheel_width = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_width; m_response_car_license = (char*) (tp_dispatch_response_from_plc_to_manager->m_response_car_license+2); m_response_car_type = (Common_data::Car_type) tp_dispatch_response_from_plc_to_manager->m_response_car_type; m_response_uniformed_car_x = tp_dispatch_response_from_plc_to_manager->m_response_uniformed_car_x; m_response_uniformed_car_y = tp_dispatch_response_from_plc_to_manager->m_response_uniformed_car_y; m_response_anticollision_lidar_flag = (Common_data::Anticollision_lidar_flag)tp_dispatch_response_from_plc_to_manager->m_response_anticollision_lidar_flag; m_response_car_wheel_base_exact_value = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_base_exact_value; /* std::cout << " huli test :::: " << " ------------------------------------------------------ = " << std::endl; std::cout << " huli test :::: " << " m_response_key = " << m_response_key << std::endl; std::cout << " huli test :::: " << " m_response_status = " << (int)m_response_status << std::endl; std::cout << " huli test :::: " << " m_response_dispatch_motion_direction = " << (int)m_response_dispatch_motion_direction << std::endl; std::cout << " huli test :::: " << " m_response_passageway_id = " << m_response_passageway_id << std::endl; std::cout << " huli test :::: " << " m_response_passageway_direction = " << (int)m_response_passageway_direction << std::endl; std::cout << " huli test :::: " << " m_response_parkingspace_index_id = " << m_response_parkingspace_index_id << std::endl; std::cout << " huli test :::: " << " m_response_parkingspace_unit_id = " << m_response_parkingspace_unit_id << std::endl; std::cout << " huli test :::: " << " m_response_parkingspace_floor_id = " << m_response_parkingspace_floor_id << std::endl; std::cout << " huli test :::: " << " m_response_parkingspace_room_id = " << m_response_parkingspace_room_id << std::endl; std::cout << " huli test :::: " << " m_response_parkingspace_direction = " << (int)m_response_parkingspace_direction << std::endl; std::cout << " huli test :::: " << " m_response_car_center_x = " << m_response_car_center_x << std::endl; std::cout << " huli test :::: " << " m_response_car_center_y = " << m_response_car_center_y << std::endl; std::cout << " huli test :::: " << " m_response_car_license = " << m_response_car_license << std::endl; */ //状态消息, plc->调度 Dispatch_communication::Dispatch_plc_status_from_plc_to_manager * tp_dispatch_plc_status_from_plc_to_manager = & Dispatch_communication::get_instance_references().m_dispatch_plc_status_from_plc_to_manager; //need // //通过心跳帧来判断通信是否正常 // if ( m_last_heartbeat != tp_dispatch_plc_status_from_plc_to_manager->m_heartbeat ) // { // m_last_heartbeat = tp_dispatch_plc_status_from_plc_to_manager->m_heartbeat; // m_status_updata_time = std::chrono::system_clock::now(); // // if ( m_dispatch_plc_status == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_DISCONNECT ) // { // m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY; // } // } // else if(std::chrono::system_clock::now() - m_status_updata_time > std::chrono::milliseconds(COMMUNICATION_OVER_TIME_MS)) // { // m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_DISCONNECT; // } // //else 继续等待,直到消息刷新或者超时. return Error_code::SUCCESS; } //检查plc答复反馈 Error_manager Dispatch_plc::check_response_from_plc() { std::unique_lock t_lock(m_lock); #ifndef WAIT_PLC_RESPONSE //need, 调试代码, 延时10s, 直接返回成功 if ( std::chrono::system_clock::now() - m_start_time > std::chrono::seconds(20) ) { return Error_code::SUCCESS; } //返回没有收到数据 else { return Error_code::NODATA; } #endif #ifdef WAIT_PLC_RESPONSE //检查唯一码 if ( m_response_key == m_request_key ) { std::cout<< "m_response_status = " << m_response_status << std::endl; //如果故障,则添加错误码 if ( m_response_status != RESPONS_OVER ) { //添加错误码 Error_manager t_error(DISPATCH_PLC_RESPONS_ERROR, MINOR_ERROR, " Dispatch_plc::check_response_from_plc() m_respons_status is error"); return t_error; } return Error_code::SUCCESS; } //返回没有收到数据 else { return Error_code::NODATA; } #endif return Error_code::SUCCESS; } //发送调度答复信息给主控 Error_manager Dispatch_plc::send_dispatch_response_to_main_control() { std::unique_lock t_lock(m_lock); m_dispatch_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_response_msg); m_dispatch_response_msg.mutable_base_info()->set_timeout_ms(m_dispatch_request_msg.base_info().timeout_ms()); m_dispatch_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager); m_dispatch_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain); m_dispatch_response_msg.set_command_key(m_dispatch_request_msg.command_key()); m_dispatch_response_msg.set_dispatch_motion_direction(m_dispatch_request_msg.dispatch_motion_direction()); m_dispatch_response_msg.mutable_id_struct()->set_terminal_id(m_dispatch_request_msg.mutable_id_struct()->terminal_id()); m_dispatch_response_msg.mutable_id_struct()->set_unit_id(m_dispatch_request_msg.mutable_id_struct()->unit_id()); m_dispatch_response_msg.mutable_locate_information()->CopyFrom(m_dispatch_request_msg.locate_information()); m_dispatch_response_msg.mutable_parkspace_info_ex()->CopyFrom(m_dispatch_request_msg.parkspace_info_ex()); m_dispatch_response_msg.set_car_type(m_dispatch_request_msg.car_type()); if ( m_dispatch_response_msg.dispatch_motion_direction() == message::E_STORE_CAR ) { for (int i = 0; i < m_dispatch_response_msg.parkspace_info_ex_size(); ++i) { if ( m_result == Error_code::SUCCESS ) { m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_occupied); } else { m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_empty); } } } else if( m_dispatch_response_msg.dispatch_motion_direction() == message::E_PICKUP_CAR ) { for (int i = 0; i < m_dispatch_response_msg.parkspace_info_ex_size(); ++i) { if ( m_result == Error_code::SUCCESS ) { m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_empty); } else { m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_occupied); } } } m_dispatch_response_msg.mutable_error_manager()->set_error_code(m_result.get_error_code()); m_dispatch_response_msg.mutable_error_manager()->set_error_level((message::Error_level)m_result.get_error_level()); m_dispatch_response_msg.mutable_error_manager()->set_error_description(m_result.get_error_description()); std::cout << " huli test :::: " << " m_dispatch_response_msg = " << m_dispatch_response_msg.DebugString()<< std::endl; std::string t_msg = m_dispatch_response_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); return Error_code::SUCCESS; }