puai_dispatch/dispatch/carrier.cpp

//
// Created by huli on 2021/3/12.
//

#include "carrier.h"
#include "dispatch_manager.h"

Carrier::Carrier()
{
	m_request_x = 0;				//搬运器坐标x轴,
	m_request_y = 0;				//搬运器坐标y轴,
	m_request_z = 0;				//搬运器坐标z轴,
	m_request_y1 = 0;				//搬运器坐标y1轴, 搬运器抓车杆纵向移动(前轮抓杆)
	m_request_y2 = 0;				//搬运器坐标y2轴, 搬运器抓车杆纵向移动(后轮抓杆)
	m_request_clamp_motion = E_CLAMP_NO_ACTION;		//搬运器夹车杆. 0=无动作，1=夹紧，2=松开
	m_request_joint_motion_x = E_JOINT_NO_ACTION;	//电梯与X轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接
	m_request_joint_motion_y = E_JOINT_NO_ACTION;	//小跑车与Y轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接
	m_request_space_id = 0;			//搬运器空间位置的id.
	m_request_floor_id = 0;			//搬运器楼层位置的id.
	m_request_wheelbase = 0;		//搬运器抓车杆前后轮距.

	m_respons_status = RESPONS_WORKING;			//指令完成状态, 搬运器答复指令, 返回任务完成的情况
	m_respons_x = 0;				//搬运器坐标x轴,
	m_respons_y = 0;				//搬运器坐标y轴,
	m_respons_z = 0;				//搬运器坐标z轴,
	m_respons_y1 = 0;				//搬运器坐标y1轴, 搬运器抓车杆纵向移动(前轮抓杆)
	m_respons_y2 = 0;				//搬运器坐标y2轴, 搬运器抓车杆纵向移动(后轮抓杆)
	m_respons_clamp_motion = E_CLAMP_NO_ACTION;		//搬运器夹车杆. 0=无动作，1=夹紧，2=松开
	m_respons_joint_motion_x = E_JOINT_NO_ACTION;	//电梯与X轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接
	m_respons_joint_motion_y = E_JOINT_NO_ACTION;	//小跑车与Y轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接
	m_respons_space_id = 0;			//搬运器空间位置的id.
	m_respons_floor_id = 0;			//搬运器楼层位置的id.
	m_respons_wheelbase = 0;		//搬运器抓车杆前后轮距.

	m_status_updata_time = std::chrono::system_clock::now();
	m_last_heartbeat = 0;			//上一次的心跳
	m_actual_device_status = HARDWARE_DEVICE_UNKNOWN;			//搬运器的硬件设备状态
	m_actual_load_status = LOAD_UNKNOWN;				//搬运器的负载状态, 小跑车上面是否有车.
	m_actual_x = 0;					//搬运器坐标x轴,
	m_actual_y = 0;					//搬运器坐标y轴,
	m_actual_z = 0;					//搬运器坐标z轴,
	m_actual_y1 = 0;				//搬运器坐标y1轴, 搬运器抓车杆纵向移动(前轮抓杆)
	m_actual_y2 = 0;				//搬运器坐标y2轴, 搬运器抓车杆纵向移动(后轮抓杆)
	m_actual_clamp_motion1 = E_CLAMP_NO_ACTION;		//小跑车夹车杆. 0=无动作，1=夹紧，2=松开
	m_actual_clamp_motion2 = E_CLAMP_NO_ACTION;		//小跑车夹车杆. 0=无动作，1=夹紧，2=松开
	m_actual_small_sports_car_motion = E_SMALL_SPORTS_NO_ACTION;
	m_actual_joint_motion_x1 = E_JOINT_NO_ACTION;	//电梯与X轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接
	m_actual_joint_motion_x2 = E_JOINT_NO_ACTION;	//电梯与X轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接

	memset(m_actual_error_code, 0, 50);	//搬运器设备的报警信息位
	memset(m_actual_warning_code, 0, 50);	//升降机设备的报警信息位

	m_actual_coordinates_id = 0;
	m_actual_coordinates_rows = 0;
	m_actual_coordinates_columns = 0;

	m_redirect_flag = false;
	m_catcher_run_flag = false;
	m_redirect_pickup_parkspace_id = 0;
	m_redirect_pickup_car_type = Common_data::Car_type::UNKNOW_CAR_TYPE;
	m_redirect_store_parkspace_id = 0;
	m_redirect_store_car_type = Common_data::Car_type::UNKNOW_CAR_TYPE;
}

Carrier::~Carrier()
{

}

//检查任务类型, 子类必须重载, 用来检查输入的任务是否为子类所需的.
Error_manager Carrier::check_task_type(std::shared_ptr<Task_Base> p_task)
{
	//检查任务类型，
	if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK)
	{
		return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "Carrier::check_task_type  get_task_type() != CARRIER_TASK ");
	}

	return Error_code::SUCCESS;
}

//获取硬件设备的状态, 必须子类继承
Carrier::Hardware_device_status Carrier::get_actual_device_status()
{
	return m_actual_device_status;
}
std::string Carrier::get_current_command_key()
{
	return (m_request_key + " +++ " + m_respons_key);
}

//把任务单写入到内存中, 子类必须重载
Error_manager Carrier::write_task_to_memory(std::shared_ptr<Task_Base> p_task)
{
	//检查任务类型，
	if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK)
	{
		return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "Carrier::check_task_type  get_task_type() != CARRIER_TASK ");
	}
	else
	{
		Carrier_task* tp_carrier_task = (Carrier_task*)p_task.get();
		std::unique_lock<std::mutex> t_lock1(tp_carrier_task->m_lock);

		m_request_key = tp_carrier_task->m_request_key;
		m_request_x = tp_carrier_task->m_request_x;
		m_request_y = tp_carrier_task->m_request_y;
		m_request_z = tp_carrier_task->m_request_z;
		m_request_y1 = tp_carrier_task->m_request_y1;
		m_request_y2 = tp_carrier_task->m_request_y2;
		m_request_clamp_motion = (Dispatch_device_base::Clamp_motion)tp_carrier_task->m_request_clamp_motion;
		m_request_joint_motion_x = (Dispatch_device_base::Joint_motion)tp_carrier_task->m_request_joint_motion_x;
		m_request_joint_motion_y = (Dispatch_device_base::Joint_motion)tp_carrier_task->m_request_joint_motion_y;
		m_request_space_id = tp_carrier_task->m_request_space_id;
		m_request_floor_id = tp_carrier_task->m_request_floor_id;
		m_request_wheelbase = tp_carrier_task->m_request_wheelbase;

		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//更新设备底层通信数据, 子类必须重载
Error_manager Carrier::update_device_communication()
{
	std::unique_lock<std::mutex> t_lock1(Dispatch_communication::get_instance_references().m_data_lock);
	//请求消息, 调度->plc
	Dispatch_communication::Carrier_request_from_dispatch_to_plc_for_data * tp_carrier_request_from_dispatch_to_plc_for_data =
	& Dispatch_communication::get_instance_references().m_carrier_request_from_dispatch_to_plc_for_data[m_device_id];
	Dispatch_communication::Carrier_request_from_dispatch_to_plc_for_key * tp_carrier_request_from_dispatch_to_plc_for_key =
	& Dispatch_communication::get_instance_references().m_carrier_request_from_dispatch_to_plc_for_key[m_device_id];
	memset(tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key, 0, 50);

	int t_size = m_request_key.size()<=50 ? m_request_key.size() : 50 ;
	memcpy(tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size);
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_x = m_request_x;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y = m_request_y;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_z = m_request_z;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y1 = m_request_y1;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y2 = m_request_y2;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_clamp_motion = m_request_clamp_motion;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_joint_motion_x = m_request_joint_motion_x;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_joint_motion_y = m_request_joint_motion_y;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_space_id = m_request_space_id;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_floor_id = m_request_floor_id;
	tp_carrier_request_from_dispatch_to_plc_for_data->m_request_wheelbase = m_request_wheelbase;



	//答复消息, plc->调度
	Dispatch_communication::Carrier_response_from_plc_to_dispatch * tp_carrier_response_from_plc_to_dispatch =
	& Dispatch_communication::get_instance_references().m_carrier_response_from_plc_to_dispatch[m_device_id];
	m_respons_key = (char*) tp_carrier_response_from_plc_to_dispatch->m_respons_key;
	m_respons_status = (Dispatch_device_base::Respons_status)tp_carrier_response_from_plc_to_dispatch->m_respons_status;
	m_respons_x = tp_carrier_response_from_plc_to_dispatch->m_respons_x;
	m_respons_y = tp_carrier_response_from_plc_to_dispatch->m_respons_y;
	m_respons_z = tp_carrier_response_from_plc_to_dispatch->m_respons_z;
	m_respons_y1 = tp_carrier_response_from_plc_to_dispatch->m_respons_y1;
	m_respons_y2 = tp_carrier_response_from_plc_to_dispatch->m_respons_y2;
	m_respons_clamp_motion = (Dispatch_device_base::Clamp_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_clamp_motion;
	m_respons_joint_motion_x = (Dispatch_device_base::Joint_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_joint_motion_x;
	m_respons_joint_motion_y = (Dispatch_device_base::Joint_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_joint_motion_y;
	m_respons_space_id = tp_carrier_response_from_plc_to_dispatch->m_respons_space_id;
	m_respons_floor_id = tp_carrier_response_from_plc_to_dispatch->m_respons_floor_id;
	m_respons_wheelbase = tp_carrier_response_from_plc_to_dispatch->m_respons_wheelbase;


	//状态消息, plc->调度
	Dispatch_communication::Carrier_status_from_plc_to_dispatch *tp_carrier_status_from_plc_to_dispatch =
	& Dispatch_communication::get_instance_references().m_carrier_status_from_plc_to_dispatch[m_device_id];

	//设备异常  //注注注注注注注注意了, ==的优先级比&要高.
	if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x02) == 0 )
	{
		m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_EMERGENCY_STOP;
		m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT;
	}
	else if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x01) == 0 )
	{
		m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_FAULT;
		m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT;
	}
	else if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x08) == 0 )
	{
		m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_COLLISION;
		m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT;
	}
		//正常状态
	else
	{
		if (tp_carrier_status_from_plc_to_dispatch->m_work_status == 1)
		{
			m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_WORKING;
		}
		else if(tp_carrier_status_from_plc_to_dispatch->m_work_status == 2)
		{
			m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_READY;
		}
		else if(tp_carrier_status_from_plc_to_dispatch->m_work_status == 0)
		{
			m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_UNKNOWN;
		}

		//故障恢复之后   E_FAULT  ->>  E_THREE_LEVEL_WORK
		if ( m_dispatch_device_status == Dispatch_device_base::DISPATCH_DEVICE_FAULT )
		{
			m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_THREE_LEVEL_WORK;
		}
		//else 流程状态维持不变
	}

	m_actual_load_status = (Dispatch_device_base::Load_status)tp_carrier_status_from_plc_to_dispatch->m_actual_load_status;
	m_actual_x = tp_carrier_status_from_plc_to_dispatch->m_actual_x;
	m_actual_y = tp_carrier_status_from_plc_to_dispatch->m_actual_y;
	//plc没有 给3楼的中跑车写入z轴, 所以手动写入z轴, 方便后面判断坐标
	if ( m_device_id == 2 && m_actual_x >1)
	{
		m_actual_z = Dispatch_coordinates::get_instance_references().m_carrier_3th_floor_z;
	}
	else
	{
		m_actual_z = tp_carrier_status_from_plc_to_dispatch->m_actual_z;
	}
	m_actual_y1 = tp_carrier_status_from_plc_to_dispatch->m_actual_y1;
	m_actual_y2 = tp_carrier_status_from_plc_to_dispatch->m_actual_y2;
	m_actual_clamp_motion1 = (Dispatch_device_base::Clamp_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_clamp_motion1;
	m_actual_clamp_motion2 = (Dispatch_device_base::Clamp_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_clamp_motion2;
	m_actual_small_sports_car_motion = (Dispatch_device_base::Small_sports_car_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_small_sports_car_motion;
	m_actual_joint_motion_x1 = (Dispatch_device_base::Joint_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_joint_motion_x1;
	m_actual_joint_motion_x2 = (Dispatch_device_base::Joint_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_joint_motion_x2;
	memcpy(m_actual_error_code, tp_carrier_status_from_plc_to_dispatch->m_actual_error_code, 50);
	memcpy(m_actual_warning_code, tp_carrier_status_from_plc_to_dispatch->m_actual_warning_code, 50);
	m_actual_error_description = (char*)(tp_carrier_status_from_plc_to_dispatch->m_actual_error_description-2);
	update_actual_coordinates_id();


	//通过心跳帧来判断通信是否正常
	if ( m_last_heartbeat != tp_carrier_status_from_plc_to_dispatch->m_heartbeat )
	{
		m_last_heartbeat = tp_carrier_status_from_plc_to_dispatch->m_heartbeat;
		m_status_updata_time = std::chrono::system_clock::now();
/*
		if ( get_device_id() == 2)
		{
//			std::this_thread::sleep_for(std::chrono::seconds(1));
//			std::cout << " 111111111111111111111111111 = " << 123 << std::endl;
			std::cout << " huli test :::: " << " get_dispatch_device_status = " << get_dispatch_device_status() << std::endl;
//			std::cout << " huli test :::: " << " get_device_id() = " << get_device_id() << std::endl;

			std::cout << " tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key = " << tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key << std::endl;
			std::cout << " tp_carrier_response_from_plc_to_dispatch->m_respons_key = " << tp_carrier_response_from_plc_to_dispatch->m_respons_key << std::endl;
//			int b = tp_carrier_status_from_plc_to_dispatch->m_safe_status;
//			std::cout << " huli test :::: " << " tp_carrier_status_from_plc_to_dispatch->m_safe_status = " << b << std::endl;
//			int a = tp_carrier_status_from_plc_to_dispatch->m_work_status;
//			std::cout << " tp_carrier_status_from_plc_to_dispatch->m_work_status = " << a << std::endl;
//			std::cout << " m_request_key = " << m_request_key << std::endl;
//			std::cout << " m_respons_key = " << m_respons_key << std::endl;
//			std::cout << " huli test :::: " << " m_request_x = " << m_request_x << std::endl;
//			std::cout << " huli test :::: " << " m_respons_x = " << m_respons_x << std::endl;
//			std::cout << " huli test :::: " << " m_actual_x = " << m_actual_x << std::endl;
//			std::cout << " m_actual_device_status = " << m_actual_device_status << std::endl;
//			std::cout << " 111111111111111111111111111 = " << 456 << std::endl;
		}
*/
		
		//重连之后,搬运器状态   E_DISCONNECT  ->>  E_THREE_LEVEL_WORK
		if ( m_dispatch_device_status == Dispatch_device_base::DISPATCH_DEVICE_DISCONNECT )
		{
			m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_THREE_LEVEL_WORK;
		}
	}
	else if(std::chrono::system_clock::now() - m_status_updata_time > std::chrono::milliseconds(COMMUNICATION_OVER_TIME_MS))
	{
		m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_DISCONNECT;
	}
	//else 继续等待,直到消息刷新或者超时.

	return Error_code::SUCCESS;
}
//从内存中读数据到任务单, 子类必须重载
Error_manager Carrier::check_and_read_memory_to_task(std::shared_ptr<Task_Base> p_task)
{
	Dispatch_communication::get_instance_references().communication_start();

	//检查任务类型，
	if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK)
	{
		return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "Carrier::check_task_type  get_task_type() != CARRIER_TASK ");
	}
	else
	{
		if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
		{
			Carrier_task* tp_carrier_task = (Carrier_task*)p_task.get();
			std::unique_lock<std::mutex> t_lock1(tp_carrier_task->m_lock);

			tp_carrier_task->m_respons_key = m_respons_key;
			tp_carrier_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status;
			tp_carrier_task->m_respons_x = m_respons_x;
			tp_carrier_task->m_respons_y = m_respons_y;
			tp_carrier_task->m_respons_z = m_respons_z;
			tp_carrier_task->m_respons_y1 = m_respons_y1;
			tp_carrier_task->m_respons_y2 = m_respons_y2;
			tp_carrier_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion;
			tp_carrier_task->m_respons_joint_motion_x = (Carrier_task::Joint_motion)m_respons_joint_motion_x;
			tp_carrier_task->m_respons_joint_motion_y = (Carrier_task::Joint_motion)m_respons_joint_motion_y;
			tp_carrier_task->m_respons_space_id = m_respons_space_id;
			tp_carrier_task->m_respons_floor_id = m_respons_floor_id;
			tp_carrier_task->m_respons_wheelbase = m_respons_wheelbase;

			//如果故障,则添加错误码
			if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
			{
				//添加错误码
				Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
				tp_carrier_task->set_task_error_manager(t_error);
			}

//			if ( get_device_id() == 0 )
//			{
//				std::cout << " huli test :::: " << "  {{{{{{{{{{{{{{{{{{{{{   33333333333333333333333333333333333333333 }}}}}}}}}}}}}}}}}}}}}}}}}}= " << 333 << std::endl;
//				std::cout << " huli test :::: " << " m_request_key = " << m_request_key << std::endl;
//				std::cout << " huli test :::: " << " m_request_x = " << m_request_x << std::endl;
//				std::cout << " huli test :::: " << " m_request_y = " << m_request_y << std::endl;
//				std::cout << " huli test :::: " << " m_request_z = " << m_request_z << std::endl;
//				std::cout << " huli test :::: " << " m_request_clamp_motion = " << m_request_clamp_motion << std::endl;
//				std::cout << " huli test :::: " << " m_request_wheelbase = " << m_request_wheelbase << std::endl;
//				std::cout << " huli test :::: " << " m_request_y1 = " << m_request_y1 << std::endl;
//				std::cout << " huli test :::: " << " m_request_y2 = " << m_request_y2 << std::endl;
//				std::cout << " m_request_joint_motion_x = " << m_request_joint_motion_x << std::endl;
//
//				std::cout << " huli test :::: " << " m_respons_key = " << m_respons_key << std::endl;
//				std::cout << " huli test :::: " << " m_respons_status = " << m_respons_status << std::endl;
//				std::cout << " huli test :::: " << " m_respons_x = " << m_respons_x << std::endl;
//				std::cout << " huli test :::: " << " m_respons_y = " << m_respons_y << std::endl;
//				std::cout << " huli test :::: " << " m_respons_z = " << m_respons_z << std::endl;
//				std::cout << " huli test :::: " << " m_respons_clamp_motion = " << m_respons_clamp_motion << std::endl;
//				std::cout << " huli test :::: " << " m_respons_wheelbase = " << m_respons_wheelbase << std::endl;
//				std::cout << " huli test :::: " << " m_respons_y1 = " << m_respons_y1 << std::endl;
//				std::cout << " huli test :::: " << " m_respons_y2 = " << m_respons_y2 << std::endl;
//				std::cout << " m_respons_joint_motion_x = " << m_respons_joint_motion_x << std::endl;
//
//				std::cout << " m_actual_device_status = " << m_actual_device_status << std::endl;
//				std::cout << " huli test :::: " << " m_actual_load_status = " << m_actual_load_status << std::endl;
//				std::cout << " huli test :::: " << " m_actual_x = " << m_actual_x << std::endl;
//				std::cout << " huli test :::: " << " m_actual_y = " << m_actual_y << std::endl;
//				std::cout << " huli test :::: " << " m_actual_z = " << m_actual_z << std::endl;
//				std::cout << " huli test :::: " << " m_actual_y1 = " << m_actual_y1 << std::endl;
//				std::cout << " huli test :::: " << " m_actual_y2 = " << m_actual_y2 << std::endl;
//				std::cout << " huli test :::: " << " m_actual_clamp_motion1 = " << m_actual_clamp_motion1 << std::endl;
//				std::cout << " huli test :::: " << "  {{{{{{{{{{{{{{{{{{{{{{{{{{{{  44444444444444444444444444444444444444444 }}}}}}}}}}}}}}}}}}}}}}}}= " << 444 << std::endl;
//
//			}

			return Error_code::SUCCESS;
		}
			//返回没有收到数据
		else
		{
			return Error_code::NODATA;
		}
	}
	return Error_code::SUCCESS;
}



//取消下发的指令
Error_manager Carrier::cancel_command()
{
	//以后再写 need programe
	//目前调度和plc的通信指令做的很简单,没有暂停和急停 复位等操作.
	//这里先空着,以后再写.
	//调度模块单方面销毁任务, 不管底层plc的执行情况, 也不去告知plc任务取消.

	return Error_code::SUCCESS;
}

//更新真实 空间位置的id.
Error_manager Carrier::update_actual_coordinates_id()
{
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
	float t_distance_min = 99999999999999999;
	float t_distance_current = 0;
	int t_index_min = 0;

	//找出最近的坐标点
	for (auto iter = tp_dispatch_coordinates->m_packspace_coordinates_map.begin(); iter != tp_dispatch_coordinates->m_packspace_coordinates_map.end(); ++iter)
	{
		//只算x轴和z轴
		t_distance_current = (m_actual_x - iter->second.x)*(m_actual_x - iter->second.x) + (m_actual_z - iter->second.z)*(m_actual_z - iter->second.z);
		if ( t_distance_current < t_distance_min )
		{
			t_distance_min = t_distance_current;
			t_index_min = iter->first;
		}
	}
	m_actual_coordinates_id = t_index_min;

	//更新空间锁, 只是 覆盖写入加锁.
	if ( m_actual_coordinates_id != 0 )
	{
		m_actual_coordinates_rows = (m_actual_coordinates_id-1) / Dispatch_coordinates::get_instance_references().m_space_lock_columns;
		m_actual_coordinates_columns = (m_actual_coordinates_id-1) % Dispatch_coordinates::get_instance_references().m_space_lock_columns;
		//3楼的搬运器不用加空间锁
		if ( get_device_id() != 2 )
		{

			Dispatch_coordinates::get_instance_references().set_space_lock_for_carrier(m_actual_coordinates_rows, m_actual_coordinates_columns, m_device_id);
		}
	}

	return Error_code::SUCCESS;
}