chutian_dispatch/dispatch/dispatch_plc.cpp

//
// 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<std::mutex> 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<std::mutex> 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_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())
    {
        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<std::mutex> 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发送请求
				{
					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
					{
						if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
						{
							//封装 给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;
							}
						}
						else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
						{
							//判断2个出口是否空闲
							if ( Dispatch_manager::get_instance_references().m_dispatch_singlechip[2].is_outlet_ready() )
							{
								m_outlet_id = 0;
								//封装 给plc的调度请求
								m_result = encapsulate_dispatch_request_to_plc(0);
								if ( m_result == Error_code::SUCCESS )
								{
									m_dispatch_plc_status = DISPATCH_PLC_WORKING;
								}
								else
								{
									m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
								}
							}
							else if ( Dispatch_manager::get_instance_references().m_dispatch_singlechip[3].is_outlet_ready() )
							{
								m_outlet_id = 1;
								//封装 给plc的调度请求
								m_result = encapsulate_dispatch_request_to_plc(1);
								if ( m_result == Error_code::SUCCESS )
								{
									m_dispatch_plc_status = DISPATCH_PLC_WORKING;
								}
								else
								{
									m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
								}
							}
							//else无限等待
						}
						else
						{
							m_result =  Error_manager(Error_code::DISPATCH_PLC_DIRECTION_ERROR, Error_level::MINOR_ERROR,
													  " DISPATCH_PLC_DIRECTION_ERROR fun error ");
							m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
						}
					}
					break;
				}
				case DISPATCH_PLC_WORKING:
				{
					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
					{
						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<std::mutex> 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<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
							Dispatch_manager::get_instance_references().m_dispatch_response_pickup_map[m_outlet_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<std::mutex> 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<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
						Dispatch_manager::get_instance_references().m_dispatch_response_pickup_map[m_outlet_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(int outlet_id)
{
	//把m_dispatch_request_msg的信息传给本地的数据缓存.
	std::unique_lock<std::mutex> 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()->unit_id()*2+outlet_id+1;//0~2出口单元号改为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_x = 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_x = 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();
		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<std::mutex> t_lock(m_lock);
	std::unique_lock<std::mutex> 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_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_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<std::mutex> 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<std::mutex> 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());

	//need,need
	if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
	{
		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()->terminal_id()/2);

	}
	else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
	{
		m_dispatch_response_msg.mutable_id_struct()->set_terminal_id(m_dispatch_request_msg.mutable_id_struct()->unit_id()*2+m_outlet_id);
		m_dispatch_response_msg.mutable_id_struct()->set_unit_id(m_dispatch_request_msg.mutable_id_struct()->unit_id());
	}

//	m_dispatch_response_msg.set_terminal_id(m_dispatch_request_msg.terminal_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;
}