puai_dispatch/dispatch/dispatch_process.cpp

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

#include "dispatch_process.h"
#include "../system/system_communication.h"
#include "../dispatch/dispatch_manager.h"

Dispatch_process::Dispatch_process()
{
	m_dispatch_process_status = DISPATCH_PROCESS_STATUS_UNKNOW;
	m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
	m_dispatch_source = 0;
	m_dispatch_destination = 0;
	m_wheel_base = 0;

}

Dispatch_process::~Dispatch_process()
{
	Dispatch_process_uninit();
}

//初始化, 就把主控发送的请求传入即可.
Error_manager Dispatch_process::Dispatch_process_init(message::Dispatch_request_msg &dispatch_request_msg)
{
	//设定超时时间, 默认比任务指令里面的时间少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();

	//检查调度请求消息
	if ( dispatch_request_msg.terminal_id() < Dispatch_coordinates::get_instance_references().m_passageway_terminal_id_min ||
		 dispatch_request_msg.terminal_id() > Dispatch_coordinates::get_instance_references().m_passageway_terminal_id_max)
	{
		return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
							 " dispatch_request_msg.terminal_id() is error ");
	}

	//解析调度请求消息
	if ( dispatch_request_msg.dispatch_motion_direction() == message::E_STORE_CAR )
	{
		//检查调度请求消息
		if ( Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_INLET &&
			 Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_BIDIRECTION )
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
								 " m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] is error ");
		}
		//存车的车位可以是1~3个
		if ( dispatch_request_msg.parkspace_info_ex_size() <=0 || dispatch_request_msg.parkspace_info_ex_size() >3 )
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
								 " dispatch_request_msg.parkspace_info_ex_size() is error ");
		}


		//检查定位信息
		if ( dispatch_request_msg.has_locate_information() == false )
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
								 " dispatch_request_msg.has_locate_information() is error ");
		}

		m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE;
		m_dispatch_source = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ;
		//终点在运动过程中动态分配//存车需要在完全确定车位之后, 才能判断车位类型
//		m_dispatch_destination = dispatch_request_msg.parkspace_info().parkingspace_index_id() + PARKSPACE_ID_BASE;

		for (int i = 0; i < m_dispatch_request_msg.parkspace_info_ex_size(); ++i)
		{
			if(m_dispatch_request_msg.parkspace_info_ex(i).parkspace_path() == message::Parkspace_path::LEFT_PATH)
			{
				Common_data::copy_data(m_parkspace_information_left, dispatch_request_msg.parkspace_info_ex(i));
				Common_data::scaling(m_parkspace_information_left, 1000);
			}
			if(m_dispatch_request_msg.parkspace_info_ex(i).parkspace_path() == message::Parkspace_path::RIGHT_PATH)
			{
				Common_data::copy_data(m_parkspace_information_right, dispatch_request_msg.parkspace_info_ex(i));
				Common_data::scaling(m_parkspace_information_right, 1000);
			}
			if(m_dispatch_request_msg.parkspace_info_ex(i).parkspace_path() == message::Parkspace_path::TEMPORARY_CACHE_PATH)
			{
				Common_data::copy_data(m_parkspace_information_temporary, dispatch_request_msg.parkspace_info_ex(i));
				Common_data::scaling(m_parkspace_information_temporary, 1000);
			}
		}
//		Common_data::copy_data(m_parkspace_information, dispatch_request_msg.parkspace_info());
//		Common_data::scaling(m_parkspace_information, 1000);


		Common_data::copy_data(m_car_measure_information, dispatch_request_msg.locate_information());
		Common_data::scaling(m_car_measure_information, 1000);
		m_wheel_base = m_car_measure_information.car_wheel_base;
	}
	else if( dispatch_request_msg.dispatch_motion_direction() == message::E_PICKUP_CAR )
	{
		//检查调度请求消息
		if ( Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_OUTLET &&
			 Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_BIDIRECTION )
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
								 " m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] is error ");
		}
		//取车的车位必须是1个
		if ( dispatch_request_msg.parkspace_info_ex_size() != 1 )
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
								 " dispatch_request_msg.parkspace_info_ex_size() is error ");
		}

		m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP;
		m_dispatch_source = dispatch_request_msg.parkspace_info_ex(0).parkingspace_index_id() + PARKSPACE_ID_BASE;
		m_parkspace_type = (Common_data::Parkspace_type)( dispatch_request_msg.parkspace_info_ex(0).parkingspace_type() );

		//终点在运动过程中动态分配
		//目前不控制门, 所以出口有主控决定. 如果后续控制门, 那么出口可以由调度决定.
		//目前取车还是固定出口.
		m_dispatch_destination = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ;
		Common_data::copy_data(m_parkspace_information_optimal, dispatch_request_msg.parkspace_info_ex(0));
		Common_data::scaling(m_parkspace_information_optimal, 1000);
		m_wheel_base = m_parkspace_information_optimal.car_information.car_wheel_base;
	}
	else
	{
		m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
		return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
							 "  Dispatch_process::Dispatch_process_init ERROR ");
	}

	//轮距
	if ( m_wheel_base < Dispatch_coordinates::get_instance_references().m_car_wheel_base_min ||
		 m_wheel_base > Dispatch_coordinates::get_instance_references().m_car_wheel_base_max)
	{
		return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
							 "  m_wheel_base < 1000 ERROR ");
	}

	m_car_type = (Common_data::Car_type)( dispatch_request_msg.car_type() );
	m_temporary_cache_flag = false;
	m_dispatch_request_msg = dispatch_request_msg;
	m_dispatch_process_status = DISPATCH_PROCESS_CREATED;
	return Error_code::SUCCESS;
}

//反初始化
Error_manager Dispatch_process::Dispatch_process_uninit()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	for (auto iter = m_dispatch_control_node_map.begin(); iter != m_dispatch_control_node_map.end(); ++iter)
	{
		if ( iter->second.mp_dispatch_task.get() != NULL )
		{
			iter->second.mp_dispatch_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW);
			iter->second.mp_dispatch_task.reset();
			iter->second.mp_dispatch_device.reset();
		}
	}
	return Error_code::SUCCESS;
}

//检查流程是否空闲待机
Error_manager Dispatch_process::check_process_ready()
{
	return Error_code::SUCCESS;

	if ( m_dispatch_process_status == DISPATCH_PROCESS_READY )
	{
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::DISPATCH_PROCESS_IS_NOT_READY, Error_level::MINOR_ERROR,
							 " Dispatch_process::check_process_ready() fun error ");
	}
}
//判断是否为待机，
bool Dispatch_process::is_ready()
{
//	std::unique_lock<std::mutex> t_lock(m_lock);
	if ( m_dispatch_process_status == DISPATCH_PROCESS_READY && m_dispatch_carrier_node.mp_main_carrier.get() == NULL)
	{
		return true;
	}
	else
	{
		return false;
	}
}

//能否在左侧路径存车
bool Dispatch_process::is_able_store_left_path()
{
	if ( m_parkspace_information_left.parkingspace_index_id !=0 )
	{
		return true;
	}
	return false;
}
//能否在右侧路径存车
bool Dispatch_process::is_able_store_right_path()
{
	if ( m_parkspace_information_right.parkingspace_index_id !=0 )
	{
		return true;
	}
	return false;
}
//能否在3楼路径存车
bool Dispatch_process::is_able_store_temporary_cache_path()
{
	if ( m_parkspace_information_temporary.parkingspace_index_id !=0 )
	{
		return true;
	}
	return false;
}


//能否在临时缓存位存车
bool Dispatch_process::is_able_store_temporary_cache_path_2nd_floor()
{
	if ( m_parkspace_information_temporary.parkingspace_index_id >= 4 && m_parkspace_information_temporary.parkingspace_index_id <= 12 )
	{
		return true;
	}
	return false;
}
//能否在临时缓存位存车
bool Dispatch_process::is_able_store_temporary_cache_path_3rd_floor()
{
	if ( m_parkspace_information_temporary.parkingspace_index_id >= 19 && m_parkspace_information_temporary.parkingspace_index_id <= 27 )
	{
		return true;
	}
	return false;
}

void Dispatch_process::set_main_carrier(std::shared_ptr<Dispatch_device_base> p_carrier)
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	m_dispatch_carrier_node.mp_main_carrier = p_carrier;
}

std::string Dispatch_process::get_dispatch_control_command_key(Dispatch_catcher_node & dispatch_catcher_node)
{
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", m_command_key.c_str(), m_dispatch_process_type, dispatch_catcher_node.m_dispatch_control_status);
	std::string t_str = t_key;
	return t_str;
}
std::string Dispatch_process::get_dispatch_control_command_key(Dispatch_carrier_node & dispatch_carrier_node)
{
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", m_command_key.c_str(), m_dispatch_process_type, dispatch_carrier_node.m_dispatch_control_status);
	std::string t_str = t_key;
	return t_str;
}

void Dispatch_process::Main()
{
	Error_manager t_error;

	//主流程, 循环执行
	while ( std::chrono::system_clock::now() - m_start_time < std::chrono::milliseconds(m_timeout_ms) )
	{
		std::this_thread::sleep_for(std::chrono::microseconds(1));
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
		std::this_thread::sleep_for(std::chrono::seconds(1));
//		std::cout << " huli test :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: " << " m_dispatch_process_status = " << m_dispatch_process_status << std::endl;

		switch ( (Dispatch_process_status)m_dispatch_process_status )
		{
			case DISPATCH_PROCESS_CREATED://流程创建,
			{
				//检查调度请求
				m_result = check_dispatch_request_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//发送调度总计划
//				m_result = send_dispatch_plan_request_msg();
//				if ( m_result !=Error_code::SUCCESS)
//				{
//					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
//					break;
//				}
				//流程正常, 就进入等待状态, 等待调度控制发送动作指令
				m_dispatch_process_status = DISPATCH_PROCESS_READY;
				break;
			}
			case DISPATCH_PROCESS_READY://流程准备，待机
			{
				m_result = wait_resource_allocation();
				if ( m_result == Error_code::SUCCESS)
				{
					if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP )
					{
						m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY;
						m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY;
						m_dispatch_process_status = DISPATCH_PROCESS_WORKING_PICKUP;
					}
					else if(m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE)
					{
						m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY;
						m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY;
						m_dispatch_process_status = DISPATCH_PROCESS_WORKING_STORE;
					}
					//根据设备选择, 来设置终点
					if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2 )
					{
						m_parkspace_information_optimal = m_parkspace_information_temporary;
					}
					else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 0 && m_temporary_cache_flag == false )
					{
						m_parkspace_information_optimal = m_parkspace_information_left;
					}
					else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 0 && m_temporary_cache_flag == true)
					{
						m_parkspace_information_optimal = m_parkspace_information_temporary;
					}
					else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 1 && m_temporary_cache_flag == false )
					{
						m_parkspace_information_optimal = m_parkspace_information_right;
					}
					else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 1 && m_temporary_cache_flag == true)
					{
						m_parkspace_information_optimal = m_parkspace_information_temporary;
					}
				}
				//else 一直等待资源分配
				break;



				//调度控制, 并根据完成情况给答复
//				dispatch_control_motion();
//
//				//等待调度总计划答复
//				m_result = wait_dispatch_plan_response_msg();
//				if ( m_result ==Error_code::SUCCESS)
//				{
//					//流程正常, 就进入完成状态,
//					m_dispatch_process_status = DISPATCH_PROCESS_OVER;
//					break;
//				}
//				else if ( m_result !=Error_code::NODATA )
//				{
//					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
//					break;
//				}
				//else nodata, 就表示没有新的指令, 那么什么都不做, 原地待命

				break;
			}
			case DISPATCH_PROCESS_WORKING_PICKUP://流程完成
			{
				//调度控制取车
				m_result = dispatch_control_motion_pickup();
				if ( m_result ==Error_code::SUCCESS)
				{
					//流程正常, 就进入完成状态,
					m_dispatch_process_status = DISPATCH_PROCESS_OVER;
					break;
				}
				else if ( m_result !=Error_code::NODATA )
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//else nodata, 就表示没有新的指令, 那么什么都不做, 原地待命
				break;
			}
			case DISPATCH_PROCESS_WORKING_STORE://流程完成
			{
				//调度控制存车
				m_result = dispatch_control_motion_store();
				if ( m_result ==Error_code::SUCCESS)
				{
					//流程正常, 就进入完成状态,
					m_dispatch_process_status = DISPATCH_PROCESS_OVER;
					break;
				}
				else if ( m_result !=Error_code::NODATA )
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//else nodata, 就表示没有新的指令, 那么什么都不做, 原地待命
				break;
			}
			case DISPATCH_PROCESS_OVER://流程完成
			{
				//发送调度答复, 发给主控的
				m_result = send_dispatch_response_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//流程正常, 就进入等待状态, 等待调度控制发送动作指令
				m_dispatch_process_status = DISPATCH_PROCESS_RELEASE;
				break;
			}
			case DISPATCH_PROCESS_RELEASE://流程释放
			{
				//通知调度管理, 释放资源,
				m_result = release_resource();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}

				//在这里, 整个流程彻底结束, 之后线程池会自动回收 这个流程对象的资源
				return;
				break;
			}
			case DISPATCH_PROCESS_FAULT://故障
			{
				release_resource();
				Exception_handling();
				//在这里, 整个流程彻底结束, 之后线程池会自动回收 这个流程对象的资源
				return;
				break;
			}
			default:
			{

				break;
			}
		}
	}

	//任务超时
	LOG(INFO) << " Dispatch_process::Main() time out  "<< this;
	return;
}


//检查调度请求
Error_manager Dispatch_process::check_dispatch_request_msg()
{
//	std::unique_lock<std::mutex> t_lock(m_lock);
	return Error_code::SUCCESS;
}

//等待资源分配
Error_manager Dispatch_process::wait_resource_allocation()
{
	//只判断搬运器的指针是否有效.
	if ( m_dispatch_carrier_node.mp_main_carrier.get() == NULL )
	{
		return Error_code::NODATA;
	}
	else
	{
		return Error_code::SUCCESS;
	}
}

//调度控制取车
Error_manager Dispatch_process::dispatch_control_motion_pickup()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	Error_manager t_carrier_error;
	Carrier * tp_main_carrier = NULL;
	Carrier_task * tp_main_carrier_task = NULL;
	if ( m_dispatch_carrier_node.mp_main_carrier.get() != NULL && m_dispatch_carrier_node.mp_main_carrier_task.get() != NULL )
	{
		tp_main_carrier = (Carrier *)m_dispatch_carrier_node.mp_main_carrier.get();
		tp_main_carrier_task = (Carrier_task *)m_dispatch_carrier_node.mp_main_carrier_task.get();
	}
	Catcher * tp_avoid_catcher = NULL;
	Catcher_task * tp_avoid_catcher_task = NULL;
	if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL )
	{
		tp_avoid_catcher = (Catcher *)m_dispatch_carrier_node.mp_avoid_catcher.get();
		tp_avoid_catcher_task = (Catcher_task *)m_dispatch_carrier_node.mp_avoid_catcher_task.get();
	}

	//搬运器的控制动作
	switch ( m_dispatch_carrier_node.m_dispatch_control_status )
	{
		case DISPATCH_CONTROL_CREATED:
		case DISPATCH_CONTROL_READY://等待调度管理进行资源分配
		{
			m_dispatch_carrier_node.m_error = check_main_carrier(m_dispatch_carrier_node);
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP )
				{
					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_START;
				}
				else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE )
				{
					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_START;
				}
			}
			//else 原地等待
			break;
		}
		case DISPATCH_CARRIER_PICKUP_START://连接搬运器, 创建新的任务单 与设备建立连接
		{
			//注意了:3楼搬运器必须要和机器人联动, 所以先通知机器人连接
			if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2 )
			{
				//通知机器人提前开始
				m_dispatch_catcher_node.m_dispatch_control_start_flag = true;
			}
			//连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理
			m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL);
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_carrier_node.m_error = connect_dispatch_carrier(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL);
				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
				{
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
				else
				{
					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
				}
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_1:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_2://搬运器 准备开始, 需要同步任务单和设备真实数据
		{
			//注意了:3楼搬运器必须要和机器人联动, 所以先通知机器人连接
			if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2 )
			{
				//等待机器人正常连接之后, 搬运器才正式开始
				if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_2 )
				{
					//搬运器 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步
					carrier_ready_to_start(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates);
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
				//else 无限等待
			}
			else
			{
				//搬运器 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步
				carrier_ready_to_start(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_3:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_4://搬运器检查姿态 //搬运器调整到 正常待机的姿态(调节夹杆和轴距)
		{
			if ( tp_main_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
			{
				if ( tp_main_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE &&
					 Common_data::approximate_difference(m_wheel_base, tp_main_carrier->m_actual_y1-tp_main_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE))
				{
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2);
				}
				else
				{
					//搬运器调整到 正常待机的姿态(调节夹杆和轴距)
					carrier_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates);
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
				m_dispatch_carrier_node.m_error =  Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
									 "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_5:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_6://让小跑车回到中跑车上, y轴移动
		{
			if ( Common_data::approximate_difference(tp_main_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2);
			}
			else
			{
				carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_7:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_8://让中跑车回到电梯井, 进行x轴路径检查
		{
			float t_z = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].z;
			if ( Common_data::approximate_difference(tp_main_carrier->m_actual_z, t_z, DISPATCH_DEFAULT_DIFFERENCE) )
			{
				//直接到 20步 , 准备同层移动到车位
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_20);
				if (  tp_main_carrier->get_device_id() == 2 )
				{
				    //3楼的搬运器直接到 22步
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_22);
				}
			}
			else
			{
				//如果是3楼, 直接报错, 因为3楼搬运器只能在3楼移动, z轴真实值和目标值 一定是相同的
				if ( tp_main_carrier->get_device_id() !=0 && tp_main_carrier->get_device_id() !=1 )
				{
					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
					m_dispatch_carrier_node.m_error =  Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
																	 " tp_main_carrier->get_device_id()    PARAMRTER ERROR ");
				}
				else
				{
					int t_avoid_catcher_id = -1;	//需要避让的机器人
					//空间锁判断, 是否需要机器人避让
					if ( tp_main_carrier->get_device_id() == 0 )
					{
						m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0,
																										  tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns,
																										  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
					}
					else if ( tp_main_carrier->get_device_id() == 1 )
					{
						m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns,
																										  tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1,
																										  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
					}
					//判断结果
					if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
					{
						//成功则直接跳转到 13步, 不用避让, 直接控制搬运器直到电梯井
						m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_13);
					}
					else if(t_avoid_catcher_id != -1)
					{
						if ( m_dispatch_carrier_node.mp_avoid_catcher.get() == NULL )
						{
							m_dispatch_carrier_node.mp_avoid_catcher = Dispatch_manager::get_instance_references().m_catcher_map[t_avoid_catcher_id];
						}
					    //连接避让设备 使得避让机器人进行避让
						m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_avoid_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
						if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
						{
							m_dispatch_carrier_node.m_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
							if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
							{
								m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
							}
						}
						//else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接
					}
					else
					{
						m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
						m_dispatch_carrier_node.m_error =  Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
																		 " carrier_try_space_lock    fun  ERROR ");
					}
				}
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_9:
		{
			check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_10://机器人避让到最高点, 机器人上升不需要判断空间锁
		{
			catcher_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_11://机器人释放 空间锁
		{
			m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status);
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				tp_dispatch_coordinates->catcher_space_unlock(tp_avoid_catcher->m_actual_coordinates_id, tp_avoid_catcher->get_device_id());
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_12://搬运器空间锁判断, 必须成功
		{
			int t_avoid_catcher_id = -1;	//需要避让的机器人
			if ( tp_main_carrier->get_device_id() == 0 )
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0,
																								  tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns,
																								  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
			}
			else if ( tp_main_carrier->get_device_id() == 1 )
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns,
																								  tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1,
																								  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
			}
			//判断结果
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_13://让中跑车回到电梯井, x轴移动
		{
			if ( tp_main_carrier->get_device_id() == 0 )
			{
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else if ( tp_main_carrier->get_device_id() == 1 )
			{
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
				m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
																" tp_main_carrier->get_device_id() PARAMRTER ERROR ");
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_14://搬运器 释放空间锁
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id());
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_15://避让机器人 断连
		{
			if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL )
			{
				disconnect_dispatch_device(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task);
			}
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_16://中跑车收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
		{
			carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_17:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_18: //电梯移动到对应的楼层, 搬运器在电梯里面z轴移动, 不需要判断空间锁
		{
			float t_z = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].z;
			carrier_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_z);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_19:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_20://伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
		{
			if ( tp_main_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
			{
				carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2);
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_21:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_22://中跑车 x轴移动到车位, 进行x轴路径检查
		{
			float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x;
			int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns;
			if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) )
			{
				//搬运器不用x轴运动, 直接到29步, 去车位取车
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_29);
			}
			else
			{
				int t_avoid_catcher_id = -1;	//需要避让的机器人
				if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1)
				{
					m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0,
																									  tp_main_carrier->m_actual_coordinates_rows, t_column+1,
																									  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
				}
				else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1)
				{
					m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1,
																									  tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1,
																									  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
				}
				else if ( tp_main_carrier->get_device_id() == 2 )
				{
				    //3楼搬运器不用加锁, 直接x轴移动
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_27);
					break;//切换流程
				}
				else
				{
					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
					m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
																	" tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR ");
					break;//切换流程
				}

				//判断结果
				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
				{
					//左右搬运器加锁成功, 直接x轴移动
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_27);
				}
				else if(t_avoid_catcher_id != -1 && tp_main_carrier->m_actual_coordinates_rows == 2)//4楼取车, 机器人进行避让
				{
					if ( m_dispatch_carrier_node.mp_avoid_catcher.get() == NULL )
					{
						m_dispatch_carrier_node.mp_avoid_catcher = Dispatch_manager::get_instance_references().m_catcher_map[t_avoid_catcher_id];
					}
					//连接避让设备 使得避让机器人进行避让
					m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_avoid_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
					if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
					{
						m_dispatch_carrier_node.m_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
						if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
						{
							m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
						}
					}
					//else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接
				}
				//else 无限等待
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_23:
		{
			check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_24://让4楼机器人进行避让, 先加锁, 在避让
		{
			if ( tp_main_carrier->get_device_id() == 0 )
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(2, tp_avoid_catcher->m_actual_coordinates_columns,
																								  2, 12-1,
																								  tp_avoid_catcher->get_device_id());
				//判断结果
				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
				{
					//机器人移到42号口
					float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[42].x;
					catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
				//else 无限等待
			}
			else if ( tp_main_carrier->get_device_id() == 1 )
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(2, 4-1,
																								  2, tp_avoid_catcher->m_actual_coordinates_columns,
																								  tp_avoid_catcher->get_device_id());
				//判断结果
				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
				{
					//机器人移到34号口
					float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[34].x;
					catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
				//else 无限等待
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
				m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
																" tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR ");
				break;//切换流程
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_25://机器人释放 空间锁
		{
			check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status);
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				tp_dispatch_coordinates->catcher_space_unlock(tp_avoid_catcher->m_actual_coordinates_id, tp_avoid_catcher->get_device_id());
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_26://搬运器空间锁判断, 必须成功
		{
			int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns;
			int t_avoid_catcher_id = -1;	//需要避让的机器人
			if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1)
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0,
																								  tp_main_carrier->m_actual_coordinates_rows, t_column+1,
																								  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
			}
			else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1)
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1,
																								  tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1,
																								  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
			}

			//判断结果
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_27://搬运器x轴移动到车位
		{
			float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x;
			carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_28:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_29://把任务从一级升到三级, 准备取车
		{
			m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL);
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_30://小跑车 进入车位
		{
			carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_31:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_32://小跑车 夹车
		{
			carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_33:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_34://小跑车 回到中跑车
		{
			//注意了, 3楼的搬运器取车时, 机器人的位置必须在搬运器的上方.
			if ( tp_main_carrier->get_device_id() == 2 )
			{
				//等待机器人调整到3楼搬运器的上方位置
				if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_12 )
				{
					carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
				//else 无限等待
			}
			else
			{
				carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_35:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_36://让中跑车回到电梯井
		{
			//如果不在2楼, 那么就要退回电梯井
			if ( tp_main_carrier->get_device_id() == 0  && (tp_main_carrier->m_actual_coordinates_rows != 0 || m_dispatch_destination == 1100))
			{
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else if ( tp_main_carrier->get_device_id() == 1 && (tp_main_carrier->m_actual_coordinates_rows != 0 || m_dispatch_destination == 1107))
			{
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else if ( tp_main_carrier->get_device_id() == 2 )
			{
				//3楼搬运器取车后, 不用x轴移动, 直接准备交接
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_50);
			}
			else
			{
				//不用回电梯井, 直接准备x轴移动到出口上方
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_44);
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_37://搬运器 释放空间锁
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id());
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_38://避让机器人 断连
		{
			if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL )
			{
				//避让机器人 断开连接
				disconnect_dispatch_device(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task);
			}
			if ( m_dispatch_destination != 1107 || m_dispatch_destination != 1100 )
			{
			    //如不是7号出口, 那么开启 抓取机器人
			    //注意了, 取车流程只有在这里, 抓取机器人的流程才开始
				m_dispatch_catcher_node.m_dispatch_control_start_flag = true;
				if ( tp_main_carrier->get_device_id() != 2 )
				{
					m_dispatch_catcher_node.m_following_flag = true;
				}
				else
				{
					m_dispatch_catcher_node.m_following_flag = false;
				}
			}
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_39://收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
		{
			carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_40://电梯移动到2楼, (7号出口移动到1楼), 搬运器在电梯里面z轴移动, 不需要判断空间锁
		{
			float t_z =0;
			if ( (m_dispatch_destination == 1107 && tp_main_carrier->get_device_id() == 1) ||
				 (m_dispatch_destination == 1100 && tp_main_carrier->get_device_id() == 0) )//1楼出口
			{
				t_z = tp_dispatch_coordinates->m_carrier_1th_floor_z;
			}
			else if ( m_dispatch_destination >1100 && m_dispatch_destination<1107 )
			{
				t_z = tp_dispatch_coordinates->m_carrier_2th_floor_z;
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
				break;//直接跳转到故障
			}
			carrier_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_z);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_41:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_42://伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
		{
			if ( tp_main_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
			{
				carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2);
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_43:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_44://中跑车 x轴移动, 进行x轴路径检查, 加空间锁失败就进入待机位
		{
			//7号出口, 就不用判断空间锁了, 直接x轴移动
			if ( m_dispatch_destination == 1100 || m_dispatch_destination == 1107 )
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_80);
			}
			else
			{
				float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
				int t_terminal = m_dispatch_destination - PASSAGEWAY_ID_BASE;
				int t_temp = (t_terminal-1)%2;
				int t_column = 4 + ((t_terminal-1)/2)*3 + t_temp*2 -1;

				if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) )
				{
					//搬运器不用x轴运动, 直接到50步, 准备交接
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_50);
				}
				else
				{
					int t_avoid_catcher_id = -1;	//需要避让的机器人
					if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1)
					{
						m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0,
																										  tp_main_carrier->m_actual_coordinates_rows, t_column+1,
																										  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
					}
					else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1)
					{
						m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1,
																										  tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1,
																										  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
					}
					else if ( tp_main_carrier->get_device_id() == 2 )
					{
						//3楼搬运器不用加锁, 直接x轴移动
						m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_48);
						break;//切换流程
					}
					else
					{
						m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
						m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
																		" tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR ");
						break;//切换流程
					}

					//判断结果
					if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
					{
						//左右搬运器加锁成功, 直接x轴移动
						m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_48);
					}
					else
					{
						//加锁失败, 就移动到待机位.
						m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
					}
				}
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_45://中跑车 x轴移动, 移动到待机位
		{
			if ( tp_main_carrier->get_device_id() == 0 )
			{
				float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[2].x;
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else if ( tp_main_carrier->get_device_id() == 1 )
			{
				float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[14].x;
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_46:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_47://中跑车 x轴移动, 进行x轴路径检查, 加空间锁失败就 只能无限等待
		{
			float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
			int t_terminal = m_dispatch_destination - PASSAGEWAY_ID_BASE;
			int t_temp = (t_terminal-1)%2;
			int t_column = 4 + ((t_terminal-1)/2)*3 + t_temp*2 -1;

			if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) )
			{
				//搬运器不用x轴运动, 直接到50步, 准备交接
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_50);
			}
			else
			{
				int t_avoid_catcher_id = -1;	//需要避让的机器人
				if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1)
				{
					m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0,
																									  tp_main_carrier->m_actual_coordinates_rows, t_column+1,
																									  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
				}
				else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1)
				{
					m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1,
																									  tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1,
																									  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
				}
				else if ( tp_main_carrier->get_device_id() == 2 )
				{
					//3楼搬运器不用加锁, 直接交接
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_50);
					break;//切换流程
				}
				else
				{
					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
					m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
																	" tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR ");
					break;//切换流程
				}

				//判断结果
				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
				{
					//左右搬运器加锁成功, 直接x轴移动
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
				//else 无限等待
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_48://中跑车 x轴移动, 移动到取车口的上方2楼处
		{
			float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
			carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_49:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_50://小跑车 松开夹杆
		{
			carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_51:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_52://等待机器人把车从搬运器上面取走
		{
			// 等待机器人把车从搬运器上面取走
			if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_22 )
			{
				if ( tp_main_carrier->get_device_id() == 2 )
				{
				    //进入3楼搬运器取车的特殊流程, 跟随和避让
					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_70;
				}
				else
				{
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
			}
			//else 无限等待
			break;
		}
		case DISPATCH_CARRIER_PICKUP_53://搬运器退回电梯井
		{
			if ( tp_main_carrier->get_device_id() == 0 )
			{
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else if ( tp_main_carrier->get_device_id() == 1 )
			{
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
				m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
																" tp_main_carrier->get_device_id() PARAMRTER ERROR ");
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_54://搬运器 释放空间锁
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id());
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_55://收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
		{
			//7号出口, 必须收回对接
			if ( m_dispatch_destination == 1100 || m_dispatch_destination == 1107 )
			{
				carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			//如果后面有一级存车任务, 那么就跳过
			else if ( tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL))
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2);
			}
			else
			{
				carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_56:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_57://主搬运器 断连
		{
			disconnect_dispatch_device(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_END);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_70://3楼搬运器跟随, 到机器人旁边一个车位
		{
			//3楼搬运器跟随, 到机器人旁边一个车位
			float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
			float t_following_x = 0;
			if ( m_dispatch_destination == 1101 )
			{
				t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1102].x;
			}
			else if ( m_dispatch_destination == 1106 )
			{
				t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1105].x;
			}
			else if ( tp_main_carrier->m_actual_x < t_x)
			{
				t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination-1].x;
			}
			else
			{
				t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination+1].x;
			}
			carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_following_x);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_71:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_72://3楼搬运器移到机器人下方
		{
			//如果后面有一级存车任务, 那么就跳过
			if ( tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL))
			{
				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2);
			}
			else
			{
				if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_34 )
				{
					float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
					carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
				}
				//else 无限等待
			}
			break;
		}
		case DISPATCH_CARRIER_PICKUP_73:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_74://主搬运器 断连
		{
			disconnect_dispatch_device(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_END);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_80://中跑车 x轴移动, 移动到7号取车口
		{
			float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
			carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_81:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_82://小跑车 进入车位
		{
			carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_83:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_84://小跑车 松开车
		{
			carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_85:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_86://小跑车 回到中跑车
		{
			carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_87:
		{
			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_88:
		{
			//回到正常流程
			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_53);
			break;
		}
		case DISPATCH_CARRIER_PICKUP_END:
		{
			m_dispatch_carrier_node.m_error = Error_code::SUCCESS;
			break;
		}
		default:
		{

			break;
		}
	}

	Error_manager t_catcher_error;
	Catcher * tp_main_catcher = NULL;
	Catcher_task * tp_main_catcher_task = NULL;
	if ( m_dispatch_catcher_node.mp_main_catcher.get() != NULL && m_dispatch_catcher_node.mp_main_catcher_task.get() != NULL )
	{
		tp_main_catcher = (Catcher *)m_dispatch_catcher_node.mp_main_catcher.get();
		tp_main_catcher_task = (Catcher_task *)m_dispatch_catcher_node.mp_main_catcher_task.get();
	}
	Carrier * tp_following_carrier = NULL;
	Carrier_task * tp_following_carrier_task = NULL;
	if ( m_dispatch_catcher_node.mp_following_carrier.get() != NULL && m_dispatch_catcher_node.mp_following_carrier_task.get() != NULL )
	{
		tp_following_carrier = (Carrier *)m_dispatch_catcher_node.mp_following_carrier.get();
		tp_following_carrier_task = (Carrier_task *)m_dispatch_catcher_node.mp_following_carrier_task.get();
	}

	//抓取器的控制动作
	switch ( m_dispatch_catcher_node.m_dispatch_control_status )
	{
		case DISPATCH_CONTROL_CREATED:
		case DISPATCH_CONTROL_READY:
		{
			m_dispatch_catcher_node.m_error = check_main_catcher(m_dispatch_catcher_node);
			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
			{
				if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP )
				{
					m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_START;
				}
				else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE )
				{
					m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_START;
				}
			}
			//else 原地等待
			break;
		}
		case DISPATCH_CATCHER_PICKUP_START://连接机器人, 创建新的任务单 与设备建立连接
		{
			if ( m_dispatch_catcher_node.mp_main_catcher.get() == NULL )
			{
				m_dispatch_catcher_node.mp_main_catcher = Dispatch_manager::get_instance_references().m_catcher_map[1];
			}
			m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL);
			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_catcher_node.m_error = connect_dispatch_catcher(m_dispatch_catcher_node.mp_main_catcher, m_dispatch_catcher_node.mp_main_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL);
				if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
				{
					m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
				}
				//else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接
			}
			//else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接
			break;
		}
		case DISPATCH_CATCHER_PICKUP_1:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_2://机器人 准备开始, 需要同步任务单和设备真实数据
		{
			//机器人 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步
			catcher_ready_to_start(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates);
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_3:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_4://机器手调整到 正常待机的姿态(调节夹杆和轴距)
		{
			if ( tp_main_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
			{
				if ( tp_main_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE ||
					 tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance >
					 tp_dispatch_coordinates->m_catcher_wheel_base_limit ||
					 tp_main_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW	)
				{
					catcher_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates);
					m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
				}
				else
				{
					m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2);
				}
			}
			else
			{
				m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
				m_dispatch_catcher_node.m_error = Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
									 "Dispatch_process::excute_robot_catch_car_from_carrier() fun error ");
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_5:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_6://机器手 调整z轴, (z轴上升到4楼. 不需要判断空间锁)
		{
			if ( tp_main_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z )
			{
				//检测正常, 直接跳过即可
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2);
			}
			else
			{
				catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_7://机器人释放 空间锁
		{
			m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
			{
				tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id());
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_8://连接3楼的跟随机器人
		{
			if ( m_dispatch_catcher_node.m_following_flag )
			{
				if ( m_dispatch_catcher_node.mp_following_carrier.get() == NULL )
				{
					m_dispatch_catcher_node.mp_following_carrier = Dispatch_manager::get_instance_references().m_carrier_map[2];
				}
				m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_following_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
				if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
				{
					m_dispatch_catcher_node.m_error = connect_dispatch_carrier(m_dispatch_catcher_node.mp_following_carrier, m_dispatch_catcher_node.mp_following_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
					if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
					{
						m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
					}
					//else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接
				}
				//else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接
			}
			else
			{
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_9://机器人 x轴移动之前, 判断空间锁.
		{
			int t_terminal = m_dispatch_destination - 1100;
			int temp = (t_terminal-1)%2;
			int t_column = 4 + ((t_terminal-1)/2)*3 + temp*2 -1;

			if ( tp_avoid_catcher->m_actual_coordinates_columns < t_column )
			{
				m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(2, tp_main_catcher->m_actual_coordinates_columns,
																								  2, t_column+2,
																								  tp_main_catcher->get_device_id());
			}
			else
			{
				m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(2, t_column-2,
																								  2, tp_main_catcher->m_actual_coordinates_columns,
																								  tp_main_catcher->get_device_id());
			}
			//判断结果
			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			//else 无限等待
			break;
		}
		case DISPATCH_CATCHER_PICKUP_10://机器手调整到 对接搬运器的姿态
		{
			float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
			float t_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
			if ( Common_data::approximate_difference(tp_main_catcher->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) &&
				 Common_data::approximate_difference(tp_main_catcher->m_actual_y, t_y, DISPATCH_DEFAULT_DIFFERENCE))
			{
				//检测正常, 直接跳过即可
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2);
			}
			else
			{
				//机器手调整到 对接搬运器的姿态
				catcher_adjust_from_carrier(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_x);
				//3楼搬运器跟随, 到机器人旁边一个车位
				if ( m_dispatch_catcher_node.m_following_flag )
				{
					float t_following_x = 0;
					if ( m_dispatch_destination == 1101 )
					{
						t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1102].x;
					}
					else if ( m_dispatch_destination == 1106 )
					{
						t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1105].x;
					}
					else if ( tp_following_carrier->m_actual_x < t_x)
				    {
				        t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination-1].x;
				    }
					else
					{
						t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination+1].x;
					}
					carrier_move_x(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_following_carrier, tp_following_carrier_task, tp_dispatch_coordinates, t_following_x);
				}
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_11://机器人释放 空间锁
		{
			m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task);
			if ( m_dispatch_catcher_node.m_following_flag )
			{
				m_dispatch_catcher_node.m_error.compare_and_cover_error(check_task_status(m_dispatch_catcher_node.mp_following_carrier_task));
			}
			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
			{
				tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id());
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_12://机器手 准备下降抓车之前, 等待搬运器就位, 然后把一级任务升到三级
		{
			//等待搬运器就位, 把一级任务升到三级
			if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_52 )
			{
				m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL);
				if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
				{
					m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
				}
				else
				{
					m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
				}
			}
			//else 无限等待
			break;
		}
		case DISPATCH_CATCHER_PICKUP_13://机器手 z轴下降, 空间加锁
		{
			//2楼交接的空间已经由搬运器加锁, 机器人下降时就不判断了, 强制加锁即可.
			if ( m_dispatch_catcher_node.m_following_flag )//去3楼取车不用加锁
			{
				int t_terminal = m_dispatch_destination - 1100;
				int temp = (t_terminal-1)%2;
				int t_column = 4 + ((t_terminal-1)/2)*3 + temp*2 -1;
				m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_force_space_lock(0, t_column-1,
																									2, t_column+1,
																									tp_main_catcher->get_device_id());
			}
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_14://机器手 z轴下降
		{
			float t_z = 0;
			if ( m_dispatch_catcher_node.m_following_flag )
			{
				//下降到2楼抓车
				t_z = tp_dispatch_coordinates->m_catcher_2th_floor_z;
			}
			else
			{
				//下降到3楼抓车
				t_z = tp_dispatch_coordinates->m_catcher_3th_floor_z;
			}
			catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_z);
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_15:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_16://修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
		{
			float temp_wheel_base = tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance;
			//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
			if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
			{
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2);
			}
			else
			{
				catcher_adjust_wheel_base(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates);
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_17:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_18://机器手 夹车
		{
			catcher_move_c(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT);
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_19:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_20://机器手 z轴上升, 上升190mm就足以
		{
			float t_z = 0;
			if ( m_dispatch_catcher_node.m_following_flag )
			{
				t_z = tp_dispatch_coordinates->m_catcher_2th_floor_z + tp_dispatch_coordinates->m_separated_distance_z;
			}
			else
			{
				t_z = tp_dispatch_coordinates->m_catcher_3th_floor_z + tp_dispatch_coordinates->m_separated_distance_z;
			}
			catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_z);
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_21:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_22://机器手调整到 准备把车放到地面的姿态
		{
			float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
			float t_y = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].y;
			if ( Common_data::approximate_difference(tp_main_catcher->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) &&
				 Common_data::approximate_difference(tp_main_catcher->m_actual_y, t_y, DISPATCH_DEFAULT_DIFFERENCE))
			{
				//检测正常, 直接跳过即可
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2);
			}
			else
			{
				catcher_adjust_to_ground(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_x, t_y);
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_23:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_24://等待搬运器离开后, 机器人就可以下降了
		{
			//等待搬运器离开后, 机器人就可以下降了
			if ( (m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_55 &&
				  m_dispatch_carrier_node.m_dispatch_control_status <= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_57)
				 || (m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_72 &&
					 m_dispatch_carrier_node.m_dispatch_control_status <= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_74)
				 || (m_dispatch_carrier_node.m_dispatch_control_status == Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_END) )
			{
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			else if(m_dispatch_carrier_node.m_dispatch_control_status == Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_54 &&
					Common_data::approximate_difference(tp_main_carrier->m_actual_x, tp_main_catcher->m_actual_x, tp_dispatch_coordinates->m_separated_distance_x) == false )
			{
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			//else 无限等待
			break;
		}
		case DISPATCH_CATCHER_PICKUP_25://机器人z轴下降, 判断空间锁
		{
			int t_terminal = m_dispatch_destination - 1100;
			int temp = (t_terminal-1)%2;
			int t_column = 4 + ((t_terminal-1)/2)*3 + temp*2 -1;
			m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(0, t_column-1,
																							  2, t_column+1,
																							  tp_main_catcher->get_device_id());

			//判断结果
			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
			{
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			//else 无限等待
			break;
		}
		case DISPATCH_CATCHER_PICKUP_26://机器人下降到地面放车
		{
			catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_1th_floor_z);
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_27:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_28://机器手 松开夹杆
		{
			catcher_move_c(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE);
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_29:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_30://机器手调整到 正常待机的姿态(调节夹杆和轴距)
		{
			if ( tp_main_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE ||
				 tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance >
				 tp_dispatch_coordinates->m_catcher_wheel_base_limit ||
				 tp_main_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW	)
			{
				catcher_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates);
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2);
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_31:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_32://机器手 调整z轴, z轴上升到4楼
		{
			catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_33://机器人释放 空间锁
		{
			m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
			{
				tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id());
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_34://3楼的搬运器跟随到机器人下方
		{
			//如果后面有一级存车任务, 那么就跳过
			if ( m_dispatch_catcher_node.m_following_flag &&
				 tp_main_catcher->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL) == false)
			{
				float t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
				carrier_move_x(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_following_carrier, tp_following_carrier_task, tp_dispatch_coordinates, t_following_x);
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
			}
			else
			{
				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2);
			}
			break;
		}
		case DISPATCH_CATCHER_PICKUP_35:
		{
			check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
			break;
		}		
		case DISPATCH_CATCHER_PICKUP_36://主搬运器 断连
		{
			disconnect_dispatch_device(m_dispatch_catcher_node.mp_main_catcher, m_dispatch_catcher_node.mp_main_catcher_task);
			if ( m_dispatch_catcher_node.m_following_flag )
			{
				disconnect_dispatch_device(m_dispatch_catcher_node.mp_following_carrier, m_dispatch_catcher_node.mp_following_carrier_task);
			}
			m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_END);
			break;
		}
		case DISPATCH_CATCHER_PICKUP_END:
		{
			m_dispatch_catcher_node.m_error = Error_code::SUCCESS;
			break;
		}
		default:
		{

			break;
		}
	}

	std::cout << " huli test :::: " << " =========================================================== = " << 1 << std::endl;
	std::cout << " huli test :::: " << " m_dispatch_carrier_node.m_dispatch_control_status = " << m_dispatch_carrier_node.m_dispatch_control_status << std::endl;
	if ( tp_main_carrier != NULL )
	{
	    std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << tp_main_carrier->get_device_id() << std::endl;
	}
	else
	{
		std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << -1 << std::endl;
	}
	if ( tp_avoid_catcher != NULL )
	{
		std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << tp_avoid_catcher->get_device_id() << std::endl;
	}
	else
	{
		std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << -1 << std::endl;
	}
	std::cout << " huli test :::: " << " -------------------------- = " << 2 << std::endl;
	std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_dispatch_control_status = " << m_dispatch_catcher_node.m_dispatch_control_status << std::endl;
	if ( tp_main_catcher != NULL )
	{
		std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << tp_main_catcher->get_device_id() << std::endl;
	}
	else
	{
		std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << -1 << std::endl;
	}
	if ( tp_following_carrier != NULL )
	{
		std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << tp_following_carrier->get_device_id() << std::endl;
	}
	else
	{
		std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << -1 << std::endl;
	}

	char ch;
	std::cin >> ch ;


	//结果汇总
	if ( m_dispatch_carrier_node.m_dispatch_control_status == DISPATCH_CARRIER_PICKUP_END &&
		 m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CATCHER_PICKUP_END)
	{
		return Error_code::SUCCESS;
	}
	else if ( m_dispatch_carrier_node.m_dispatch_control_status == DISPATCH_CONTROL_FAULT )
	{
		return m_dispatch_carrier_node.m_error;
	}
	else if(m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CONTROL_FAULT)
	{
		return m_dispatch_catcher_node.m_error;
	}
	else
	{
	    return Error_code::NODATA;
	}
}
//调度控制存车
Error_manager Dispatch_process::dispatch_control_motion_store()
{
	Error_manager t_error;
	std::unique_lock<std::mutex> t_lock(m_lock);

	//搬运器的控制动作
	switch ( m_dispatch_catcher_node.m_dispatch_control_status )
	{
		case DISPATCH_CONTROL_CREATED:
		case DISPATCH_CONTROL_READY:
		{
			;
			break;
		}
		case DISPATCH_CONTROL_CONNECT_DEVICE:
		{
			;
			break;
		}
		default:
		{

			break;
		}
	}

	//抓取器的控制动作
	switch ( m_dispatch_catcher_node.m_dispatch_control_status )
	{
		case DISPATCH_CONTROL_CREATED:
		case DISPATCH_CONTROL_READY:
		{
			;
			break;
		}
		case DISPATCH_CONTROL_CONNECT_DEVICE:
		{
			;
			break;
		}
		default:
		{

			break;
		}
	}
}

//检查核心搬运器
Error_manager Dispatch_process::check_main_carrier(Dispatch_carrier_node & dispatch_carrier_node)
{
	//只判断搬运器的指针是否有效.
	if ( dispatch_carrier_node.mp_main_carrier.get() != NULL )
	{
		if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP )
		{
			m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_START;
		}
		else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE )
		{
			m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_START;
		}
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_code::NODATA;
	}
}
//检查核心抓取器
Error_manager Dispatch_process::check_main_catcher(Dispatch_catcher_node & dispatch_catcher_node)
{
	//只判断搬 调度设备机器人启动标志位
	if ( dispatch_catcher_node.m_dispatch_control_start_flag )
	{
		//为抓取器分配对应的设备, 目前只有2号机器人
		if ( dispatch_catcher_node.mp_main_catcher.get() ==NULL )
		{
			dispatch_catcher_node.mp_main_catcher = Dispatch_manager::get_instance_references().m_catcher_map[1];
		}

		if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP )
		{
			m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_START;
		}
		else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE )
		{
			m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_START;
		}
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_code::NODATA;
	}
}

//连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理
Error_manager Dispatch_process::connect_dispatch_carrier(std::shared_ptr<Dispatch_device_base> & p_carrier, std::shared_ptr<Task_Base> & p_carrier_task, Dispatch_device_base::Dispatch_task_level dispatch_task_level)
{
	Error_manager t_error;
	if ( p_carrier.get() != NULL )
	{
		Carrier* tp_carrier = (Carrier*)p_carrier.get();
		//检查设备能否执行任务, 主搬运器是管理层分配好的, 一般是可以直接连接.
		t_error = tp_carrier->check_task_level(dispatch_task_level);
		if ( t_error == Error_code::SUCCESS )
		{
			//创建任务单
			p_carrier_task = std::shared_ptr<Task_Base>(new Carrier_task);
			p_carrier_task->task_init(NULL,std::chrono::milliseconds(15000));
			Carrier_task * tp_carrier_task = (Carrier_task *)p_carrier_task.get();

			//第一次发送 空的唯一码, 可以和设备建立联系
			tp_carrier_task->m_request_key = "";
			tp_carrier_task->m_request_x = tp_carrier->m_actual_x;
			tp_carrier_task->m_request_y = tp_carrier->m_actual_y;
			tp_carrier_task->m_request_z = tp_carrier->m_actual_z;
			tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y1;
			tp_carrier_task->m_request_y2 = tp_carrier->m_actual_y2;
			tp_carrier_task->m_request_clamp_motion = (Carrier_task::Clamp_motion)tp_carrier->m_actual_clamp_motion1;
			tp_carrier_task->m_request_joint_motion_x = (Carrier_task::Joint_motion)tp_carrier->m_actual_joint_motion_x1;
			tp_carrier_task->m_request_joint_motion_y = Carrier_task::Joint_motion::E_JOINT_NO_ACTION;
			tp_carrier_task->m_request_space_id = 0;
			tp_carrier_task->m_request_floor_id = 0;
			tp_carrier_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;

			t_error = tp_carrier->execute_task(p_carrier_task, dispatch_task_level);
		}
	}
	else
	{
		t_error.error_manager_reset(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
									" Dispatch_process::connect_dispatch_carrier() POINTER IS NULL ");
	}
	return t_error;
}

//连接抓取器, 创建新的任务单 与设备建立连接, 可以失败, 失败就重新执行
Error_manager Dispatch_process::connect_dispatch_catcher(std::shared_ptr<Dispatch_device_base> & p_catcher, std::shared_ptr<Task_Base> & p_catcher_task, Dispatch_device_base::Dispatch_task_level dispatch_task_level)
{
	Error_manager t_error;
	if ( p_catcher.get() != NULL )
	{
		Catcher* tp_catcher = (Catcher*)p_catcher.get();
		//检查设备能否执行任务, 主抓取器是需要调度流程去争抢的.
		t_error = tp_catcher->check_task_level(dispatch_task_level);
		if ( t_error == Error_code::SUCCESS )
		{
			//创建任务单
			p_catcher_task = std::shared_ptr<Task_Base>(new Catcher_task);
			p_catcher_task->task_init(NULL,std::chrono::milliseconds(15000));
			Catcher_task * tp_catcher_task = (Catcher_task *)p_catcher_task.get();

			//第一次发送 空的唯一码, 可以和设备建立联系
			tp_catcher_task->m_request_key = "";
			tp_catcher_task->m_request_x = tp_catcher->m_actual_x;
			tp_catcher_task->m_request_y = tp_catcher->m_actual_y;
			tp_catcher_task->m_request_b = tp_catcher->m_actual_b;
			tp_catcher_task->m_request_z = tp_catcher->m_actual_z;
			tp_catcher_task->m_request_d1 = tp_catcher->m_actual_d1;
			tp_catcher_task->m_request_d2 = tp_catcher->m_actual_d2;
			tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;
			tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)tp_catcher->m_actual_clamp_motion1;

			t_error = tp_catcher->execute_task(m_dispatch_catcher_node.mp_main_catcher_task, dispatch_task_level);
		}
	}
	else
	{
		t_error.error_manager_reset(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
									" Dispatch_process::connect_dispatch_catcher() POINTER IS NULL ");
	}
	return t_error;
}

//断开调度设备, 收回任务单, 释放任务单 与设备解除连接
Error_manager Dispatch_process::disconnect_dispatch_device(std::shared_ptr<Dispatch_device_base> & p_device, std::shared_ptr<Task_Base> & p_device_task)
{
	if ( p_device.get() != NULL && p_device_task.get() != NULL )
	{
		p_device_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW);
		p_device.reset();
		p_device_task.reset();
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
}




//发送调度总计划
Error_manager Dispatch_process::send_dispatch_plan_request_msg()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	m_dispatch_plan_request_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_plan_request_msg);
	m_dispatch_plan_request_msg.mutable_base_info()->set_timeout_ms(m_timeout_ms);
	m_dispatch_plan_request_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
	m_dispatch_plan_request_msg.mutable_base_info()->set_receiver(message::Communicator::eDispatch_control);
	m_dispatch_plan_request_msg.set_command_key(m_dispatch_request_msg.command_key());

	m_dispatch_plan_request_msg.set_dispatch_task_type((message::Dispatch_task_type)m_dispatch_process_type);
	m_dispatch_plan_request_msg.set_dispatch_source(m_dispatch_source);
	m_dispatch_plan_request_msg.set_dispatch_destination(m_dispatch_destination);
	//这里不写错误码

	std::string t_msg = m_dispatch_plan_request_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg);
	return Error_code::SUCCESS;
}

//执行调度控制指令, 并根据完成情况给答复
Error_manager Dispatch_process::dispatch_control_motion()
{
	Error_manager t_error;
	std::unique_lock<std::mutex> t_lock(m_lock);
//	m_result.error_manager_clear_all();
	for (auto iter = m_dispatch_control_node_map.begin(); iter != m_dispatch_control_node_map.end(); ++iter)
	{
		switch ( iter->second.m_dispatch_control_status )
		{
			case DISPATCH_CONTROL_STATUS_UNKNOW:
			case DISPATCH_CONTROL_CREATED:
			case DISPATCH_CONTROL_READY:
			{
				t_error = wait_dispatch_control_request_msg(iter->first, iter->second);
				if ( t_error == Error_code::SUCCESS)
				{
					//流程正常, 就进入完成状态,
					iter->second.m_dispatch_control_status = DISPATCH_CONTROL_CONNECT_DEVICE;
				}
				//else 原地待命
				break;
			}
			case DISPATCH_CONTROL_CONNECT_DEVICE:
			{
				//连接调度设备
				t_error = connect_dispatch_device(iter->first, iter->second);
				if ( t_error !=Error_code::SUCCESS)
				{
					iter->second.m_error = t_error;
					iter->second.m_dispatch_control_status = DISPATCH_CONTROL_RESPONSE;
				}
				else
				{
					//流程正常, 就进入工作状态,
					iter->second.m_dispatch_control_status = DISPATCH_CONTROL_WORKING;
					iter->second.m_time_to_send_control_response = std::chrono::system_clock::now();
				}
				break;
			}
			case DISPATCH_CONTROL_WORKING:
			{
				//执行调度控制指令, 并根据完成情况给答复
				t_error = excute_dispatch_control(iter->first, iter->second);
				if ( t_error == Error_code::NODATA )
				{
					//继续 长流程, 什么也不做

					if ( std::chrono::system_clock::now() - iter->second.m_time_to_send_control_response >= std::chrono::seconds(1) )
					{
						//发送调度控制答复, 发给调度控制的
						t_error = send_dispatch_control_response_msg(iter->first, iter->second, message::E_TASK_WORKING);
						iter->second.m_time_to_send_control_response = std::chrono::system_clock::now();
					}
				}
				else
				{
					//长流程结束, 就答复 control_response_msg
					// 注:这里执行调度控制, 即使报错了 也要答复给调度控制, 交给调度控制来进行决策.
					iter->second.m_error = t_error;
					iter->second.m_dispatch_control_status = DISPATCH_CONTROL_TASK_WITHDRAW;
				}
				break;
			}
			case DISPATCH_CONTROL_TASK_WITHDRAW://流程 收回任务单
			{
				//发送调度控制答复, 发给调度控制的
				t_error = dispatch_control_withdraw_task(iter->first, iter->second);
				//流程正常, 就进入等待状态, 等待调度控制发送动作指令
				iter->second.m_dispatch_control_status = DISPATCH_CONTROL_RESPONSE;
				break;
			}
			case DISPATCH_CONTROL_RESPONSE://流程 给调度控制答复
			{
				//发送调度控制答复, 发给调度控制的
				t_error = send_dispatch_control_response_msg(iter->first, iter->second, message::E_TASK_OVER);
				//流程正常, 就进入等待状态, 等待调度控制发送动作指令
				iter->second.m_dispatch_control_status = DISPATCH_CONTROL_DISCONNECT_DEVICE;
				break;
			}
			case DISPATCH_CONTROL_DISCONNECT_DEVICE://流程 解除设备
			{
				//断开调度设备, 释放任务单 与设备解除连接
				t_error = disconnect_dispatch_device(iter->first, iter->second);
				if ( t_error == Error_code::SUCCESS )
				{
					//流程正常, 就回到 等待状态, 等待调度控制发送动作指令
					iter->second.m_error.error_manager_clear_all();
					iter->second.m_dispatch_control_status = DISPATCH_CONTROL_READY;
				}
				//else 保持不变继续等待

				break;
			}
			default:
			{
				break;
			}
		}
	}
	return Error_code::SUCCESS;
}


//等待控制指令
Error_manager Dispatch_process::wait_dispatch_control_request_msg(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
{
	//key不相等 就表示 收到了新的控制指令
	if ( dispatch_control_node.m_dispatch_control_request_msg.command_key() != dispatch_control_node.m_dispatch_control_response_msg.command_key() )
	{
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_code::NODATA;
	}
	return Error_code::SUCCESS;
}


//连接调度设备, 创建新的任务单 与设备建立连接
Error_manager Dispatch_process::connect_dispatch_device(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;

	if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 ||
		 dispatch_device_type == message::Dispatch_device_type::ROBOT_2 )
	{
		//找到对应的设备
		if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 )
		{
			dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_catcher_map[0];
		}
		if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_2 )
		{
			dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_catcher_map[1];
		}

		Catcher* tp_catcher = (Catcher*)dispatch_control_node.mp_dispatch_device.get();
		//检查设备状态
		if ( tp_catcher->check_status() == Error_code::SUCCESS &&
			 tp_catcher->m_actual_device_status == Dispatch_device_base::HARDWARE_DEVICE_READY )
		{
			//创建任务单
			dispatch_control_node.mp_dispatch_task = std::shared_ptr<Task_Base>(new Catcher_task);
			dispatch_control_node.mp_dispatch_task->task_init(NULL,std::chrono::milliseconds(15000));
			Catcher_task * tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();

			//第一次发送 空的唯一码, 可以和设备建立联系
			tp_catcher_task->m_request_key = "";
			tp_catcher_task->m_request_x = tp_catcher->m_actual_x;
			tp_catcher_task->m_request_y = tp_catcher->m_actual_y;
			tp_catcher_task->m_request_b = tp_catcher->m_actual_b;
			tp_catcher_task->m_request_z = tp_catcher->m_actual_z;
			tp_catcher_task->m_request_d1 = tp_catcher->m_actual_d1;
			tp_catcher_task->m_request_d2 = tp_catcher->m_actual_d2;
			tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;
			tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)tp_catcher->m_actual_clamp_motion1;

			t_error = tp_catcher->execute_task(dispatch_control_node.mp_dispatch_task, Dispatch_device_base::DISPATCH_TASK_ONE_LEVEL);
			if ( t_error != Error_code::SUCCESS )
			{
				dispatch_control_node.m_error = t_error;
				return t_error;
			}
		}
		else
		{
			std::cout << " huli test :::: " << " 123123123123123123123 = " << 1233 << std::endl;
			std::cout << " huli test :::: " << " tp_catcher->check_status() = " << tp_catcher->check_status().to_string() << std::endl;
			std::cout << " huli test :::: " << " tp_catcher->m_actual_device_status = " << tp_catcher->m_actual_device_status << std::endl;
			t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
									" tp_catcher->m_actual_device_status device_status error ");
			dispatch_control_node.m_error = t_error;
			return t_error;
		}

		//设置起点
		dispatch_control_node.m_source_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates_map[dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()];
		//设置终点
		dispatch_control_node.m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates_map[dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination()];

		std::cout << " huli test :::: " << " ---------------------------------------- = " << 123 << std::endl;
		std::cout << " huli test :::: " << " dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() = " << dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() << std::endl;
		std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.x = " << dispatch_control_node.m_destination_coordinates.x << std::endl;
		std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.y = " << dispatch_control_node.m_destination_coordinates.y << std::endl;
		std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.z = " << dispatch_control_node.m_destination_coordinates.z << std::endl;
		std::cout << " huli test :::: " << "*******************************************" << 123 << std::endl;
	}
		//搬运器的配置 准备工作
	else if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 ||
			  dispatch_device_type == message::Dispatch_device_type::CARRIER_2 ||
			  dispatch_device_type == message::Dispatch_device_type::CARRIER_3  )
	{
		//找到对应的设备
		if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 )
		{
			dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[0];
		}
		if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_2 )
		{
			dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[1];
		}
		if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_3 )
		{
			dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[2];
		}

		Carrier* tp_carrier = (Carrier*)dispatch_control_node.mp_dispatch_device.get();
		//检查设备状态
		if ( tp_carrier->check_status() == Error_code::SUCCESS &&
			 tp_carrier->m_actual_device_status == Dispatch_device_base::HARDWARE_DEVICE_READY )
		{
			//创建任务单
			dispatch_control_node.mp_dispatch_task = std::shared_ptr<Task_Base>(new Carrier_task);

			dispatch_control_node.mp_dispatch_task->task_init(NULL,std::chrono::milliseconds(15000));
			Carrier_task * tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();

			//第一次发送 空的唯一码, 可以和设备建立联系
			tp_carrier_task->m_request_key = "";
			tp_carrier_task->m_request_x = tp_carrier->m_actual_x;
			tp_carrier_task->m_request_y = tp_carrier->m_actual_y;
			tp_carrier_task->m_request_z = tp_carrier->m_actual_z;
			tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y1;
			tp_carrier_task->m_request_y2 = tp_carrier->m_actual_y2;
			tp_carrier_task->m_request_clamp_motion = (Carrier_task::Clamp_motion)tp_carrier->m_actual_clamp_motion1;
			tp_carrier_task->m_request_joint_motion_x = (Carrier_task::Joint_motion)tp_carrier->m_actual_joint_motion_x1;
			tp_carrier_task->m_request_joint_motion_y = Carrier_task::Joint_motion::E_JOINT_NO_ACTION;
			tp_carrier_task->m_request_space_id = 0;
			tp_carrier_task->m_request_floor_id = 0;
			tp_carrier_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;

			t_error = tp_carrier->execute_task(dispatch_control_node.mp_dispatch_task, Dispatch_device_base::DISPATCH_TASK_ONE_LEVEL);
			if ( t_error != Error_code::SUCCESS )
			{
				dispatch_control_node.m_error = t_error;
				return t_error;
			}
		}
		else
		{
			std::cout << " huli test :::: " << " 123123123123123123123 = " << 4566 << std::endl;
			std::cout << " huli test :::: " << " tp_carrier->check_status() = " << tp_carrier->check_status().to_string() << std::endl;
			std::cout << " huli test :::: " << " tp_carrier->m_actual_device_status = " << tp_carrier->m_actual_device_status << std::endl;
			t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
									" tp_carrier->m_actual_device_status device_status error ");
			dispatch_control_node.m_error = t_error;
			return t_error;
		}

		//设置起点
		dispatch_control_node.m_source_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates_map[dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()];
		//设置终点
		dispatch_control_node.m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates_map[dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination()];

		std::cout << " huli test :::: " << " ---------------------------------------- = " << 123 << std::endl;
		std::cout << " huli test :::: " << " dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() = " << dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() << std::endl;
		std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.x = " << dispatch_control_node.m_destination_coordinates.x << std::endl;
		std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.y = " << dispatch_control_node.m_destination_coordinates.y << std::endl;
		std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.z = " << dispatch_control_node.m_destination_coordinates.z << std::endl;
		std::cout << " huli test :::: " << "*******************************************" << 123 << std::endl;
	}
	else
	{
		t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_TYPE_ERROR, Error_level::MINOR_ERROR,
								" DISPATCH_PROCESS_DEVICE_TYPE_ERROR fun error ");
		dispatch_control_node.m_error = t_error;
		return t_error;
	}

	return Error_code::SUCCESS;
}


//执行调度控制指令, 并根据完成情况给答复
Error_manager Dispatch_process::excute_dispatch_control(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 ||
		 dispatch_device_type == message::Dispatch_device_type::ROBOT_2 )
	{
		Catcher_task * tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
//		while (1)
//		{
//			char in;
//			std::cin >> in ;
//			if ( in == 'p' )
//			{
//				break;
//			}
//		}
//		std::cout << " huli test :::: " << "ppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppp  = " << tp_catcher_task->m_step << std::endl;
//		std::cout << " huli test :::: " << "1111111 tp_catcher_task->m_step   = " << tp_catcher_task->m_step << std::endl;

		//设备的动作也使用外部的Main()的线程来循环
		switch ( dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type() )
		{
			case message::Dispatch_task_type::ROBOT_CATCH_CAR_FROM_INLET:
			{
				t_error = excute_robot_catch_car_from_inlet(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::ROBOT_PUT_CAR_TO_CARRIER:
			{
				t_error = excute_robot_put_car_to_carrier(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::ROBOT_CATCH_CAR_FROM_CARRIER:
			{
				t_error = excute_robot_catch_car_from_carrier(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::ROBOT_PUT_CAR_TO_OUTLET:
			{
				t_error = excute_robot_put_car_to_outlet(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::ROBOT_MOVE:
			{
				t_error = excute_robot_move(dispatch_control_node);
				break;
			}

			default:
			{
				return Error_manager(Error_code::DISPATCH_PROCESS_TASK_STATUS_ERROR, Error_level::MINOR_ERROR,
									 "Dispatch_process::excute_dispatch_control() fun error ");
				break;
			}
		}
//		std::cout << " huli test :::: " << "222222 tp_catcher_task->m_step   = " << tp_catcher_task->m_step << std::endl;
//		std::cout << " huli test :::: " << " t_error_catcher = " << t_error.to_string() << std::endl;
	}
	else if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 ||
			  dispatch_device_type == message::Dispatch_device_type::CARRIER_2 ||
			  dispatch_device_type == message::Dispatch_device_type::CARRIER_3  )
	{
		Catcher_task *tp_carrier_task = (Catcher_task *) dispatch_control_node.mp_dispatch_task.get();
//		while (1)
//		{
//			char in;
//			std::cin >> in;
//			if (in == 'l')
//			{
//				break;
//			}
//		}
//		std::cout << " huli test :::: "
//				  << "lllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll   = "
//				  << tp_carrier_task->m_step << std::endl;
//		std::cout << " huli test :::: " << "3333333 tp_carrier_task->m_step   = " << tp_carrier_task->m_step
//				  << std::endl;

		//设备的动作也使用外部的Main()的线程来循环
		switch (dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type())
		{
			case message::Dispatch_task_type::CARRIER_RECEIVE_CAR_FROM_ROBOT:
			{
				t_error = excute_carrier_receive_car_from_robot(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::CARRIER_STORE_CAR_TO_PARKINGSPACE:
			{
				t_error = excute_carrier_store_car_to_parkingspace(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::CARRIER_STORE_CAR_TO_PARKINGSPACE_EX:
			{
				t_error = excute_carrier_store_car_to_parkingspace_ex(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::CARRIER_PICKUP_CAR_FROM_PARKINGSPACE:
			{
				t_error = excute_carrier_pickup_car_from_parkingspace(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::CARRIER_PICKUP_CAR_FROM_PARKINGSPACE_EX:
			{
				t_error = excute_carrier_pickup_car_from_parkingspace_ex(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::CARRIER_DELIVER_CAR_TO_ROBOT:
			{
				t_error = excute_carrier_deliver_car_to_robot(dispatch_control_node);
				break;
			}
			case message::Dispatch_task_type::CARRIER_MOVE:
			{
				t_error = excute_carrier_move(dispatch_control_node);
				break;
			}
			default:
			{
				return Error_manager(Error_code::DISPATCH_PROCESS_TASK_STATUS_ERROR, Error_level::MINOR_ERROR,
									 "Dispatch_process::excute_dispatch_control() fun error ");
				break;
			}
		}
//		std::cout << " huli test :::: " << "44444444 tp_carrier_task->m_step   = " << tp_carrier_task->m_step << std::endl;
//		std::cout << " huli test :::: " << " t_error_carrier = " << t_error.to_string() << std::endl;
	}
	dispatch_control_node.m_error = t_error;
	return t_error;
}
//流程通知设备 收回任务单
Error_manager Dispatch_process::dispatch_control_withdraw_task(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
{
	if ( dispatch_control_node.mp_dispatch_device.get() != NULL && dispatch_control_node.mp_dispatch_task.get() != NULL )
	{
		dispatch_control_node.mp_dispatch_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW);
	}
	return Error_code::SUCCESS;
}
//发送调度控制答复, 发给调度控制的
Error_manager Dispatch_process::send_dispatch_control_response_msg(int dispatch_device_type, Dispatch_control_node & dispatch_control_node, message::Dispatch_device_task_status dispatch_device_task_status)
{
	dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_control_response_msg);
	dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_timeout_ms(dispatch_control_node.m_dispatch_control_request_msg.base_info().timeout_ms());
	dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
	dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_receiver(message::Communicator::eDispatch_control);
	dispatch_control_node.m_dispatch_control_response_msg.set_command_key(dispatch_control_node.m_dispatch_control_request_msg.command_key());

	dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_task_type(dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type());
	dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_type(dispatch_control_node.m_dispatch_control_request_msg.dispatch_device_type());
	dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_source(dispatch_control_node.m_dispatch_control_request_msg.dispatch_source());
	dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_destination(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination());

	dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_code(dispatch_control_node.m_error.get_error_code());
	dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_level((message::Error_level)dispatch_control_node.m_error.get_error_level());
	dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_description(dispatch_control_node.m_error.get_error_description(), dispatch_control_node.m_error.get_description_length());

	dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_target_status(dispatch_control_node.m_dispatch_control_request_msg.dispatch_device_target_status());
	dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_task_status(dispatch_device_task_status);

	std::string t_msg = dispatch_control_node.m_dispatch_control_response_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg);

	if ( dispatch_device_task_status == message::Dispatch_device_task_status::E_TASK_OVER )
	{
		LOG(INFO) << " dispatch_control_node.m_dispatch_control_response_msg =  "<< dispatch_control_node.m_dispatch_control_response_msg.DebugString() <<  "   " << this;
	}
	return Error_code::SUCCESS;
}
//断开调度设备, 释放任务单 与设备解除连接
Error_manager Dispatch_process::disconnect_dispatch_device(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
{
	if ( dispatch_control_node.mp_dispatch_device.get() != NULL && dispatch_control_node.mp_dispatch_task.get() != NULL )
	{
		if ( dispatch_control_node.mp_dispatch_task->get_task_statu() == Task_Base::Task_statu::TASK_FREE )
		{
			dispatch_control_node.mp_dispatch_task.reset();
			dispatch_control_node.mp_dispatch_device.reset();
			return Error_code::SUCCESS;
		}
		else
		{
			return Error_code::NODATA;
		}
	}
	return Error_code::SUCCESS;
}

//等待调度总计划答复
Error_manager Dispatch_process::wait_dispatch_plan_response_msg()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	//key 相等 就表示 收到了总计划答复
	if ( m_dispatch_plan_request_msg.command_key() == m_dispatch_plan_response_msg.command_key() )
	{
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_code::NODATA;
	}
}

//发送调度答复, 发给主控的
Error_manager Dispatch_process::send_dispatch_response_msg()
{
	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.mutable_error_manager()->CopyFrom(m_dispatch_plan_response_msg.error_manager());

	std::string t_msg = m_dispatch_response_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg);
	return Error_code::SUCCESS;
}

//通知调度管理, 释放资源,
Error_manager Dispatch_process::release_resource()
{
	return Dispatch_manager::get_instance_references().release_dispatch_process(m_command_key);
}

//异常处理
Error_manager Dispatch_process::Exception_handling()
{
	LOG(INFO) << " -------------Dispatch_process::Exception_handling------------------- "<< this;
	LOG(INFO) << " m_result =  "<< m_result.to_string() << "   " << this;
	return Error_code::SUCCESS;
}





//机器手去入口抓车(例如:机器手移动到2号入口上方,然后下降到一楼抓车,最后上升到最上方)
Error_manager Dispatch_process::excute_robot_catch_car_from_inlet(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Catcher * tp_catcher = NULL;
	Catcher_task * tp_catcher_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
		tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
//	getchar();
	if ( tp_catcher_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 1 )//检查姿态
	{
		if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
		{
			if ( tp_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE ||
				 tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance >
				 tp_dispatch_coordinates->m_catcher_wheel_base_limit ||
				 tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW	)
			{
				catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
				tp_catcher_task->m_step++;
			}
			else
			{
				tp_catcher_task->m_step +=2;
			}
		}
		else
		{
			return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "Dispatch_process::excute_robot_catch_car_from_inlet() fun error ");
		}
	}
	if ( tp_catcher_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 3 )//机器手z轴移到最高点.
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 5 )//机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车.
	{
		catcher_adjust_from_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 7 )//机器手z轴移到最低点.
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_1th_floor_z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 9 )//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
	{
		float temp_wheel_base = tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance;
		//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
		if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_catcher_task->m_step +=2;
		}
		else
		{
			catcher_adjust_wheel_base(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
			tp_catcher_task->m_step++;
		}
	}
	if ( tp_catcher_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 11 )//机器手 夹车
	{
		catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 13 )//机器手 z轴上升到最高处
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 14 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 15 )//机器手调整到 准备把车放到搬运器的姿态
	{
		//这里机器手要反向,
		catcher_adjust_to_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, true);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 16 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 17 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//机器手把车放到中跑车上面(例如:机器手下降到中跑车上放车,最后上升到最上方)(通过目标点 指定放到哪一个中跑车上)
Error_manager Dispatch_process::excute_robot_put_car_to_carrier(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Catcher * tp_catcher = NULL;
	Catcher_task * tp_catcher_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
		tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
//	getchar();
	if ( tp_catcher_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 1 )//检查姿态
	{
		if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR &&
			 tp_catcher->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT &&
			 (Common_data::approximate_difference(tp_catcher->m_actual_b, 90, DISPATCH_DEFAULT_DIFFERENCE) || Common_data::approximate_difference(tp_catcher->m_actual_b, 270, DISPATCH_DEFAULT_DIFFERENCE) ) )
		{
			//检测正常, 直接跳过即可
			tp_catcher_task->m_step +=2;
		}
		else
		{
			return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "Dispatch_process::excute_robot_put_car_to_carrier() fun error ");
		}
	}
	if ( tp_catcher_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴
	{
		if ( tp_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z )
		{
			//检测正常, 直接跳过即可
			tp_catcher_task->m_step +=2;
		}
		else
		{
			catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
			tp_catcher_task->m_step++;
		}
	}
	if ( tp_catcher_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到搬运器的姿态
	{
		float temp_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
		if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) &&
			 Common_data::approximate_difference(tp_catcher->m_actual_y, temp_y, DISPATCH_DEFAULT_DIFFERENCE))
		{
			//检测正常, 直接跳过即可
			tp_catcher_task->m_step +=2;
		}
		else
		{
			catcher_adjust_to_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, false);
			tp_catcher_task->m_step++;
		}
	}
	if ( tp_catcher_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 9 )//机器手 松开夹杆
	{
		catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 11 )//机器手 z轴上升
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 13 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//机器手去中跑车上抓车(例如:机器手移动到1号中跑车上方,然后下降到中跑车抓车,最后上升到最上方)
Error_manager Dispatch_process::excute_robot_catch_car_from_carrier(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Catcher * tp_catcher = NULL;
	Catcher_task * tp_catcher_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
		tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
//	getchar();
	if ( tp_catcher_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 1 )
	{
		if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
		{
			if ( tp_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE ||
				 tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance >
				 tp_dispatch_coordinates->m_catcher_wheel_base_limit ||
				 tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW	)
			{
				catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
				tp_catcher_task->m_step++;
			}
			else
			{
				tp_catcher_task->m_step +=2;
			}
		}
		else
		{
			return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "Dispatch_process::excute_robot_catch_car_from_carrier() fun error ");
		}
	}
	if ( tp_catcher_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴
	{
		if ( tp_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z )
		{
			//检测正常, 直接跳过即可
			tp_catcher_task->m_step +=2;
		}
		else
		{
			catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
			tp_catcher_task->m_step++;
		}
	}
	if ( tp_catcher_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到搬运器的姿态
	{
		float temp_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
		if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) &&
			 Common_data::approximate_difference(tp_catcher->m_actual_y, temp_y, DISPATCH_DEFAULT_DIFFERENCE))
		{
			//检测正常, 直接跳过即可
			tp_catcher_task->m_step +=2;
		}
		else
		{
			catcher_adjust_from_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
			tp_catcher_task->m_step++;
		}
	}
	if ( tp_catcher_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 9 )//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
	{
		float temp_wheel_base = tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance;
		//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
		if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_catcher_task->m_step +=2;
		}
		else
		{
			catcher_adjust_wheel_base(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
			tp_catcher_task->m_step++;
		}
	}
	if ( tp_catcher_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 11 )//机器手 夹车
	{
		catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 13 )//机器手 z轴上升
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 14 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 15 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//机器手去出口放车(例如:机器手移动到3号出口上方,然后下降到一楼放车,最后上升到最上方)
Error_manager Dispatch_process::excute_robot_put_car_to_outlet(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Catcher * tp_catcher = NULL;
	Catcher_task * tp_catcher_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
		tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
//	getchar();
	if ( tp_catcher_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 1 )//检查姿态
	{
		if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR &&
			 tp_catcher->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT &&
			 (Common_data::approximate_difference(tp_catcher->m_actual_b, 90, DISPATCH_DEFAULT_DIFFERENCE) || Common_data::approximate_difference(tp_catcher->m_actual_b, 270, DISPATCH_DEFAULT_DIFFERENCE) ) )
		{
			//检测正常, 直接跳过即可
			tp_catcher_task->m_step +=2;
		}
		else
		{
			return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "Dispatch_process::excute_robot_put_car_to_carrier() fun error ");
		}
	}
	if ( tp_catcher_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴
	{
		if ( tp_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z )
		{
			//检测正常, 直接跳过即可
			tp_catcher_task->m_step +=2;
		}
		else
		{
			catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
			tp_catcher_task->m_step++;
		}
	}
	if ( tp_catcher_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到地面的姿态
	{
		if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) &&
			 Common_data::approximate_difference(tp_catcher->m_actual_y, dispatch_control_node.m_destination_coordinates.y, DISPATCH_DEFAULT_DIFFERENCE))
		{
			//检测正常, 直接跳过即可
			tp_catcher_task->m_step +=2;
		}
		else
		{
			catcher_adjust_to_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
			tp_catcher_task->m_step++;
		}
	}
	if ( tp_catcher_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 9 )//机器手 松开夹杆
	{
		catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 11 )//机器手 z轴上升
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 13 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//机器手的自由移动(例如:机器手移动到6号出入口的上方4楼处,给其他设备避让)(不进行抓车和放车的操作)
Error_manager Dispatch_process::excute_robot_move(Dispatch_control_node & dispatch_control_node)
{

	Error_manager t_error;
	Catcher * tp_catcher = NULL;
	Catcher_task * tp_catcher_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
		tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
	}
	if ( tp_catcher_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 1 )//检查姿态
	{
		if (  tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance >
			  tp_dispatch_coordinates->m_catcher_wheel_base_limit ||
			  tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW	)
		{
			catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
			tp_catcher_task->m_step++;
		}
		else
		{
			tp_catcher_task->m_step +=2;
		}
	}
	if ( tp_catcher_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴
	{
		catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到地面的姿态
	{
		catcher_adjust_to_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 7 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//搬运器从机器手上接车(例如:搬运器移动到2号入口的上方,然后等待机器手把车放到中跑车上,小跑车保持松夹杆状态)
Error_manager Dispatch_process::excute_carrier_receive_car_from_robot(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Carrier * tp_carrier = NULL;
	Carrier_task * tp_carrier_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
		tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_carrier_task->m_step << std::endl;
//	getchar();
	if ( tp_carrier_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 1 )//检查姿态
	{
		//检查姿态
		if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
		{
			if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE &&
				 Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE))
			{
				tp_carrier_task->m_step +=2;
			}
			else
			{
				carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
				tp_carrier_task->m_step++;
			}
		}
		else
		{
			return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
		}
	}
	if ( tp_carrier_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				tp_carrier_task->m_step++;
			}
			else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				tp_carrier_task->m_step++;
			}
			else
			{
				return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
									 " m_destination_coordinates.z PARAMRTER ERROR ");
			}
		}
	}
	if ( tp_carrier_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 11 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
	{
		if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2)
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
			tp_carrier_task->m_step++;
		}
		else
		{
			tp_carrier_task->m_step +=2;
		}
	}
	if ( tp_carrier_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 14 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 15 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至电梯井)
Error_manager Dispatch_process::excute_carrier_store_car_to_parkingspace(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Carrier * tp_carrier = NULL;
	Carrier_task * tp_carrier_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
		tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_carrier_task->m_step << std::endl;
//	getchar();
	if ( tp_carrier_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 1 )//检查姿态
	{
		//检查姿态(注注注注注意了, 调试阶段是无车, 真实环境是有车的)
		if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR &&
			 tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE)
		{
			//检测正常, 直接跳过即可
			tp_carrier_task->m_step +=2;
		}
			//注意了, 如果是去7号出口, 那么就直接跳过轮距修正和夹车.
		else if(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() == 1107)
		{
			tp_carrier_task->m_step = 7;
		}
		else
		{
			return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "Dispatch_process::excute_carrier_store_car_to_parkingspace() fun error ");
		}
	}
	if ( tp_carrier_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 3 )//修正轴距
	{
		float temp_wheel_base = tp_carrier->m_actual_y1 - tp_carrier->m_actual_y2;
		if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_adjust_wheel_base(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 5 )//让小跑车 夹车
	{
		carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 7 )//让中跑车回到电梯井
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				tp_carrier_task->m_step++;
			}
			else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				tp_carrier_task->m_step++;
			}
			else
			{
				return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
									 " m_destination_coordinates.z PARAMRTER ERROR ");
			}
		}
	}
	if ( tp_carrier_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 9 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 11 )//电梯移动到对应的楼层
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 13 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
	{
		if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2)
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
			tp_carrier_task->m_step++;
		}
		else
		{
			tp_carrier_task->m_step +=2;
		}
	}
	if ( tp_carrier_task->m_step == 14 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 15 )//中跑车 x轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 16 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 17 )//小跑车 进入车位
	{
		carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 18 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 19 )//小跑车 松开夹杆
	{
		carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 20 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 21 )//小跑车 回到中跑车
	{
		carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 22 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 23 )//让中跑车回到电梯井
	{
		if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
			tp_carrier_task->m_step++;
		}
		else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
			tp_carrier_task->m_step++;
		}
		else
		{
			return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
								 " m_destination_coordinates.z PARAMRTER ERROR ");
		}
	}
	if ( tp_carrier_task->m_step == 24 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 25 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 26 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 27 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至56车位外面即可)
Error_manager Dispatch_process::excute_carrier_store_car_to_parkingspace_ex(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Carrier * tp_carrier = NULL;
	Carrier_task * tp_carrier_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
		tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
//	getchar();
	if ( tp_carrier_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 1 )//检查姿态
	{
		//检查姿态(注注注注注意了, 调试阶段是无车, 真实环境是有车的)
		if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR &&
			 tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE)
		{
			//检测正常, 直接跳过即可
			tp_carrier_task->m_step +=2;
		}
			//注意了, 如果是去7号出口, 那么就直接跳过轮距修正和夹车.
		else if(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() == 1107)
		{
			tp_carrier_task->m_step = 7;
		}
		else
		{
			return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "Dispatch_process::excute_carrier_store_car_to_parkingspace() fun error ");
		}
	}
	if ( tp_carrier_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 3 )//修正轴距
	{
		float temp_wheel_base = tp_carrier->m_actual_y1 - tp_carrier->m_actual_y2;
		if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_adjust_wheel_base(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 5 )//让小跑车 夹车
	{
		carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 7 )//让中跑车回到电梯井
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				tp_carrier_task->m_step++;
			}
			else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				tp_carrier_task->m_step++;
			}
			else
			{
				return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
									 " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
			}
		}
	}
	if ( tp_carrier_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 9 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 11 )//电梯移动到对应的楼层
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 13 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
	{
		if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2)
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
			tp_carrier_task->m_step++;
		}
		else
		{
			tp_carrier_task->m_step +=2;
		}
	}
	if ( tp_carrier_task->m_step == 14 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 15 )//中跑车 x轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 16 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 17 )//小跑车 进入车位
	{
		carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 18 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 19 )//小跑车 松开夹杆
	{
		carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 20 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 21 )//小跑车 回到中跑车
	{
		carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 22 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 23 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至电梯井)
Error_manager Dispatch_process::excute_carrier_pickup_car_from_parkingspace(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Carrier * tp_carrier = NULL;
	Carrier_task * tp_carrier_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
		tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
//	getchar();
	if ( tp_carrier_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 1 )//检查姿态
	{
		//检查姿态
		if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
		{
			if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE &&
				 Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE))
			{
				tp_carrier_task->m_step +=2;
			}
			else
			{
				carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
				tp_carrier_task->m_step++;
			}
		}
		else
		{
			return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
		}
	}
	if ( tp_carrier_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				tp_carrier_task->m_step++;
			}
			else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				tp_carrier_task->m_step++;
			}
			else
			{
				return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
									 " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
			}
		}
	}
	if ( tp_carrier_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 11 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
	{
		if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() != 2)
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
			tp_carrier_task->m_step++;
		}
		else
		{
			tp_carrier_task->m_step +=2;
		}
	}
	if ( tp_carrier_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 14 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 15 )//小跑车 进入车位
	{
		carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 16 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 17 )//小跑车 夹车
	{
		carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 18 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 19 )//小跑车 回到中跑车
	{
		carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 20 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 21 )//让中跑车回到电梯井
	{
		if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
			tp_carrier_task->m_step++;
		}
		else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
			tp_carrier_task->m_step++;
		}
		else
		{
			return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
								 " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
		}
	}
	if ( tp_carrier_task->m_step == 22 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 23 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( tp_carrier->get_device_id() != 2 )
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			tp_carrier_task->m_step++;
		}
		else
		{
			tp_carrier_task->m_step +=2;
		}

	}
	if ( tp_carrier_task->m_step == 24 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 25 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至56车位外面即可)
Error_manager Dispatch_process::excute_carrier_pickup_car_from_parkingspace_ex(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Carrier * tp_carrier = NULL;
	Carrier_task * tp_carrier_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
		tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
//	getchar();
	if ( tp_carrier_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 1 )//检查姿态
	{
		//检查姿态
		if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
		{
			if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE &&
				 Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE))
			{
				tp_carrier_task->m_step +=2;
			}
			else
			{
				carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
				tp_carrier_task->m_step++;
			}
		}
		else
		{
			return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
		}
	}
	if ( tp_carrier_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				tp_carrier_task->m_step++;
			}
			else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				tp_carrier_task->m_step++;
			}
			else
			{
				return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
									 " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
			}
		}
	}
	if ( tp_carrier_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 11 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
	{
		if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() != 2)
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
			tp_carrier_task->m_step++;
		}
		else
		{
			tp_carrier_task->m_step +=2;
		}
	}
	if ( tp_carrier_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 14 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 15 )//小跑车 进入车位
	{
		carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 16 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 17 )//小跑车 夹车
	{
		carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 18 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 19 )//小跑车 回到中跑车
	{
		carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 20 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 21 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//搬运器把车交付给机器手(例如:搬运器移动到3号入口的上方,小跑车松夹杆,然后等待机器手把车从中跑车上取走)
Error_manager Dispatch_process::excute_carrier_deliver_car_to_robot(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Carrier * tp_carrier = NULL;
	Carrier_task * tp_carrier_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
		tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
//	getchar();
	if ( tp_carrier_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 1 )//检查姿态
	{
		//检查姿态
		if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR &&
			 tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT)
		{
			//检测正常, 直接跳过
			tp_carrier_task->m_step +=2;
		}
		else
		{
			return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
								 "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
		}
	}
	if ( tp_carrier_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				tp_carrier_task->m_step++;
			}
			else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				tp_carrier_task->m_step++;
			}
			else
			{
				return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
									 " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
			}
		}
	}
	if ( tp_carrier_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 11 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
	{
		if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2)
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
			tp_carrier_task->m_step++;
		}
		else
		{
			tp_carrier_task->m_step +=2;
		}
	}
	if ( tp_carrier_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 14 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 15 )//小跑车 松开夹杆
	{
		carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
		tp_carrier_task->m_step++;
	}
	if ( tp_carrier_task->m_step == 16 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 17 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//搬运器的自由移动(可以提前到2楼来准备接车,或者为了避让就退回至电梯井)(小跑车不进行取车和存车)
Error_manager Dispatch_process::excute_carrier_move(Dispatch_control_node & dispatch_control_node)
{
	Error_manager t_error;
	Carrier * tp_carrier = NULL;
	Carrier_task * tp_carrier_task = NULL;
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();

	if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
		tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
	}
//	std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
//	getchar();
	if ( tp_carrier_task->m_step == 0 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 1 )//让小跑车回到中跑车上
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 2 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 3 )//让中跑车回到电梯井
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
				tp_carrier_task->m_step++;
			}
			else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
			{
				carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
				tp_carrier_task->m_step++;
			}
			else
			{
				return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
									 " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
			}
		}
	}
	if ( tp_carrier_task->m_step == 4 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 5 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 6 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 7 )//电梯移动到对应的楼层
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 8 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 9 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
	{
		if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2)
		{
			carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
			tp_carrier_task->m_step++;
		}
		else
		{
			tp_carrier_task->m_step +=2;
		}
	}
	if ( tp_carrier_task->m_step == 10 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 11 )//中跑车 x轴移动
	{
		if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 12 )
	{
		return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 13 )
	{
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}

//执行通道口动作
Error_manager Dispatch_process::excute_passageway_motion(Dispatch_control_node & dispatch_control_node)
{
	return Error_code::SUCCESS;
}


//检查 任务单 是否完成任务, 里面不会调整短步骤
Error_manager Dispatch_process::check_task_status(std::shared_ptr<Task_Base> p_task)
{
	if ( p_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_passageway_task  POINTER IS NULL ");
	}
	else
	{
		if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
//			dispatch_control_status = (Dispatch_control_status)(dispatch_control_status+1);
//			return Error_code::NODATA;	//这里返回nodata 表示任务继续下一步
			//注意了, 这里返回成功, 用作动作完成判断
			return Error_code::SUCCESS;
		}
		else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
//			dispatch_control_status = DISPATCH_CONTROL_FAULT;
			return p_task->get_task_error_manager();
		}
		else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING ||
				  p_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED ||
				  p_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			//继续等待任务, 直到状态改变
			return Error_code::NODATA;
		}
		else
		{
			LOG(INFO) << "Dispatch_process::check_task_ex failed p_catcher_task->get_task_statu() =  "<< p_task->get_task_statu() << "   " << this;
			return Error_code::NODATA;
		}
	}
	return Error_code::SUCCESS;
}//检查 任务单 是否完成任务, 里面会调整短步骤
Error_manager Dispatch_process::check_task_status(std::shared_ptr<Task_Base> p_task, Dispatch_control_status & dispatch_control_status)
{
	if ( p_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_passageway_task  POINTER IS NULL ");
	}
	else
	{
		if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			dispatch_control_status = (Dispatch_control_status)(dispatch_control_status+1);
//			return Error_code::NODATA;	//这里返回nodata 表示任务继续下一步
			//注意了, 这里返回成功, 用作动作完成判断
			return Error_code::SUCCESS;
		}
		else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			dispatch_control_status = DISPATCH_CONTROL_FAULT;
			return p_task->get_task_error_manager();
		}
		else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING ||
				  p_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED ||
				  p_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			//继续等待任务, 直到状态改变
			return Error_code::NODATA;
		}
		else
		{
			LOG(INFO) << "Dispatch_process::check_task_ex failed p_catcher_task->get_task_statu() =  "<< p_task->get_task_statu() << "   " << this;
			return Error_code::NODATA;
		}
	}
	return Error_code::SUCCESS;
}
//检查 任务单 是否完成任务, 里面会调整短步骤
Error_manager Dispatch_process::check_task_ex(std::shared_ptr<Task_Base> p_task, int& step)
{
	if ( p_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_passageway_task  POINTER IS NULL ");
	}
	else
	{
//		std::cout << " huli test :::: " << " ffffffffffffffffffffffffffffffffffffffffffffffffffffff = " << 333 << std::endl;
//		std::cout << " huli test :::: " << " p_task->get_task_statu() = " << p_task->get_task_statu() << std::endl;
//		std::cout << " huli test :::: " << " gggggggggggggggggggggggggggggggggggggggggggggggggggggg = " << 333 << std::endl;


		if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			step++;
			return Error_code::NODATA;	//这里返回nodata 表示任务继续下一步
		}
		else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			step = 0;
			return p_task->get_task_error_manager();
		}
		else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING ||
				  p_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED ||
				  p_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			//继续等待任务, 直到状态改变
			return Error_code::NODATA;
		}
		else
		{
			LOG(INFO) << "Dispatch_process::check_task_ex failed p_catcher_task->get_task_statu() =  "<< p_task->get_task_statu() << "   " << this;
			return Error_code::NODATA;
		}
	}
	return Error_code::SUCCESS;
}

//机器手调整到正常待机的姿态(调节夹杆和轴距)
Error_manager Dispatch_process::catcher_adjust_to_ready(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_request_key = t_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//调整姿态
	tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;
	tp_catcher_task->m_request_d1 = tp_dispatch_coordinates->m_catcher_d1_min;
	tp_catcher_task->m_request_d2 = tp_dispatch_coordinates->m_catcher_d2_min;
	if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
	{
		tp_catcher_task->m_request_b = 270;
	}
	else
	{
		tp_catcher_task->m_request_b = 90;
	}

	std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;

	return Error_code::SUCCESS;
}

//机器手 移动z
Error_manager Dispatch_process::catcher_move_z(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_request_key = t_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 移动z
	tp_catcher_task->m_request_z = target;

	std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_z = " << tp_catcher_task->m_request_z << std::endl;

	return Error_code::SUCCESS;
}

//机器手调整到 准备从地面抓车前的姿态
Error_manager Dispatch_process::catcher_adjust_from_ground(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_request_key = t_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车.
	tp_catcher_task->m_request_x = m_car_measure_information.center_x;
	tp_catcher_task->m_request_y = m_car_measure_information.center_y;
	if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
	{
		//机器手如果旋转反向, 那就+180即可, 没有必要旋转归位.
		tp_catcher_task->m_request_b = 180 + m_car_measure_information.car_angle;
	}
	else
	{
		tp_catcher_task->m_request_b = m_car_measure_information.car_angle;
	}
	//限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升.
	if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit )
	{
		tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit;
	}
	else
	{
		tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = m_wheel_base;
	}
	tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;

	std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;
	std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;


	return Error_code::SUCCESS;
}

//机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
Error_manager Dispatch_process::catcher_adjust_wheel_base(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);

	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_request_key = t_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
	//修正轴距
	tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
	tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
	tp_catcher_task->m_request_wheelbase = m_wheel_base;

	std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;

	return Error_code::SUCCESS;
}

//机器手 移动c轴 夹杆
Error_manager Dispatch_process::catcher_move_c(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, Catcher_task::Clamp_motion target)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_request_key = t_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_catcher_task->m_request_clamp_motion = target;

	std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;

	return Error_code::SUCCESS;
}

//机器手调整到 准备把车放到搬运器的姿态
Error_manager Dispatch_process::catcher_adjust_to_carrier(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, bool reverse_flag)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_request_key = t_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
	tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x;
	tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
	//存车需要反向, 取车不需要
	if ( reverse_flag )
	{
		if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
		{
			tp_catcher_task->m_request_b = 90;
		}
		else
		{
			tp_catcher_task->m_request_b = 270;
		}
	}
	else
	{
		if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
		{
			tp_catcher_task->m_request_b = 270;
		}
		else
		{
			tp_catcher_task->m_request_b = 90;
		}
	}

	std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl;

	return Error_code::SUCCESS;
}

//机器手调整到 准备把车放到地面的姿态
Error_manager Dispatch_process::catcher_adjust_to_ground(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_request_key = t_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
	tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x;
	tp_catcher_task->m_request_y = dispatch_control_node.m_destination_coordinates.y;
	if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
	{
		tp_catcher_task->m_request_b = 270;
	}
	else
	{
		tp_catcher_task->m_request_b = 90;
	}

	std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl;

	return Error_code::SUCCESS;
}

//机器手调整到 对接搬运器的姿态
Error_manager Dispatch_process::catcher_adjust_from_carrier(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_request_key = t_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
	tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x;
	tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);

	if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
	{
		tp_catcher_task->m_request_b = 270;
	}
	else
	{
		tp_catcher_task->m_request_b = 90;
	}

	//限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升.
	if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit )
	{
		tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit;
	}
	else
	{
		tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = m_wheel_base;
	}
	tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;

	std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;
	std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;

	return Error_code::SUCCESS;
}


//搬运器调整到 正常待机的姿态(调节夹杆和轴距)
Error_manager Dispatch_process::carrier_adjust_to_ready(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
	tp_carrier_task->m_request_key = t_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_clamp_motion = Carrier_task::Clamp_motion::E_CLAMP_LOOSE;
	tp_carrier_task->m_request_wheelbase = m_wheel_base;
	tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
	tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;

	std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 移动x
Error_manager Dispatch_process::carrier_move_x(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
	tp_carrier_task->m_request_key = t_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_x = target;

	std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_x = " << tp_carrier_task->m_request_x << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 移动y
Error_manager Dispatch_process::carrier_move_y(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
	tp_carrier_task->m_request_key = t_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_y = target;
	tp_carrier_task->m_request_wheelbase = m_wheel_base;
	tp_carrier_task->m_request_y1 = tp_carrier_task->m_request_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
	tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;

	std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y = " << tp_carrier_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 移动z
Error_manager Dispatch_process::carrier_move_z(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
	tp_carrier_task->m_request_key = t_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_z = target;

	std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_z = " << tp_carrier_task->m_request_z << std::endl;

	return Error_code::SUCCESS;
}
//搬运器 移动c轴 夹车杆
Error_manager Dispatch_process::carrier_move_c(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Clamp_motion target)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
	tp_carrier_task->m_request_key = t_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_clamp_motion = target;

	std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl;

	return Error_code::SUCCESS;
}
//搬运器调整 水平的交接
Error_manager Dispatch_process::carrier_joint_x(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Joint_motion target)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
	tp_carrier_task->m_request_key = t_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_joint_motion_x = target;

	std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_joint_motion_x = " << tp_carrier_task->m_request_joint_motion_x << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 修正轴距
Error_manager Dispatch_process::carrier_adjust_wheel_base(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
	tp_carrier_task->m_request_key = t_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_wheelbase = m_wheel_base;
	tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
	tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;

	std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;

	return Error_code::SUCCESS;
}


//机器手 准备开始, 需要同步任务单和设备真实数据.
Error_manager Dispatch_process::catcher_ready_to_start(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{
	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_catcher_task->m_request_x = tp_catcher->m_actual_x;
	tp_catcher_task->m_request_y = tp_catcher->m_actual_y;
	tp_catcher_task->m_request_b = tp_catcher->m_actual_b;
	tp_catcher_task->m_request_z = tp_catcher->m_actual_z;
	tp_catcher_task->m_request_d1 = tp_catcher->m_actual_d1;
	tp_catcher_task->m_request_d2 = tp_catcher->m_actual_d2;
	tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;
	tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)tp_catcher->m_actual_clamp_motion1;

	return Error_code::SUCCESS;
}
//机器手调整到正常待机的姿态(调节夹杆和轴距)
Error_manager Dispatch_process::catcher_adjust_to_ready(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//调整姿态
	tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;
	tp_catcher_task->m_request_d1 = tp_dispatch_coordinates->m_catcher_d1_min;
	tp_catcher_task->m_request_d2 = tp_dispatch_coordinates->m_catcher_d2_min;
	if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
	{
		tp_catcher_task->m_request_b = 270;
	}
	else
	{
		tp_catcher_task->m_request_b = 90;
	}

	std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;

	return Error_code::SUCCESS;
}

//机器手 移动x
Error_manager Dispatch_process::catcher_move_x(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 移动x
	tp_catcher_task->m_request_x = target_x;

	std::cout << " huli test :::: " << " catcher_move_x = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_move_x = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;

	return Error_code::SUCCESS;
}
//机器手 移动y
Error_manager Dispatch_process::catcher_move_y(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_y)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 移动y
	tp_catcher_task->m_request_y = target_y;

	std::cout << " huli test :::: " << " catcher_move_y = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_move_y = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;

	return Error_code::SUCCESS;
}
//机器手 移动z
Error_manager Dispatch_process::catcher_move_z(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_z)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 移动z
	tp_catcher_task->m_request_z = target_z;

	std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_z = " << tp_catcher_task->m_request_z << std::endl;

	return Error_code::SUCCESS;
}

//机器手调整到 准备从地面抓车前的姿态
Error_manager Dispatch_process::catcher_adjust_from_ground(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车.
	tp_catcher_task->m_request_x = m_car_measure_information.center_x;
	tp_catcher_task->m_request_y = m_car_measure_information.center_y;
	if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
	{
		//机器手如果旋转反向, 那就+180即可, 没有必要旋转归位.
		tp_catcher_task->m_request_b = 180 + m_car_measure_information.car_angle;
	}
	else
	{
		tp_catcher_task->m_request_b = m_car_measure_information.car_angle;
	}
	//限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升.
	if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit )
	{
		tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit;
	}
	else
	{
		tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = m_wheel_base;
	}
	tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;

	std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;
	std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;


	return Error_code::SUCCESS;
}

//机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
Error_manager Dispatch_process::catcher_adjust_wheel_base(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
	//修正轴距
	tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
	tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
	tp_catcher_task->m_request_wheelbase = m_wheel_base;

	std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;

	return Error_code::SUCCESS;
}

//机器手 移动c轴 夹杆
Error_manager Dispatch_process::catcher_move_c(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, Catcher_task::Clamp_motion target)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_catcher_task->m_request_clamp_motion = target;

	std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;

	return Error_code::SUCCESS;
}

//机器手调整到 准备把车放到搬运器的姿态
Error_manager Dispatch_process::catcher_adjust_to_carrier(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x, bool reverse_flag)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
	tp_catcher_task->m_request_x = target_x;
	tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
	//存车需要反向, 取车不需要
	if ( reverse_flag )
	{
		if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
		{
			tp_catcher_task->m_request_b = 90;
		}
		else
		{
			tp_catcher_task->m_request_b = 270;
		}
	}
	else
	{
		if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
		{
			tp_catcher_task->m_request_b = 270;
		}
		else
		{
			tp_catcher_task->m_request_b = 90;
		}
	}

	std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl;

	return Error_code::SUCCESS;
}

//机器手调整到 准备把车放到地面的姿态
Error_manager Dispatch_process::catcher_adjust_to_ground(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x, float target_y)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
	tp_catcher_task->m_request_x = target_x;
	tp_catcher_task->m_request_y = target_y;
	if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
	{
		tp_catcher_task->m_request_b = 270;
	}
	else
	{
		tp_catcher_task->m_request_b = 90;
	}

	std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl;

	return Error_code::SUCCESS;
}

//机器手调整到 对接搬运器的姿态
Error_manager Dispatch_process::catcher_adjust_from_carrier(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x)
{

	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	tp_catcher_task->m_request_key = dispatch_control_command_key;
	tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);

	//机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
	tp_catcher_task->m_request_x = target_x;
	tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);

	if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
	{
		tp_catcher_task->m_request_b = 270;
	}
	else
	{
		tp_catcher_task->m_request_b = 90;
	}

	//限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升.
	if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit )
	{
		tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit;
	}
	else
	{
		tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = m_wheel_base;
	}
	tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;

	std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;
	std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 准备开始, 需要同步任务单和设备真实数据.
Error_manager Dispatch_process::carrier_ready_to_start(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates)
{
	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	tp_carrier_task->m_request_key = dispatch_control_command_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_x = tp_carrier->m_actual_x;
	tp_carrier_task->m_request_y = tp_carrier->m_actual_y;
	tp_carrier_task->m_request_z = tp_carrier->m_actual_z;
	tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y1;
	tp_carrier_task->m_request_y2 = tp_carrier->m_actual_y2;
	tp_carrier_task->m_request_clamp_motion = (Carrier_task::Clamp_motion)tp_carrier->m_actual_clamp_motion1;
	tp_carrier_task->m_request_joint_motion_x = (Carrier_task::Joint_motion)tp_carrier->m_actual_joint_motion_x1;
	tp_carrier_task->m_request_joint_motion_y = Carrier_task::Joint_motion::E_JOINT_NO_ACTION;
	tp_carrier_task->m_request_space_id = 0;
	tp_carrier_task->m_request_floor_id = 0;
	tp_carrier_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;

	return Error_code::SUCCESS;
}
//搬运器调整到 正常待机的姿态(调节夹杆和轴距)
Error_manager Dispatch_process::carrier_adjust_to_ready(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	tp_carrier_task->m_request_key = dispatch_control_command_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_clamp_motion = Carrier_task::Clamp_motion::E_CLAMP_LOOSE;
	tp_carrier_task->m_request_wheelbase = m_wheel_base;
	tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
	tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;

	std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 移动x
Error_manager Dispatch_process::carrier_move_x(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	tp_carrier_task->m_request_key = dispatch_control_command_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_x = target_x;

	std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_x = " << tp_carrier_task->m_request_x << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 移动y
Error_manager Dispatch_process::carrier_move_y(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_y)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	tp_carrier_task->m_request_key = dispatch_control_command_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_y = target_y;
	tp_carrier_task->m_request_wheelbase = m_wheel_base;
	tp_carrier_task->m_request_y1 = tp_carrier_task->m_request_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
	tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;

	std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y = " << tp_carrier_task->m_request_y << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 移动z
Error_manager Dispatch_process::carrier_move_z(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_z)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	tp_carrier_task->m_request_key = dispatch_control_command_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_z = target_z;

	std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_z = " << tp_carrier_task->m_request_z << std::endl;

	return Error_code::SUCCESS;
}
//搬运器 移动c轴 夹车杆
Error_manager Dispatch_process::carrier_move_c(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Clamp_motion target)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	tp_carrier_task->m_request_key = dispatch_control_command_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_clamp_motion = target;

	std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl;

	return Error_code::SUCCESS;
}
//搬运器调整 水平的交接
Error_manager Dispatch_process::carrier_joint_x(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Joint_motion target)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	tp_carrier_task->m_request_key = dispatch_control_command_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_joint_motion_x = target;

	std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_joint_motion_x = " << tp_carrier_task->m_request_joint_motion_x << std::endl;

	return Error_code::SUCCESS;
}

//搬运器 修正轴距
Error_manager Dispatch_process::carrier_adjust_wheel_base(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates)
{

	std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
	tp_carrier_task->m_request_key = dispatch_control_command_key;
	tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);

	tp_carrier_task->m_request_wheelbase = m_wheel_base;
	tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
	tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;

	std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_request_key << std::endl;
	std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_respons_key << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
	std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;

	return Error_code::SUCCESS;
}