//
// 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_dispatch_process_catcher_motion = CATCHER_MOTION_UNKNOW;
	m_dispatch_process_carrier_motion = CARRIER_MOTION_UNKNOW;
}

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().parkspace_id() + PARKSPACE_ID_BASE;
		Common_data::copy_data(m_parkspace_information, dispatch_request_msg.parkspace_info());
		Common_data::copy_data(m_car_measure_information, dispatch_request_msg.locate_information());
	}
	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().parkspace_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());
	}
	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 ");
	}

	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()
{
	return Error_code::SUCCESS;
}

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));

		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:
			{
				//等待控制指令
				m_result = wait_dispatch_control_request_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					//不成功, 就表示没有新的指令, 那么什么都不做, 原地待命
				}
				else
				{
					//流程正常, 就去连接设备
					m_dispatch_process_status = DISPATCH_PROCESS_CONNECT_DEVICE;
					break;
				}

				//等待调度总计划答复
				m_result = wait_dispatch_plan_response_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					//不成功, 就表示没有总计划答复, 那么什么都不做, 原地待命
				}
				else
				{
					//流程正常, 就进入完成状态,
					m_dispatch_process_status = DISPATCH_PROCESS_OVER;
					break;
				}
				break;
			}
			case DISPATCH_PROCESS_CONNECT_DEVICE:
			{
				//连接调度设备
				m_result = connect_dispatch_device();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//流程正常, 就进入工作状态,
				m_dispatch_process_status = DISPATCH_PROCESS_WORKING;
				break;

				break;
			}
			case DISPATCH_PROCESS_WORKING:
			{

				break;
			}
			case DISPATCH_PROCESS_RESPONSE:
			{
				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:
			{
				break;
			}
			default:
			{

				break;
			}
		}
	}

	//任务超时

	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_mamager);
	m_dispatch_plan_request_msg.mutable_base_info()->set_receiver(message::Communicator::eDispatch_control);
	m_dispatch_plan_request_msg.set_command_key(m_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::wait_dispatch_control_request_msg()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	//key不相等 就表示 收到了新的控制指令
	if ( m_dispatch_control_request_msg.command_key() == m_dispatch_control_response_msg.command_key() )
	{
		return Error_code::NODATA;
	}
	else
	{
		return Error_code::SUCCESS;
	}
}



//连接调度设备
Error_manager Dispatch_process::connect_dispatch_device()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	Error_manager t_error;

	//机器手的配置 准备工作
	if ( m_dispatch_control_request_msg.dispatch_device_type() >= message::Dispatch_device_type::ROBOT_1 &&
		 m_dispatch_control_request_msg.dispatch_device_type() <= message::Dispatch_device_type::ROBOT_2 )
	{
		//找到对应的设备
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::ROBOT_1 )
		{
			mp_catcher = Dispatch_manager::get_instance_references().m_catcher_map[0];
		}
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::ROBOT_2 )
		{
			mp_catcher = Dispatch_manager::get_instance_references().m_catcher_map[1];
		}

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

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

			t_error = tp_catcher->execute_task(mp_catcher_task, Dispatch_device_base::E_ONE_LEVEL);
			if ( t_error != Error_code::SUCCESS )
			{
				return t_error;
			}
		}
		else
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
								 " Dispatch_process::excute_robot_catch_car_from_inlet device_status error ");
		}

		//设置起点
		m_source_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates[m_dispatch_control_request_msg.dispatch_source()];
		//设置终点
		m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates[m_dispatch_control_request_msg.dispatch_destination()];
	}
		//搬运器的配置 准备工作
	else if ( m_dispatch_control_request_msg.dispatch_device_type() >= message::Dispatch_device_type::CARRIER_1 &&
			  m_dispatch_control_request_msg.dispatch_device_type() <= message::Dispatch_device_type::CARRIER_3 )
	{
		//找到对应的设备
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::CARRIER_1 )
		{
			mp_carrier = Dispatch_manager::get_instance_references().m_carrier_map[0];
		}
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::CARRIER_2 )
		{
			mp_carrier = Dispatch_manager::get_instance_references().m_carrier_map[1];
		}
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::CARRIER_3 )
		{
			mp_carrier = Dispatch_manager::get_instance_references().m_carrier_map[1];
		}

		Carrier* tp_carrier = (Carrier*)mp_carrier.get();
		//检查设备状态
		if ( tp_carrier->check_status() == Error_code::SUCCESS &&
			 tp_carrier->m_actual_device_status == Dispatch_device_base::DEVICE_READY )
		{
			//创建任务单
			mp_carrier_task = std::shared_ptr<Task_Base>(new Carrier_task);
			mp_carrier_task->task_init(NULL,std::chrono::milliseconds(15000));
			Carrier_task * tp_carrier_task = (Carrier_task *)mp_carrier_task.get();

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

			t_error = tp_carrier->execute_task(mp_catcher_task, Dispatch_device_base::E_ONE_LEVEL);
			if ( t_error != Error_code::SUCCESS )
			{
				return t_error;
			}
		}
		else
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
								 " Dispatch_process::excute_robot_catch_car_from_inlet device_status error ");
		}

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

	}

	return Error_code::SUCCESS;
}

//检查机器手任务单
Error_manager Dispatch_process::check_catcher_task()
{
	if ( mp_catcher_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_catcher_task  POINTER IS NULL ");
	}
	else
	{
		Catcher_task * tp_catcher_task = (Catcher_task *)mp_catcher_task.get();

		if ( tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			return Error_code::SUCCESS;
		}
		else if ( tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			return tp_catcher_task->get_task_error_manager();
		}
		else if ( tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING &&
				  tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED &&
				  tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			return Error_code::NODATA;
		}
	}
}
//检查搬运器任务单
Error_manager Dispatch_process::check_carrier_task()
{
	if ( mp_carrier_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_carrier_task  POINTER IS NULL ");
	}
	else
	{
		Catcher_task * tp_carrier_task = (Catcher_task *)mp_carrier_task.get();

		if ( tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			return Error_code::SUCCESS;
		}
		else if ( tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			return tp_carrier_task->get_task_error_manager();
		}
		else if ( tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING &&
				  tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED &&
				  tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			return Error_code::NODATA;
		}
	}
}
//检查通道口任务单
Error_manager Dispatch_process::check_passageway_task()
{
	if ( mp_passageway_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_passageway_task  POINTER IS NULL ");
	}
	else
	{
		Catcher_task * tp_passageway_task = (Catcher_task *)mp_passageway_task.get();

		if ( tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			return Error_code::SUCCESS;
		}
		else if ( tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			return tp_passageway_task->get_task_error_manager();
		}
		else if ( tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING &&
				  tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED &&
				  tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			return Error_code::NODATA;
		}
	}
}

//检查 任务单 是否完成任务, 里面会调整短步骤
Error_manager Dispatch_process::check_task_ex(std::shared_ptr<Task_Base> p_catcher_task, int& step)
{
	if ( p_catcher_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_passageway_task  POINTER IS NULL ");
	}
	else
	{
		if ( p_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			step++;
			return Error_code::NODATA;	//这里返回nodata 表示任务继续下一步
		}
		else if ( p_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			step = 0;
			return p_catcher_task->get_task_error_manager();
		}
		else if ( p_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING &&
				  p_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED &&
				  p_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			//继续等待任务, 直到状态改变
			return Error_code::NODATA;
		}
	}
}

//执行调度控制指令, 并根据完成情况给答复
Error_manager Dispatch_process::excute_dispatch_control()
{
//设备的动作也使用外部的Main()的线程来循环
	return Error_code::SUCCESS;
}

//机器手去入口抓车(例如:机器手移动到2号入口上方,然后下降到一楼抓车,最后上升到最上方)
Error_manager Dispatch_process::excute_robot_catch_car_from_inlet()
{
	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 ( mp_catcher.get() == NULL || mp_catcher_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 *)mp_catcher.get();
		tp_catcher_task = (Catcher_task *)mp_catcher_task.get();
	}

	if ( tp_catcher_task->m_step == 0 )
	{
		return check_task_ex(mp_catcher_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	)
			{
				std::unique_lock<std::mutex> t_lock1(m_lock);
				std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
				char t_key[50] = {0};
				sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
				tp_catcher_task->m_respons_key = t_key;

				//调整姿态
				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;
				tp_catcher_task->m_request_b = 90;

				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,
		    					"tp_catcher->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
		}
	}
	if ( tp_catcher_task->m_step == 2 )
	{
		return check_task_ex(mp_catcher_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 3 )//机器手z轴移到最高点.
	{
		std::unique_lock<std::mutex> t_lock1(m_lock);
		std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
		char t_key[50] = {0};
		sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
		tp_catcher_task->m_respons_key = t_key;

		//机器手z轴移到最高点.
		tp_catcher_task->m_request_z = tp_dispatch_coordinates->m_catcher_4th_floor_z;

		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 4 )
	{
		return check_task_ex(mp_catcher_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 5 )//机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车.
	{
		std::unique_lock<std::mutex> t_lock1(m_lock);
		std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
		char t_key[50] = {0};
		sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
		tp_catcher_task->m_respons_key = t_key;

		//机器手 调整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_car_measure_information.car_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_car_measure_information.car_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
			tp_catcher_task->m_request_d2 = (m_car_measure_information.car_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
			tp_catcher_task->m_request_wheelbase = m_car_measure_information.car_wheel_base;
		}
		tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;

		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 6 )
	{
		return check_task_ex(mp_catcher_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 7 )//机器手z轴移到最低点.
	{
		std::unique_lock<std::mutex> t_lock1(m_lock);
		std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
		char t_key[50] = {0};
		sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
		tp_catcher_task->m_respons_key = t_key;

		//机器手z轴移到最低点.
		tp_catcher_task->m_request_z = tp_dispatch_coordinates->m_catcher_1th_floor_z;

		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 8 )
	{
		return check_task_ex(mp_catcher_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 9 )//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
	{
		std::unique_lock<std::mutex> t_lock1(m_lock);
		std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
		//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
		if ( m_car_measure_information.car_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit )
		{
			std::unique_lock<std::mutex> t_lock1(m_lock);
			std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
			char t_key[50] = {0};
			sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
			tp_catcher_task->m_respons_key = t_key;
			//修正轴距
			tp_catcher_task->m_request_d1 = (m_car_measure_information.car_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
			tp_catcher_task->m_request_d2 = (m_car_measure_information.car_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
			tp_catcher_task->m_request_wheelbase = m_car_measure_information.car_wheel_base;
			tp_catcher_task->m_step++;
		}
		else
		{
			tp_catcher_task->m_step = tp_catcher_task->m_step +2;
		}
	}
	if ( tp_catcher_task->m_step == 10 )
	{
		return check_task_ex(mp_catcher_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 11 )//机器手 夹车
	{
		std::unique_lock<std::mutex> t_lock1(m_lock);
		std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
		char t_key[50] = {0};
		sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
		tp_catcher_task->m_respons_key = t_key;

		//机器手 夹车
		tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_TIGHT;

		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 12 )
	{
		return check_task_ex(mp_catcher_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 13 )//机器手 z轴上升到最高处
	{
		std::unique_lock<std::mutex> t_lock1(m_lock);
		std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
		char t_key[50] = {0};
		sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
		tp_catcher_task->m_respons_key = t_key;

		//机器手 z轴上升到最高处
		tp_catcher_task->m_request_z = tp_dispatch_coordinates->m_catcher_4th_floor_z;

		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 14 )
	{
		return check_task_ex(mp_catcher_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 15 )
	{
		std::unique_lock<std::mutex> t_lock1(m_lock);
		std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
		char t_key[50] = {0};
		sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
		tp_catcher_task->m_respons_key = t_key;

		//机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
		tp_catcher_task->m_request_x = m_destination_coordinates.x;
		tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (tp_catcher_task->m_request_wheelbase /2);
		//存车需要反向, 取车不需要
		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;
		}
		tp_catcher_task->m_step++;
	}
	if ( tp_catcher_task->m_step == 16 )
	{
		return check_task_ex(mp_catcher_task, tp_catcher_task->m_step);
	}
	if ( tp_catcher_task->m_step == 17 )
	{
		return Error_code::SUCCESS;
	}
}
//机器手把车放到中跑车上面(例如:机器手下降到中跑车上放车,最后上升到最上方)(通过目标点 指定放到哪一个中跑车上)
Error_manager Dispatch_process::excute_robot_put_car_to_carrier()
{

}
//机器手去中跑车上抓车(例如:机器手移动到1号中跑车上方,然后下降到中跑车抓车,最后上升到最上方)
Error_manager Dispatch_process::excute_robot_catch_car_from_carrier()
{

}
//机器手去出口放车(例如:机器手移动到3号出口上方,然后下降到一楼放车,最后上升到最上方)
Error_manager Dispatch_process::excute_robot_put_car_to_outlet()
{

}
//机器手的自由移动(例如:机器手移动到6号出入口的上方4楼处,给其他设备避让)(不进行抓车和放车的操作)
Error_manager Dispatch_process::excute_robot_move()
{

}
//搬运器从机器手上接车(例如:搬运器移动到2号入口的上方,然后等待机器手把车放到中跑车上,小跑车保持松夹杆状态)
Error_manager Dispatch_process::excute_carrier_receive_car_from_robot()
{
	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 ( mp_carrier.get() == NULL || mp_carrier_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 *)mp_carrier.get();
		tp_carrier_task = (Carrier_task *)mp_carrier_task.get();
	}

	if ( tp_carrier_task->m_step == 0 )
	{
		return check_task_ex(mp_carrier_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 )
			{
				std::unique_lock<std::mutex> t_lock1(m_lock);
				std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
				char t_key[50] = {0};
				sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
				tp_carrier_task->m_respons_key = t_key;

				//调整姿态
				tp_carrier_task->m_request_clamp_motion = Carrier_task::Clamp_motion::E_CLAMP_LOOSE;

				tp_carrier_task->m_step++;
			}
			else
			{
				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(mp_catcher_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
	{
		if ( Common_data::approximate(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, 0.01) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			std::unique_lock<std::mutex> t_lock1(m_lock);
			std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
			char t_key[50] = {0};
			sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
			tp_carrier_task->m_respons_key = t_key;

			//让小跑车回到中跑车上
			tp_carrier_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y_back;
			tp_carrier_task->m_request_wheelbase = m_car_measure_information.car_wheel_base;
			tp_carrier_task->m_request_y1 = tp_dispatch_coordinates->m_carrier_default_y1_back;
			tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 4 )
	{
		return check_task_ex(mp_catcher_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
	{
		if ( Common_data::approximate(tp_carrier->m_actual_z, m_destination_coordinates.z, 0.01) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			std::unique_lock<std::mutex> t_lock1(m_lock);
			std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
			char t_key[50] = {0};
			sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
			tp_carrier_task->m_respons_key = t_key;

			if ( mp_carrier->get_device_id() == 0 )
			{
				//让中跑车回到电梯井
				tp_carrier_task->m_request_x = tp_dispatch_coordinates->m_carrier_default_x_left;
				tp_carrier_task->m_step++;
			}
			else if ( mp_carrier->get_device_id() == 1 )
			{
				//让中跑车回到电梯井
				tp_carrier_task->m_request_x = 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(mp_catcher_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate(tp_carrier->m_actual_z, m_destination_coordinates.z, 0.01) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			std::unique_lock<std::mutex> t_lock1(m_lock);
			std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
			char t_key[50] = {0};
			sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
			tp_carrier_task->m_respons_key = t_key;

			//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
			tp_carrier_task->m_request_joint_motion_x = Carrier_task::Joint_motion::E_JOINT_TAKE_BACK;
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 8 )
	{
		return check_task_ex(mp_catcher_task, tp_carrier_task->m_step);
	}
	if ( tp_carrier_task->m_step == 9 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
	{
		if ( Common_data::approximate(tp_carrier->m_actual_z, m_destination_coordinates.z, 0.01) )
		{
			tp_carrier_task->m_step +=2;
		}
		else
		{
			std::unique_lock<std::mutex> t_lock1(m_lock);
			std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
			char t_key[50] = {0};
			sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
			tp_carrier_task->m_respons_key = t_key;

			//电梯移动到对应的楼层
			tp_carrier_task->m_request_z = m_destination_coordinates.z;
			tp_carrier_task->m_step++;
		}
	}
	if ( tp_carrier_task->m_step == 10 )
	{
		return check_task_ex(mp_catcher_task, tp_carrier_task->m_step);
	}

}
//搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至电梯井)
Error_manager Dispatch_process::excute_carrier_store_car_to_parkingspace()
{

}
//搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至56车位外面即可)
Error_manager Dispatch_process::excute_carrier_store_car_to_parkingspace_ex()
{

}
//搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至电梯井)
Error_manager Dispatch_process::excute_carrier_pickup_car_from_parkingspace()
{

}
//搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至56车位外面即可)
Error_manager Dispatch_process::excute_carrier_pickup_car_from_parkingspace_ex()
{

}
//搬运器把车交付给机器手(例如:搬运器移动到3号入口的上方,小跑车松夹杆,然后等待机器手把车从中跑车上取走)
Error_manager Dispatch_process::excute_carrier_deliver_car_to_robot()
{

}
//搬运器的自由移动(可以提前到2楼来准备接车,或者为了避让就退回至电梯井)(小跑车不进行取车和存车)
Error_manager Dispatch_process::excute_carrier_move()
{

}

//执行通道口动作
Error_manager Dispatch_process::excute_passageway_motion()
{
	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_timeout_ms);
	m_dispatch_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_mamager);
	m_dispatch_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
	m_dispatch_response_msg.set_command_key(m_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()
{
	return Error_code::SUCCESS;
}




//机器手调整到正常待机的姿态
Error_manager Dispatch_process::cartcher_adjust_to_ready(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{
	std::unique_lock<std::mutex> t_lock1(m_lock);
	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_respons_key = t_key;

	//调整姿态
	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;
	tp_catcher_task->m_request_b = 90;
	return Error_code::SUCCESS;
}

//机器手 移动z
Error_manager Dispatch_process::cartcher_move_z(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
{
	std::unique_lock<std::mutex> t_lock1(m_lock);
	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_respons_key = t_key;
	//机器手 移动z
	tp_catcher_task->m_request_z = target;
	return Error_code::SUCCESS;
}

//机器手调整到 准备从地面抓车前的姿态
Error_manager Dispatch_process::cartcher_adjust_from_ground(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{
	std::unique_lock<std::mutex> t_lock1(m_lock);
	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_respons_key = t_key;

	//机器手 调整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_car_measure_information.car_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_car_measure_information.car_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_d2 = (m_car_measure_information.car_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
		tp_catcher_task->m_request_wheelbase = m_car_measure_information.car_wheel_base;
	}
	tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;

	return Error_code::SUCCESS;
}

//机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
Error_manager Dispatch_process::cartcher_adjust_wheel_base(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{
	std::unique_lock<std::mutex> t_lock1(m_lock);
	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);

	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_respons_key = t_key;
	//修正轴距
	tp_catcher_task->m_request_d1 = (m_car_measure_information.car_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
	tp_catcher_task->m_request_d2 = (m_car_measure_information.car_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
	tp_catcher_task->m_request_wheelbase = m_car_measure_information.car_wheel_base;
	return Error_code::SUCCESS;
}

//机器手 移动c轴 夹杆
Error_manager Dispatch_process::cartcher_move_c(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, Catcher_task::Clamp_motion target)
{
	std::unique_lock<std::mutex> t_lock1(m_lock);
	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_respons_key = t_key;

	//机器手 夹车
	tp_catcher_task->m_request_clamp_motion = target;
}

//机器手调整到 准备把车放到搬运器的姿态
Error_manager Dispatch_process::cartcher_adjust_to_carrier(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
{
	std::unique_lock<std::mutex> t_lock1(m_lock);
	std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
	char t_key[50] = {0};
	sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
	tp_catcher_task->m_respons_key = t_key;

	//机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
	tp_catcher_task->m_request_x = m_destination_coordinates.x;
	tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (tp_catcher_task->m_request_wheelbase /2);
	//存车需要反向, 取车不需要
	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;
	}
}