puai_dispatch/dispatch/dispatch_process.h

/*
        dispatch_source 和 dispatch_destination 的表示含义

        1~165           2楼~12楼的停车位

        1100            0号出口(在一楼)(目前没有)
        1101~1106       1~6号出入口 (在一楼)
        1107            7号出口(在一楼)
        1201~1206       1~6号出入口上方2楼处
        1301~1306       1~6号出入口上方3楼处
        1401~1406       1~6号出入口上方4楼处

        2101~2112       左侧电梯井(1楼~12楼)(一号搬运器的待机点)
        2201~2212       右侧电梯井(1楼~12楼)(二号搬运器的待机点)
*/




#ifndef NNXX_TESTS_DISPATCH_PROCESS_H
#define NNXX_TESTS_DISPATCH_PROCESS_H

#include <error_code.h>
#include "../tool/thread_condition.h"
#include "../tool/TaskQueue/BaseTask.h"
#include "../tool/common_data.h"
#include <glog/logging.h>
#include "../dispatch/carrier.h"
#include "../dispatch/catcher.h"
#include "../dispatch/passageway.h"
#include "../dispatch/dispatch_coordinates.h"

#include "../message/dispatch_message.pb.h"
#include "../message/dispatch_control.pb.h"
#include "../tool/point3D_tool.h"
#include <atomic>



//调度流程 存车
class Dispatch_process : public tq::BaseTask
{
public:
	//调度指令超时时间 300000ms = 5min
#define DISPATCH_PROCESS_TIMEOUT_MS							600000
//调度指令超时时间  衰减值 在原有的基础上减少10秒
#define DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS				10000

//调度默认 差值 2mm (比较结果, 对精度要求较高)
#define DISPATCH_DEFAULT_DIFFERENCE							2
//调度默认 比值 1% (比较结果, 对精度要求较高)
#define DISPATCH_DEFAULT_RATE								0.01

	//调度流程的状态, 控制步骤(对于整个大的调度任务)
	enum Dispatch_process_status
	{
		DISPATCH_PROCESS_STATUS_UNKNOW          = 0,    //未知
		DISPATCH_PROCESS_CREATED				= 1, 	//流程创建,
		DISPATCH_PROCESS_READY               	= 2,    //流程准备，待机

		DISPATCH_PROCESS_WORKING_PICKUP					= 103,	//流程取车
		DISPATCH_PROCESS_WORKING_STORE					= 104,	//流程存车


		DISPATCH_PROCESS_CONNECT_DEVICE			= 3, 	//流程 连接设备
		DISPATCH_PROCESS_WORKING				= 4, 	//流程工作正忙, 进行长流程
		DISPATCH_PROCESS_RESPONSE				= 5, 	//流程 给调度控制答复
		DISPATCH_PROCESS_DISCONNECT_DEVICE		= 6, 	//流程 解除设备


		DISPATCH_PROCESS_OVER					= 7, 	//流程完成
		DISPATCH_PROCESS_RELEASE				= 8, 	//流程释放

		DISPATCH_PROCESS_FAULT					= 10,	//故障
		DISPATCH_PROCESS_MOTION_FAILED			= 11,	//单个小动作执行失败
	};




	//调度控制的状态, 控制步骤(对于一个设备的长流程的控制)
	enum Dispatch_control_status
	{
//		DISPATCH_CONTROL_STATUS_UNKNOW          = 0,    //未知
//		DISPATCH_CONTROL_CREATED				= 1, 	//流程创建,
//		DISPATCH_CONTROL_READY               	= 2,    //流程准备，待机
//		DISPATCH_CONTROL_CONNECT_DEVICE			= 3, 	//流程 连接设备
		DISPATCH_CONTROL_WORKING				= 4, 	//流程工作正忙, 进行长流程
		DISPATCH_CONTROL_TASK_WITHDRAW			= 5,	//流程 收回任务单
		DISPATCH_CONTROL_RESPONSE				= 6, 	//流程 给调度控制答复
//		DISPATCH_CONTROL_DISCONNECT_DEVICE		= 7, 	//流程 解除设备
//		DISPATCH_CONTROL_OVER					= 8, 	//流程完成
//		DISPATCH_CONTROL_RELEASE				= 9, 	//流程释放
//
//		DISPATCH_CONTROL_FAULT					= 10,	//故障
//		DISPATCH_CONTROL_MOTION_FAILED			= 11,	//单个小动作执行失败

		DISPATCH_CONTROL_STATUS_UNKNOW          = 0,    //未知
		DISPATCH_CONTROL_CREATED				= 1, 	//流程创建,
		DISPATCH_CONTROL_READY               	= 2,    //流程准备，待机
		DISPATCH_CONTROL_CONNECT_DEVICE			= 3, 	//流程 连接设备

		DISPATCH_CARRIER_PICKUP_START			= 200, 	//取车流程 开始
		DISPATCH_CARRIER_PICKUP_1				= 201,
		DISPATCH_CARRIER_PICKUP_2				= 202,
		DISPATCH_CARRIER_PICKUP_3				= 203,
		DISPATCH_CARRIER_PICKUP_4				= 204,
		DISPATCH_CARRIER_PICKUP_5				= 205,
		DISPATCH_CARRIER_PICKUP_6				= 206,
		DISPATCH_CARRIER_PICKUP_7				= 207,
		DISPATCH_CARRIER_PICKUP_8				= 208,
		DISPATCH_CARRIER_PICKUP_9				= 209,
		DISPATCH_CARRIER_PICKUP_10				= 210,
		DISPATCH_CARRIER_PICKUP_11				= 211,
		DISPATCH_CARRIER_PICKUP_12				= 212,
		DISPATCH_CARRIER_PICKUP_13				= 213,
		DISPATCH_CARRIER_PICKUP_14				= 214,
		DISPATCH_CARRIER_PICKUP_15				= 215,
		DISPATCH_CARRIER_PICKUP_16				= 216,
		DISPATCH_CARRIER_PICKUP_17				= 217,
		DISPATCH_CARRIER_PICKUP_18				= 218,
		DISPATCH_CARRIER_PICKUP_19				= 219,
		DISPATCH_CARRIER_PICKUP_20				= 220,
		DISPATCH_CARRIER_PICKUP_21				= 221,
		DISPATCH_CARRIER_PICKUP_22				= 222,
		DISPATCH_CARRIER_PICKUP_23				= 223,
		DISPATCH_CARRIER_PICKUP_24				= 224,
		DISPATCH_CARRIER_PICKUP_25				= 225,
		DISPATCH_CARRIER_PICKUP_26				= 226,
		DISPATCH_CARRIER_PICKUP_27				= 227,
		DISPATCH_CARRIER_PICKUP_28				= 228,
		DISPATCH_CARRIER_PICKUP_29				= 229,
		DISPATCH_CARRIER_PICKUP_30				= 230,
		DISPATCH_CARRIER_PICKUP_31				= 231,
		DISPATCH_CARRIER_PICKUP_32				= 232,
		DISPATCH_CARRIER_PICKUP_33				= 233,
		DISPATCH_CARRIER_PICKUP_34				= 234,
		DISPATCH_CARRIER_PICKUP_35				= 235,
		DISPATCH_CARRIER_PICKUP_36				= 236,
		DISPATCH_CARRIER_PICKUP_37				= 237,
		DISPATCH_CARRIER_PICKUP_38				= 238,
		DISPATCH_CARRIER_PICKUP_39				= 239,
		DISPATCH_CARRIER_PICKUP_40				= 240,
		DISPATCH_CARRIER_PICKUP_41				= 241,
		DISPATCH_CARRIER_PICKUP_42				= 242,
		DISPATCH_CARRIER_PICKUP_43				= 243,
		DISPATCH_CARRIER_PICKUP_44				= 244,
		DISPATCH_CARRIER_PICKUP_45				= 245,
		DISPATCH_CARRIER_PICKUP_46				= 246,
		DISPATCH_CARRIER_PICKUP_47				= 247,
		DISPATCH_CARRIER_PICKUP_48				= 248,
		DISPATCH_CARRIER_PICKUP_49				= 249,
		DISPATCH_CARRIER_PICKUP_50				= 250,
		DISPATCH_CARRIER_PICKUP_51				= 251,
		DISPATCH_CARRIER_PICKUP_52				= 252,
		DISPATCH_CARRIER_PICKUP_53				= 253,
		DISPATCH_CARRIER_PICKUP_54				= 254,
		DISPATCH_CARRIER_PICKUP_55				= 255,
		DISPATCH_CARRIER_PICKUP_56				= 256,
		DISPATCH_CARRIER_PICKUP_57				= 257,
		DISPATCH_CARRIER_PICKUP_58				= 258,
		DISPATCH_CARRIER_PICKUP_59				= 259,
		DISPATCH_CARRIER_PICKUP_60				= 260,
		DISPATCH_CARRIER_PICKUP_61				= 261,

		DISPATCH_CARRIER_PICKUP_70				= 270,
		DISPATCH_CARRIER_PICKUP_71				= 271,
		DISPATCH_CARRIER_PICKUP_72				= 272,
		DISPATCH_CARRIER_PICKUP_73				= 273,
		DISPATCH_CARRIER_PICKUP_74				= 274,
		DISPATCH_CARRIER_PICKUP_75				= 275,
		DISPATCH_CARRIER_PICKUP_76				= 276,
		DISPATCH_CARRIER_PICKUP_77				= 277,
		DISPATCH_CARRIER_PICKUP_78				= 278,
		DISPATCH_CARRIER_PICKUP_79				= 279,

		DISPATCH_CARRIER_PICKUP_80				= 280,
		DISPATCH_CARRIER_PICKUP_81				= 281,
		DISPATCH_CARRIER_PICKUP_82				= 282,
		DISPATCH_CARRIER_PICKUP_83				= 283,
		DISPATCH_CARRIER_PICKUP_84				= 284,
		DISPATCH_CARRIER_PICKUP_85				= 285,
		DISPATCH_CARRIER_PICKUP_86				= 286,
		DISPATCH_CARRIER_PICKUP_87				= 287,
		DISPATCH_CARRIER_PICKUP_88				= 288,
		DISPATCH_CARRIER_PICKUP_89				= 289,
		DISPATCH_CARRIER_PICKUP_END				= 299,


		DISPATCH_CATCHER_PICKUP_START			= 300, 	//取车流程 开始
		DISPATCH_CATCHER_PICKUP_1				= 301,
		DISPATCH_CATCHER_PICKUP_2				= 302,
		DISPATCH_CATCHER_PICKUP_3				= 303,
		DISPATCH_CATCHER_PICKUP_4				= 304,
		DISPATCH_CATCHER_PICKUP_5				= 305,
		DISPATCH_CATCHER_PICKUP_6				= 306,
		DISPATCH_CATCHER_PICKUP_7				= 307,
		DISPATCH_CATCHER_PICKUP_8				= 308,
		DISPATCH_CATCHER_PICKUP_9				= 309,
		DISPATCH_CATCHER_PICKUP_10				= 310,
		DISPATCH_CATCHER_PICKUP_11				= 311,
		DISPATCH_CATCHER_PICKUP_12				= 312,
		DISPATCH_CATCHER_PICKUP_13				= 313,
		DISPATCH_CATCHER_PICKUP_14				= 314,
		DISPATCH_CATCHER_PICKUP_15				= 315,
		DISPATCH_CATCHER_PICKUP_16				= 316,
		DISPATCH_CATCHER_PICKUP_17				= 317,
		DISPATCH_CATCHER_PICKUP_18				= 318,
		DISPATCH_CATCHER_PICKUP_19				= 319,
		DISPATCH_CATCHER_PICKUP_20				= 320,
		DISPATCH_CATCHER_PICKUP_21				= 321,
		DISPATCH_CATCHER_PICKUP_22				= 322,
		DISPATCH_CATCHER_PICKUP_23				= 323,
		DISPATCH_CATCHER_PICKUP_24				= 324,
		DISPATCH_CATCHER_PICKUP_25				= 325,
		DISPATCH_CATCHER_PICKUP_26				= 326,
		DISPATCH_CATCHER_PICKUP_27				= 327,
		DISPATCH_CATCHER_PICKUP_28				= 328,
		DISPATCH_CATCHER_PICKUP_29				= 329,
		DISPATCH_CATCHER_PICKUP_30				= 330,
		DISPATCH_CATCHER_PICKUP_31				= 331,
		DISPATCH_CATCHER_PICKUP_32				= 332,
		DISPATCH_CATCHER_PICKUP_33				= 333,
		DISPATCH_CATCHER_PICKUP_34				= 334,
		DISPATCH_CATCHER_PICKUP_35				= 335,
		DISPATCH_CATCHER_PICKUP_36				= 336,
		DISPATCH_CATCHER_PICKUP_37				= 337,
		DISPATCH_CATCHER_PICKUP_38				= 338,
		DISPATCH_CATCHER_PICKUP_39				= 339,
		DISPATCH_CATCHER_PICKUP_40				= 340,
		DISPATCH_CATCHER_PICKUP_END				= 399,



		DISPATCH_CARRIER_STORE_START			= 400, 	//取车流程 开始
		DISPATCH_CARRIER_STORE_1				= 401,
		DISPATCH_CARRIER_STORE_2				= 402,
		DISPATCH_CARRIER_STORE_3				= 403,
		DISPATCH_CARRIER_STORE_4				= 404,
		DISPATCH_CARRIER_STORE_5				= 405,
		DISPATCH_CARRIER_STORE_6				= 406,
		DISPATCH_CARRIER_STORE_7				= 407,
		DISPATCH_CARRIER_STORE_8				= 408,
		DISPATCH_CARRIER_STORE_9				= 409,
		DISPATCH_CARRIER_STORE_10				= 410,
		DISPATCH_CARRIER_STORE_11				= 411,
		DISPATCH_CARRIER_STORE_12				= 412,
		DISPATCH_CARRIER_STORE_13				= 413,
		DISPATCH_CARRIER_STORE_14				= 414,
		DISPATCH_CARRIER_STORE_15				= 415,
		DISPATCH_CARRIER_STORE_16				= 416,
		DISPATCH_CARRIER_STORE_17				= 417,
		DISPATCH_CARRIER_STORE_18				= 418,
		DISPATCH_CARRIER_STORE_19				= 419,
		DISPATCH_CARRIER_STORE_20				= 420,
		DISPATCH_CARRIER_STORE_21				= 421,
		DISPATCH_CARRIER_STORE_22				= 422,
		DISPATCH_CARRIER_STORE_23				= 423,
		DISPATCH_CARRIER_STORE_24				= 424,
		DISPATCH_CARRIER_STORE_25				= 425,
		DISPATCH_CARRIER_STORE_26				= 426,
		DISPATCH_CARRIER_STORE_27				= 427,
		DISPATCH_CARRIER_STORE_28				= 428,
		DISPATCH_CARRIER_STORE_29				= 429,
		DISPATCH_CARRIER_STORE_30				= 430,
		DISPATCH_CARRIER_STORE_31				= 431,
		DISPATCH_CARRIER_STORE_32				= 432,
		DISPATCH_CARRIER_STORE_33				= 433,
		DISPATCH_CARRIER_STORE_34				= 434,
		DISPATCH_CARRIER_STORE_35				= 435,
		DISPATCH_CARRIER_STORE_36				= 436,
		DISPATCH_CARRIER_STORE_37				= 437,
		DISPATCH_CARRIER_STORE_38				= 438,
		DISPATCH_CARRIER_STORE_39				= 439,
		DISPATCH_CARRIER_STORE_40				= 440,
		DISPATCH_CARRIER_STORE_41				= 441,
		DISPATCH_CARRIER_STORE_42				= 442,
		DISPATCH_CARRIER_STORE_43				= 443,
		DISPATCH_CARRIER_STORE_44				= 444,
		DISPATCH_CARRIER_STORE_45				= 445,
		DISPATCH_CARRIER_STORE_46				= 446,
		DISPATCH_CARRIER_STORE_47				= 447,
		DISPATCH_CARRIER_STORE_48				= 448,
		DISPATCH_CARRIER_STORE_49				= 449,
		DISPATCH_CARRIER_STORE_50				= 450,
		DISPATCH_CARRIER_STORE_51				= 451,
		DISPATCH_CARRIER_STORE_52				= 452,
		DISPATCH_CARRIER_STORE_53				= 453,
		DISPATCH_CARRIER_STORE_54				= 454,
		DISPATCH_CARRIER_STORE_55				= 455,
		DISPATCH_CARRIER_STORE_56				= 456,
		DISPATCH_CARRIER_STORE_57				= 457,
		DISPATCH_CARRIER_STORE_58				= 458,
		DISPATCH_CARRIER_STORE_59				= 459,
		DISPATCH_CARRIER_STORE_60				= 460,
		DISPATCH_CARRIER_STORE_61				= 461,
		DISPATCH_CARRIER_STORE_END				= 499,

		DISPATCH_CATCHER_STORE_START			= 500, 	//取车流程 开始
		DISPATCH_CATCHER_STORE_1				= 501,
		DISPATCH_CATCHER_STORE_2				= 502,
		DISPATCH_CATCHER_STORE_3				= 503,
		DISPATCH_CATCHER_STORE_4				= 504,
		DISPATCH_CATCHER_STORE_5				= 505,
		DISPATCH_CATCHER_STORE_6				= 506,
		DISPATCH_CATCHER_STORE_7				= 507,
		DISPATCH_CATCHER_STORE_8				= 508,
		DISPATCH_CATCHER_STORE_9				= 509,
		DISPATCH_CATCHER_STORE_10				= 510,
		DISPATCH_CATCHER_STORE_11				= 511,
		DISPATCH_CATCHER_STORE_12				= 512,
		DISPATCH_CATCHER_STORE_13				= 513,
		DISPATCH_CATCHER_STORE_14				= 514,
		DISPATCH_CATCHER_STORE_15				= 515,
		DISPATCH_CATCHER_STORE_16				= 516,
		DISPATCH_CATCHER_STORE_17				= 517,
		DISPATCH_CATCHER_STORE_18				= 518,
		DISPATCH_CATCHER_STORE_19				= 519,
		DISPATCH_CATCHER_STORE_20				= 520,
		DISPATCH_CATCHER_STORE_21				= 521,
		DISPATCH_CATCHER_STORE_22				= 522,
		DISPATCH_CATCHER_STORE_23				= 523,
		DISPATCH_CATCHER_STORE_24				= 524,
		DISPATCH_CATCHER_STORE_25				= 525,
		DISPATCH_CATCHER_STORE_26				= 526,
		DISPATCH_CATCHER_STORE_27				= 527,
		DISPATCH_CATCHER_STORE_28				= 528,
		DISPATCH_CATCHER_STORE_29				= 529,
		DISPATCH_CATCHER_STORE_30				= 530,
		DISPATCH_CATCHER_STORE_31				= 531,
		DISPATCH_CATCHER_STORE_32				= 532,
		DISPATCH_CATCHER_STORE_33				= 533,
		DISPATCH_CATCHER_STORE_34				= 534,
		DISPATCH_CATCHER_STORE_35				= 535,
		DISPATCH_CATCHER_STORE_36				= 536,
		DISPATCH_CATCHER_STORE_37				= 537,
		DISPATCH_CATCHER_STORE_38				= 538,
		DISPATCH_CATCHER_STORE_39				= 539,
		DISPATCH_CATCHER_STORE_40				= 540,
		DISPATCH_CATCHER_STORE_END				= 599,

		DISPATCH_CONTROL_DISCONNECT_DEVICE		= 7, 	//流程 解除设备
		DISPATCH_CONTROL_OVER					= 8, 	//流程完成
		DISPATCH_CONTROL_RELEASE				= 9, 	//流程释放

		DISPATCH_CONTROL_FAULT					= 10,	//故障
		DISPATCH_CONTROL_MOTION_FAILED			= 11,	//单个小动作执行失败
	};
	//调度控制节点, 对应一个设备
	struct Dispatch_control_node
	{
		Dispatch_control_status					m_dispatch_control_status;			//调度控制的状态, 控制步骤(对于一个设备的长流程的控制)
		std::shared_ptr<Dispatch_device_base>	mp_dispatch_device;					//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Task_Base>				mp_dispatch_task;					//调度任务指针, 内存由本模块管理

		//注释
		message::Dispatch_control_request_msg	m_dispatch_control_request_msg;		//3调度控制的任务请求(调度算法->调度管理) 注释
		message::Dispatch_control_response_msg	m_dispatch_control_response_msg;	//4调度控制的任务答复(调度管理->调度算法) 注释
		Point3D_tool::Point3D					m_source_coordinates;				//起点 的坐标 (Dispatch_control 长流程的起点) 注释
		Point3D_tool::Point3D					m_destination_coordinates;			//终点 的坐标 (Dispatch_control 长流程的终点) 注释

		Error_manager							m_error;							//错误码
		std::chrono::system_clock::time_point	m_time_to_send_control_response;		//发送 控制的任务答复 的时间 注释
	};


	//调度设备机器人节点
	struct Dispatch_catcher_node
	{
		Dispatch_control_status						m_dispatch_control_status;			//调度控制的状态, 控制步骤
		bool										m_dispatch_control_start_flag =false;		//调度设备机器人启动标志位
		// 核心机器人, 2号
		std::shared_ptr<Dispatch_device_base>		mp_main_catcher;						//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Task_Base>					mp_main_catcher_task;					//调度任务指针, 内存由本模块管理
		Dispatch_device_base::Dispatch_task_level 	m_main_catcher_task_level;

		//跟随搬运器, 3号搬运器
		bool										m_following_flag;						//3号搬运器是否跟随机器人, 如果车位在3楼, 则为false
		std::shared_ptr<Dispatch_device_base>		mp_following_carrier;					//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Task_Base>					mp_following_carrier_task;				//调度任务指针, 内存由本模块管理
		//目前没有让别人避让的功能

		Error_manager								m_error;							//错误码
	};

	//调度设备搬运器节点
	struct Dispatch_carrier_node
	{
		Dispatch_control_status						m_dispatch_control_status;			//调度控制的状态, 控制步骤
		bool										m_dispatch_control_start_flag =false;		//调度设备机器人启动标志位
		//核心搬运器, 搬运汽车的搬运器
		std::shared_ptr<Dispatch_device_base>		mp_main_carrier;					//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Task_Base>					mp_main_carrier_task;				//调度任务指针, 内存由本模块管理
		Dispatch_device_base::Dispatch_task_level 	m_main_carrier_task_level;

		//给机器人发送避让指令, 2号机器人(可选)
		std::shared_ptr<Dispatch_device_base>		mp_avoid_catcher;						//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Task_Base>					mp_avoid_catcher_task;					//调度任务指针, 内存由本模块管理
		//给3楼搬运器发送避让指令,
		std::shared_ptr<Dispatch_device_base>		mp_avoid_carrier;					//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Task_Base>					mp_avoid_carrier_task;				//调度任务指针, 内存由本模块管理

		Error_manager								m_error;							//错误码
	};



public:
	Dispatch_process();
	Dispatch_process(const Dispatch_process& other)= default;
	Dispatch_process& operator =(const Dispatch_process& other)= default;
	~Dispatch_process();
public://API functions
	//初始化, 就把主控发送的请求传入即可.
	Error_manager Dispatch_process_init(message::Dispatch_request_msg &dispatch_request_msg);
	//反初始化
	Error_manager Dispatch_process_uninit();

	//检查流程是否空闲待机
	Error_manager check_process_ready();
	//判断是否为待机，
	virtual bool is_ready();
	//能否在左侧路径存车
	bool is_able_store_left_path();
	//能否在右侧路径存车
	bool is_able_store_right_path();
	//能否在临时缓存位存车
	bool is_able_store_temporary_cache_path();
	//能否在临时缓存位存车
	bool is_able_store_temporary_cache_path_2nd_floor();
	//能否在临时缓存位存车
	bool is_able_store_temporary_cache_path_3rd_floor();
public://get or set member variable
	void set_main_carrier(std::shared_ptr<Dispatch_device_base> p_carrier);
	std::string get_dispatch_control_command_key(Dispatch_catcher_node & dispatch_catcher_node);
	std::string get_dispatch_control_command_key(Dispatch_carrier_node & dispatch_carrier_node);
protected://主流程

	//主线程
	virtual void Main();

	//检查调度请求
	Error_manager check_dispatch_request_msg();
	//等待资源分配
	Error_manager wait_resource_allocation();
	//调度控制取车
	Error_manager dispatch_control_motion_pickup();
	//调度控制存车
	Error_manager dispatch_control_motion_store();

	//检查核心搬运器
	Error_manager check_main_carrier(Dispatch_carrier_node & dispatch_carrier_node);
	//检查核心抓取器
	Error_manager check_main_catcher(Dispatch_catcher_node & dispatch_catcher_node);
	//连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理
	Error_manager connect_dispatch_carrier(std::shared_ptr<Dispatch_device_base> & p_carrier, std::shared_ptr<Task_Base> & p_carrier_task, Dispatch_device_base::Dispatch_task_level dispatch_task_level);
	//连接抓取器, 创建新的任务单 与设备建立连接, 可以失败, 失败就重新执行
	Error_manager connect_dispatch_catcher(std::shared_ptr<Dispatch_device_base> & p_catcher, std::shared_ptr<Task_Base> & p_catcher_task, Dispatch_device_base::Dispatch_task_level dispatch_task_level);
	//断开调度设备, 收回任务单, 释放任务单 与设备解除连接
	Error_manager disconnect_dispatch_device(std::shared_ptr<Dispatch_device_base> & p_device, std::shared_ptr<Task_Base> & p_device_task);



	//发送调度总计划
	Error_manager send_dispatch_plan_request_msg();
	//调度控制, 并根据完成情况给答复
	Error_manager dispatch_control_motion();

	//等待控制指令
	Error_manager wait_dispatch_control_request_msg(int dispatch_device_type, Dispatch_control_node & dispatch_control_node);
	//连接调度设备, 创建新的任务单 与设备建立连接
	Error_manager connect_dispatch_device(int dispatch_device_type, Dispatch_control_node & dispatch_control_node);
	//执行调度控制指令, 并根据完成情况给答复
	Error_manager excute_dispatch_control(int dispatch_device_type, Dispatch_control_node & dispatch_control_node);
	//流程通知设备 收回任务单
	Error_manager dispatch_control_withdraw_task(int dispatch_device_type, Dispatch_control_node & dispatch_control_node);
	//发送调度控制答复, 发给调度控制的
	Error_manager send_dispatch_control_response_msg(int dispatch_device_type, Dispatch_control_node & dispatch_control_node, message::Dispatch_device_task_status dispatch_device_task_status);
	//断开调度设备, 释放任务单 与设备解除连接
	Error_manager disconnect_dispatch_device(int dispatch_device_type, Dispatch_control_node & dispatch_control_node);

	//等待调度总计划答复
	Error_manager wait_dispatch_plan_response_msg();
	//发送调度答复, 发给主控的
	Error_manager send_dispatch_response_msg();

	//通知调度管理, 释放资源,
	Error_manager release_resource();
	//异常处理
	Error_manager Exception_handling();

protected://长流程
	//机器手去入口抓车(例如:机器手移动到2号入口上方,然后下降到一楼抓车,最后上升到最上方)
	Error_manager excute_robot_catch_car_from_inlet(Dispatch_control_node & dispatch_control_node);
	//机器手把车放到中跑车上面(例如:机器手下降到中跑车上放车,最后上升到最上方)(通过目标点 指定放到哪一个中跑车上)
	Error_manager excute_robot_put_car_to_carrier(Dispatch_control_node & dispatch_control_node);
	//机器手去中跑车上抓车(例如:机器手移动到1号中跑车上方,然后下降到中跑车抓车,最后上升到最上方)
	Error_manager excute_robot_catch_car_from_carrier(Dispatch_control_node & dispatch_control_node);
	//机器手去出口放车(例如:机器手移动到3号出口上方,然后下降到一楼放车,最后上升到最上方)
	Error_manager excute_robot_put_car_to_outlet(Dispatch_control_node & dispatch_control_node);
	//机器手的自由移动(例如:机器手移动到6号出入口的上方4楼处,给其他设备避让)(不进行抓车和放车的操作)
	Error_manager excute_robot_move(Dispatch_control_node & dispatch_control_node);

	//搬运器从机器手上接车(例如:搬运器移动到2号入口的上方,然后等待机器手把车放到中跑车上,小跑车保持松夹杆状态)
	Error_manager excute_carrier_receive_car_from_robot(Dispatch_control_node & dispatch_control_node);
	//搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至电梯井)
	Error_manager excute_carrier_store_car_to_parkingspace(Dispatch_control_node & dispatch_control_node);
	//搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至56车位外面即可)
	Error_manager excute_carrier_store_car_to_parkingspace_ex(Dispatch_control_node & dispatch_control_node);
	//搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至电梯井)
	Error_manager excute_carrier_pickup_car_from_parkingspace(Dispatch_control_node & dispatch_control_node);
	//搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至56车位外面即可)
	Error_manager excute_carrier_pickup_car_from_parkingspace_ex(Dispatch_control_node & dispatch_control_node);
	//搬运器把车交付给机器手(例如:搬运器移动到3号入口的上方,小跑车松夹杆,然后等待机器手把车从中跑车上取走)
	Error_manager excute_carrier_deliver_car_to_robot(Dispatch_control_node & dispatch_control_node);
	//搬运器的自由移动(可以提前到2楼来准备接车,或者为了避让就退回至电梯井)(小跑车不进行取车和存车)
	Error_manager excute_carrier_move(Dispatch_control_node & dispatch_control_node);

	//执行通道口动作
	Error_manager excute_passageway_motion(Dispatch_control_node & dispatch_control_node);

protected://短流程
	//检查 任务单 是否完成任务, 里面不会调整短步骤
	Error_manager check_task_status(std::shared_ptr<Task_Base> p_task);
	//检查 任务单 是否完成任务, 里面会调整短步骤
	Error_manager check_task_status(std::shared_ptr<Task_Base> p_task, Dispatch_control_status & dispatch_control_status);
	//检查 任务单 是否完成任务, 里面会调整短步骤
	Error_manager check_task_ex(std::shared_ptr<Task_Base> p_task, int& step);

	//机器手调整到 正常待机的姿态(调节夹杆和轴距)
	Error_manager catcher_adjust_to_ready(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//机器手 移动z
	Error_manager catcher_move_z(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target);
	//机器手调整到 准备从地面抓车前的姿态
	Error_manager catcher_adjust_from_ground(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
	Error_manager catcher_adjust_wheel_base(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//机器手 移动c轴 夹杆
	Error_manager 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);
	//机器手调整到 对接搬运器的姿态
	Error_manager 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);
	//机器手调整到 准备把车放到地面的姿态
	Error_manager catcher_adjust_to_ground(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//机器手调整到 对接搬运器的姿态
	Error_manager catcher_adjust_from_carrier(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);

	//搬运器调整到 正常待机的姿态(调节夹杆和轴距)
	Error_manager carrier_adjust_to_ready(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//搬运器 移动x
	Error_manager carrier_move_x(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target);
	//搬运器 移动y
	Error_manager carrier_move_y(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target);
	//搬运器 移动z
	Error_manager carrier_move_z(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target);
	//搬运器 移动c轴 夹车杆
	Error_manager 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);
	//搬运器调整 水平的交接
	Error_manager 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);
	//搬运器 修正轴距
	Error_manager carrier_adjust_wheel_base(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates);

	//机器手 准备开始, 需要同步任务单和设备真实数据.
	Error_manager catcher_ready_to_start(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//机器手调整到 正常待机的姿态(调节夹杆和轴距)
	Error_manager catcher_adjust_to_ready(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//机器手 移动x
	Error_manager catcher_move_x(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x);
	//机器手 移动y
	Error_manager catcher_move_y(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_y);
	//机器手 移动z
	Error_manager catcher_move_z(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_z);
	//机器手调整到 准备从地面抓车前的姿态
	Error_manager catcher_adjust_from_ground(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
	Error_manager catcher_adjust_wheel_base(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//机器手 移动c轴 夹杆
	Error_manager catcher_move_c(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, Catcher_task::Clamp_motion target);
	//机器手调整到 对接搬运器的姿态
	Error_manager catcher_adjust_to_carrier(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x, bool reverse_flag);
	//机器手调整到 准备把车放到地面的姿态
	Error_manager catcher_adjust_to_ground(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x, float target_y);
	//机器手调整到 对接搬运器的姿态
	Error_manager catcher_adjust_from_carrier(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x);

	//搬运器 准备开始, 需要同步任务单和设备真实数据.
	Error_manager carrier_ready_to_start(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//搬运器调整到 正常待机的姿态(调节夹杆和轴距)
	Error_manager carrier_adjust_to_ready(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates);
	//搬运器 移动x
	Error_manager carrier_move_x(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x);
	//搬运器 移动y
	Error_manager carrier_move_y(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_y);
	//搬运器 移动z
	Error_manager carrier_move_z(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_z);
	//搬运器 移动c轴 夹车杆
	Error_manager carrier_move_c(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Clamp_motion target);
	//搬运器调整 水平的交接
	Error_manager carrier_joint_x(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Joint_motion target);
	//搬运器 修正轴距
	Error_manager carrier_adjust_wheel_base(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates);

public://member variable
	std::atomic<Dispatch_process_status>	m_dispatch_process_status;	//调度流程的状态, 控制步骤

	//流程总规划数据, 外部指令
	std::mutex                          	m_lock;
	Common_data::Dispatch_process_type		m_dispatch_process_type;	//调度任务类型
	int 									m_dispatch_source;			//调度的起点,源头 (Dispatch_plan 总计划的起点)
	int 									m_dispatch_destination;		//调度的终点,目的地(Dispatch_plan 总计划的终点)
	Common_data::Car_measure_information	m_car_measure_information;	//整车的测量信息
	Common_data::Parkspace_information		m_parkspace_information_optimal;	//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
	Common_data::Parkspace_information		m_parkspace_information_left;		//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
	Common_data::Parkspace_information		m_parkspace_information_right;		//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
	Common_data::Parkspace_information		m_parkspace_information_temporary;	//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
	Common_data::Parkspace_type				m_parkspace_type;					//车位类型
	Common_data::Car_type					m_car_type;							//车辆类型
	bool 									m_temporary_cache_flag;				//是否使用临时缓存的标记位

	std::string								m_command_key;						//任务唯一码, 索引
	int 									m_timeout_ms;						//超时时间,单位ms
	std::chrono::system_clock::time_point	m_start_time;						//流程开始时间
	Error_manager 							m_result;							//流程的执行结果
	float 									m_wheel_base;						//轮距


	//调度设备控制节点, 核心设备由调度管理进行资源分配, 本流程模块只负责使用
	Dispatch_catcher_node					m_dispatch_catcher_node;			//调度设备机器人节点
	Dispatch_carrier_node					m_dispatch_carrier_node;			//调度设备搬运器节点




	//调度控制的长流程节点map
	std::map<int, Dispatch_control_node> 	m_dispatch_control_node_map;

	//通信缓存
	message::Dispatch_request_msg			m_dispatch_request_msg;				//1执行搬运请求(主控->调度管理)
	message::Dispatch_plan_request_msg		m_dispatch_plan_request_msg;		//2调度总规划的请求, 用于启动整个调度算法(调度管理->调度算法)

	message::Dispatch_plan_response_msg		m_dispatch_plan_response_msg;		//5调度总规划的答复(调度算法->调度管理)
	message::Dispatch_response_msg			m_dispatch_response_msg;			//6搬运动作执行完成后反馈结果(调度管理->主控)
	//流程通信顺序为  收1->发2  ->收3->发4->收3->发4->收3->发4.....  ->收5->发6


//	std::chrono::system_clock::time_point  m_time_to_send_control_response;		//发送 控制的任务答复 的时间

	Dispatch_control_status						mcarrier;
	Dispatch_control_status						mcatcher;
private:

};


#endif //NNXX_TESTS_DISPATCH_PROCESS_H