chutian_dispatch/dispatch/dispatch_manager.cpp

//
// Created by huli on 2020/7/20.
//

#include "dispatch_manager.h"
#include "../tool/proto_tool.h"

#include <cstdlib>

Dispatch_manager::Dispatch_manager()
{
	m_dispatch_manager_status = E_DISPATCH_MANAGER_UNKNOW;
	m_dispatch_manager_id = -1;


	m_dispatch_manager_thread = nullptr;


}

Dispatch_manager::~Dispatch_manager()
{
	dispatch_manager_uninit();
}



//调度管理 初始化
Error_manager Dispatch_manager::dispatch_manager_init(int dispatch_manager_id)
{
	m_dispatch_manager_id = dispatch_manager_id;
	return  dispatch_manager_init_from_protobuf(DISPATCH_DEVICE_PARAMETER_PATH);
}

//调度管理 初始化
Error_manager Dispatch_manager::dispatch_manager_init()
{
	return  dispatch_manager_init_from_protobuf(DISPATCH_DEVICE_PARAMETER_PATH);
}

//初始化 调度管理 模块。从文件读取
Error_manager Dispatch_manager::dispatch_manager_init_from_protobuf(std::string prototxt_path)
{
	Dispatch_proto::Dispatch_device_parameter_all t_dispatch_device_parameter_all;
	if(!  proto_tool::read_proto_param(prototxt_path,t_dispatch_device_parameter_all) )
	{
		return Error_manager(DISPATCH_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,
							 "Dispatch_manager read_proto_param  failed");
	}
	return dispatch_manager_init_from_protobuf(t_dispatch_device_parameter_all);
}
//初始化 调度管理 模块。从protobuf读取
Error_manager Dispatch_manager::dispatch_manager_init_from_protobuf(Dispatch_proto::Dispatch_device_parameter_all& dispatch_device_parameter_all)
{
	LOG(INFO) << " ----Dispatch_manager::dispatch_manager_init_from_protobuf----- "<< this;

	Error_manager t_error;

	m_dispatch_plc.dispatch_plc_init(m_dispatch_manager_id);

	m_dispatch_motion_direction_next = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_PICKUP;

	//	线程默认开启
	m_dispatch_manager_condition.reset(false, true, false);
	m_dispatch_manager_thread = new std::thread(&Dispatch_manager::resource_allocation, this);

	m_dispatch_manager_status = E_DISPATCH_MANAGER_READY;
	return Error_code::SUCCESS;
}

//调度管理 反初始化
Error_manager Dispatch_manager::dispatch_manager_uninit()
{
	if (m_dispatch_manager_thread)
	{
		m_dispatch_manager_condition.kill_all();
	}
	if (m_dispatch_manager_thread)
	{
		m_dispatch_manager_thread->join();
		delete m_dispatch_manager_thread;
		m_dispatch_manager_thread = NULL;
	}


	m_dispatch_manager_status = E_DISPATCH_MANAGER_UNKNOW;
	m_dispatch_manager_id = -1;


	return Error_code::SUCCESS;
}

//调度管理 设备复位
Error_manager Dispatch_manager::dispatch_manager_device_reset()
{
	//以后再写
	return Error_code::SUCCESS;
}

//对外的接口函数，负责接受并处理任务单，
Error_manager Dispatch_manager::execute_task(Dispatch_manager::Dispatch_motion_direction dispatch_motion_direction)
{
	return Error_code::SUCCESS;
//	std::this_thread::sleep_for(std::chrono::seconds(rand()%3+3));

	if ( dispatch_motion_direction == E_PICKUP_CAR )
	{
		return Error_code::SUCCESS;
	}

//	return Error_code::SUCCESS;

//	srand(0);
	unsigned int t_probability = rand();

	if ( t_probability%100 >=20 )
	{
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
							 " Dispatch_manager::execute_task() error ");
	}
}

//检查能否执行消息指令
Error_manager Dispatch_manager::check_execute_msg(Communication_message* p_msg)
{
	Error_manager t_error = Dispatch_manager::get_instance_references().check_status();
	if ( t_error == Error_code::SUCCESS )
	{
		return Error_code::SUCCESS;
	}
	else if (t_error.get_error_level() == NEGLIGIBLE_ERROR)//一级故障,轻微故障,
	{
		std::cout << "Dispatch_manager   _is_busy , " << std::endl;
		//返回繁忙之后, 通信模块1秒后再次调用check
		return Error_code::COMMUNICATION_EXCUTER_IS_BUSY;
	}

	switch ( p_msg->get_message_type() )
	{
		case Communication_message::Message_type::eDispatch_request_msg:
		{
			return Error_code::SUCCESS;
			break;
		}
		case Communication_message::Message_type::eDispatch_plan_response_msg:
		{
			return Error_code::SUCCESS;
			break;
		}
		case Communication_message::Message_type::eDispatch_control_request_msg:
		{
			return Error_code::SUCCESS;
			break;
		}
		default :
		{
			//无效的消息,
			return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
								 " INVALID_MESSAGE error ");
			break;
		}
	}
	return Error_code::SUCCESS;
}
//检查状态
Error_manager Dispatch_manager::check_status()
{
	if ( m_dispatch_manager_status == E_DISPATCH_MANAGER_READY )
	{
		return Error_code::SUCCESS;
	}
	else if ( m_dispatch_manager_status == E_DISPATCH_MANAGER_STORE || m_dispatch_manager_status == E_DISPATCH_MANAGER_STORE )
	{
		return Error_manager(Error_code::DISPATCH_MANAGER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
							 " Dispatch_manager::check_status() error ");
	}
	else
	{
		return Error_manager(Error_code::DISPATCH_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Dispatch_manager::check_status() error ");
	}
	return Error_code::SUCCESS;
}

//调度模块 //执行搬运请求(主控->调度管理)
Error_manager Dispatch_manager::execute_for_dispatch_request_msg(message::Dispatch_request_msg& dispatch_request_msg)
{
	LOG(INFO) << " ---Dispatch_manager::execute_for_dispatch_request_msg--- "<< dispatch_request_msg.DebugString() << " "<< this;
	LOG(INFO) << " dispatch_request_msg->m_command_key =  "<<dispatch_request_msg.command_key()<< "   "<< this;

	Error_manager t_error;

	std::unique_lock<std::mutex> t_lock(m_lock);
	if ( dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
	{
		m_dispatch_request_store_list.push_back(dispatch_request_msg);
	}
	else
	{
		m_dispatch_request_pickup_list.push_back(dispatch_request_msg);
	}



	//need, 目前直接转发给调度plc执行, 后续要存队列,然后增加调度逻辑.

//	m_dispatch_plc.execute_for_dispatch_request_msg(dispatch_request_msg);



	return Error_code::SUCCESS;
}
//调度模块 答复数据异常
Error_manager Dispatch_manager::send_dispatch_response_msg_with_error(message::Dispatch_request_msg &dispatch_request_msg, Error_manager error)
{
	message::Dispatch_response_msg t_dispatch_response_msg;
	t_dispatch_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_response_msg);
	t_dispatch_response_msg.mutable_base_info()->set_timeout_ms(dispatch_request_msg.base_info().timeout_ms());
	t_dispatch_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
	t_dispatch_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
	t_dispatch_response_msg.set_command_key(dispatch_request_msg.command_key());

	t_dispatch_response_msg.mutable_error_manager()->set_error_code(error.get_error_code());
	t_dispatch_response_msg.mutable_error_manager()->set_error_level((message::Error_level)error.get_error_level());
	t_dispatch_response_msg.mutable_error_manager()->set_error_description(error.get_error_description());

	System_communication::get_instance_references().encapsulate_msg(t_dispatch_response_msg.SerializeAsString());
	return Error_code::SUCCESS;
}

//调度模块 //调度总规划的答复(调度算法->调度管理)
Error_manager Dispatch_manager::execute_for_dispatch_plan_response_msg(message::Dispatch_plan_response_msg &dispatch_plan_response_msg)
{
	LOG(INFO) << " ---Dispatch_manager::execute_for_dispatch_plan_response_msg--- "<< this;
	LOG(INFO) << " dispatch_plan_response_msg->m_command_key =  "<<dispatch_plan_response_msg.command_key()<< "   "<< this;

	Error_manager t_error;

	return Error_code::SUCCESS;
}
//调度模块 //调度控制的任务请求(调度算法->调度管理)
Error_manager Dispatch_manager::execute_for_dispatch_control_request_msg(message::Dispatch_control_request_msg &dispatch_control_request_msg)
{
	LOG(INFO) << " ---Dispatch_manager::execute_for_dispatch_control_request_msg--- "<< this;
	LOG(INFO) << " dispatch_control_request_msg->m_command_key =  "<<dispatch_control_request_msg.command_key()<< "   "<< this;

	Error_manager t_error;

	return Error_code::SUCCESS;
}

//定时发送 调度管理的状态
Error_manager Dispatch_manager::encapsulate_send_dispatch_manager_status()
{
	Error_manager t_error;

	int t_dispatch_manager_id = get_dispatch_manager_id();

	std::string t_msg1;
	//创建一条 调度管理总管理的状态  旧版
	message::Dispatch_terminal_status_msg t_dispatch_terminal_status_msg;
	t_dispatch_terminal_status_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_status_msg);
	t_dispatch_terminal_status_msg.mutable_base_info()->set_timeout_ms(5000);
	t_dispatch_terminal_status_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
	t_dispatch_terminal_status_msg.mutable_base_info()->set_receiver(message::Communicator::eEmpty);

	t_dispatch_terminal_status_msg.set_terminal_id(t_dispatch_manager_id*2);
	t_dispatch_terminal_status_msg.set_terminal_status(message::Terminal_status::E_TERMINAL_READY);
	t_dispatch_terminal_status_msg.set_passageway_direction(message::Passageway_direction::E_BILATERAL);

	t_msg1 = t_dispatch_terminal_status_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg1);
	t_dispatch_terminal_status_msg.set_terminal_id(t_dispatch_manager_id*2+1);
	t_msg1 = t_dispatch_terminal_status_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg1);


	std::string t_msg2;
	//创建一条 调度管理总管理的状态  新版
	message::Dispatch_manager_status_msg t_dispatch_manager_status_msg;
	t_dispatch_manager_status_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_manager_status_msg);
	t_dispatch_manager_status_msg.mutable_base_info()->set_timeout_ms(5000);
	t_dispatch_manager_status_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
	t_dispatch_manager_status_msg.mutable_base_info()->set_receiver(message::Communicator::eEmpty);

	t_dispatch_manager_status_msg.set_dispatch_id(t_dispatch_manager_id*2);
	Dispatch_manager::Dispatch_manager_status t_dispatch_manager_status = get_dispatch_manager_status();
	t_dispatch_manager_status_msg.set_dispatch_manager_status((message::Dispatch_manager_status)t_dispatch_manager_status);

	t_msg2 = t_dispatch_manager_status_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg2);
	t_dispatch_manager_status_msg.set_dispatch_id(t_dispatch_manager_id*2+1);
	t_msg2 = t_dispatch_manager_status_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg2);

	return Error_code::SUCCESS;
}

//在流程的map 里面释放指定的流程
Error_manager Dispatch_manager::release_dispatch_process(std::string command_key)
{
	std::unique_lock<std::mutex> t_lock(m_lock);

	return Error_code::SUCCESS;
}

//调度模块 ///地面雷达的状态消息(地面雷达->null)
Error_manager Dispatch_manager::execute_for_ground_status_msg(message::Ground_status_msg ground_status_msg)
{
	//将nnxx的protobuf 转化为 snap7的DB块,  把店面雷达数据转发给plc
	int t_inlet_id = ground_status_msg.terminal_id() %2;
	std::unique_lock<std::mutex> t_lock(Dispatch_communication::get_instance_references().m_data_lock);
	Dispatch_communication::Ground_lidar_response_from_manager_to_plc_for_data * p_response_data = & Dispatch_communication::get_instance_references().m_ground_lidar_response_from_manager_to_plc_for_data[t_inlet_id];
	Dispatch_communication::Ground_lidar_response_from_manager_to_plc_for_key * p_response_key = & Dispatch_communication::get_instance_references().m_ground_lidar_response_from_manager_to_plc_for_key[t_inlet_id];
	Dispatch_communication::Ground_lidar_request_from_plc_to_manager * p_request = & Dispatch_communication::get_instance_references().m_ground_lidar_request_from_plc_to_manager[t_inlet_id];

	if ( p_request->m_communication_mode == 0 )
	{
		p_response_key->m_heartbeat++;
		p_response_key->m_communication_mode = p_request->m_communication_mode;
		p_response_key->m_refresh_command = p_request->m_refresh_command;

		p_response_data->m_response_car_center_x = ground_status_msg.locate_information_realtime().locate_x();
		p_response_data->m_response_car_center_y = ground_status_msg.locate_information_realtime().locate_y();
		p_response_data->m_response_car_angle = ground_status_msg.locate_information_realtime().locate_angle();
		p_response_data->m_response_car_front_theta = ground_status_msg.locate_information_realtime().locate_front_theta();
		p_response_data->m_response_car_length = ground_status_msg.locate_information_realtime().locate_length();
		p_response_data->m_response_car_width = ground_status_msg.locate_information_realtime().locate_width();
		p_response_data->m_response_car_height = ground_status_msg.locate_information_realtime().locate_height();
		p_response_data->m_response_car_wheel_base = ground_status_msg.locate_information_realtime().locate_wheel_base();
		p_response_data->m_response_car_wheel_width = ground_status_msg.locate_information_realtime().locate_wheel_width();
		p_response_data->m_response_locate_correct = ground_status_msg.locate_information_realtime().locate_correct();
	}
	else if ( p_response_key->m_refresh_command == p_request->m_refresh_command )
	{
		p_response_key->m_heartbeat++;
		p_response_key->m_communication_mode = p_request->m_communication_mode;
		p_response_key->m_refresh_command = p_request->m_refresh_command;
	}
	else
	{
		p_response_key->m_heartbeat++;
		p_response_key->m_communication_mode = p_request->m_communication_mode;
		p_response_key->m_refresh_command = p_request->m_refresh_command;

		p_response_data->m_response_car_center_x = ground_status_msg.locate_information_realtime().locate_x();
		p_response_data->m_response_car_center_y = ground_status_msg.locate_information_realtime().locate_y();
		p_response_data->m_response_car_angle = ground_status_msg.locate_information_realtime().locate_angle();
		p_response_data->m_response_car_front_theta = ground_status_msg.locate_information_realtime().locate_front_theta();
		p_response_data->m_response_car_length = ground_status_msg.locate_information_realtime().locate_length();
		p_response_data->m_response_car_width = ground_status_msg.locate_information_realtime().locate_width();
		p_response_data->m_response_car_height = ground_status_msg.locate_information_realtime().locate_height();
		p_response_data->m_response_car_wheel_base = ground_status_msg.locate_information_realtime().locate_wheel_base();
		p_response_data->m_response_car_wheel_width = ground_status_msg.locate_information_realtime().locate_wheel_width();
		p_response_data->m_response_locate_correct = ground_status_msg.locate_information_realtime().locate_correct();
	}

	return Error_code::SUCCESS;
}






Dispatch_manager::Dispatch_manager_status Dispatch_manager::get_dispatch_manager_status()
{
	return m_dispatch_manager_status;
}

int Dispatch_manager::get_dispatch_manager_id()
{
	return m_dispatch_manager_id;
}
void Dispatch_manager::set_dispatch_manager_id(int dispatch_manager_id)
{
	m_dispatch_manager_id = dispatch_manager_id;
}


//资源分配
void Dispatch_manager::resource_allocation()
{
	LOG(INFO) << " Dispatch_manager::resource_allocation() start " << this;
	Error_manager t_error;


	while (m_dispatch_manager_condition.is_alive())
	{
		m_dispatch_manager_condition.wait();
		if ( m_dispatch_manager_condition.is_alive() )
		{
			std::this_thread::sleep_for(std::chrono::microseconds(1));
			std::this_thread::sleep_for(std::chrono::milliseconds(100));
//			std::this_thread::sleep_for(std::chrono::seconds(1));
			std::this_thread::yield();
			std::unique_lock<std::mutex> t_lock(m_lock);


			if ( m_dispatch_plc.get_dispatch_plc_status() == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY )
			{
				if ( m_dispatch_motion_direction_next == Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_PICKUP)
				{
					auto iter = m_dispatch_request_pickup_list.begin();
					if ( iter != m_dispatch_request_pickup_list.end() )
					{
						m_dispatch_plc.execute_for_dispatch_request_msg(*iter);
						m_dispatch_request_pickup_list.erase(iter);
					}
					m_dispatch_motion_direction_next = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_STORE;
				}
				else if ( m_dispatch_motion_direction_next == Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_STORE )
				{
					auto iter = m_dispatch_request_store_list.begin();
					if ( iter != m_dispatch_request_store_list.end() )
					{
						m_dispatch_plc.execute_for_dispatch_request_msg(*iter);
						m_dispatch_request_store_list.erase(iter);
					}
					m_dispatch_motion_direction_next = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_PICKUP;
				}
				else
				{
					///////
				}

			}
		}
	}

	LOG(INFO) << " Dispatch_manager::resource_allocation() end "<< this;
	return;
}