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"

//调度流程 存车
class Dispatch_process : public tq::BaseTask
{
public:
	//调度指令超时时间 300000ms = 5min
#define DISPATCH_PROCESS_TIMEOUT_MS							600000
//调度指令超时时间  衰减值 在原有的基础上减少30秒
#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_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_process_type
	{
		DISPATCH_PROCESS_TYPE_UNKNOW 			= 0,    //未知
		DISPATCH_PROCESS_STORE  				= 101,    //存车
		DISPATCH_PROCESS_PICKUP					= 102,	//取车
	};



	//调度控制的状态, 控制步骤(对于一个设备的长流程的控制)
	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,	//单个小动作执行失败
	};
	//调度控制节点, 对应一个设备
	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;			//调度控制的状态, 控制步骤(对于一个设备的长流程的控制)
		//核心机器人, 2号
		std::shared_ptr<Catcher>				mp_main_catcher;						//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Catcher_task>			mp_main_catcher_task;					//调度任务指针, 内存由本模块管理
		//跟随搬运器, 3号搬运器
		std::shared_ptr<Carrier>				mp_following_carrier;					//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Carrier_task>			mp_following_carrier_task;				//调度任务指针, 内存由本模块管理
		//目前没有让别人避让的功能
	};

	//调度设备机器人节点
	struct Dispatch_carrier_node
	{
		Dispatch_control_status					m_dispatch_control_status;			//调度控制的状态, 控制步骤(对于一个设备的长流程的控制)
		//核心搬运器, 1号或者2号搬运器
		std::shared_ptr<Carrier>				mp_main_carrier;					//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Carrier_task>			mp_main_carrier_task;				//调度任务指针, 内存由本模块管理
		//给机器人发送避让指令, 2号机器人(可选)
		std::shared_ptr<Catcher>				mp_avoid_catcher;						//调度设备指针, 内存由Dispatch_manager来管理
		std::shared_ptr<Catcher_task>			mp_avoid_catcher_task;					//调度任务指针, 内存由本模块管理
	};


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();
public://get or set member variable

protected://主流程

	//主线程
	virtual void Main();

	//检查调度请求
	Error_manager check_dispatch_request_msg();
	//发送调度总计划
	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_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);

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

	//流程总规划数据, 外部指令
	std::mutex                          	m_lock;
	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;	//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息

	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;						//轮距


	//调度控制的节点





	//调度控制的长流程节点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;		//发送 控制的任务答复 的时间

private:

};


#endif //NNXX_TESTS_DISPATCH_PROCESS_H