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 = 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)
{
	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.dispatch_motion_direction() == message::E_STORE_CAR )
	{
		m_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;
		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 )
	{
		m_dispatch_process_type = DISPATCH_PROCESS_PICKUP;
		m_dispatch_source = dispatch_request_msg.parkspace_info().parkingspace_index_id() + PARKSPACE_ID_BASE;
		m_dispatch_destination = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ;
		Common_data::copy_data(m_parkspace_information, dispatch_request_msg.parkspace_info());
		Common_data::scaling(m_parkspace_information, 1000);
		m_wheel_base = m_parkspace_information.car_information.car_wheel_base;
	}
	else
	{
		m_dispatch_process_type = DISPATCH_PROCESS_TYPE_UNKNOW;
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							 "  Dispatch_process::Dispatch_process_init ERROR ");
	}

	if ( m_wheel_base < 1000 )
	{
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							 "  m_wheel_base < 1000 ERROR ");
	}
	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 ");
	}
}

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 ( 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://流程准备，待机
			{
				//调度控制, 并根据完成情况给答复
				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_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::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_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);
				//流程正常, 就回到 等待状态, 等待调度控制发送动作指令
				iter->second.m_error.error_manager_clear_all();
				iter->second.m_dispatch_control_status = DISPATCH_CONTROL_READY;
				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
		{
			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[dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()];
		//设置终点
		dispatch_control_node.m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates[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
		{
			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[dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()];
		//设置终点
		dispatch_control_node.m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates[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::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 )
	{
		dispatch_control_node.mp_dispatch_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW);
		dispatch_control_node.mp_dispatch_task.reset();
		dispatch_control_node.mp_dispatch_device.reset();
	}
	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 >= dispatch_control_node.m_destination_coordinates.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 >= dispatch_control_node.m_destination_coordinates.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 >= dispatch_control_node.m_destination_coordinates.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_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 )
		{
			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;
		}
		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 )
		{
			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;
		}
		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 )
		{
			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 )
		{
			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轴移动
	{
		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 == 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 )
		{
			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 )
		{
			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 )
		{
			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_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;
}