/* 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 #include "../tool/thread_condition.h" #include "../tool/TaskQueue/BaseTask.h" #include "../tool/common_data.h" #include #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, //单个小动作执行失败 }; //调度控制节点, 对应一个dispatch_control的长流程 struct Dispatch_control_node { Dispatch_control_status m_dispatch_control_status; //调度控制的状态, 控制步骤(对于一个设备的长流程的控制) std::shared_ptr mp_dispatch_device; //调度设备指针, 内存由Dispatch_manager来管理 std::shared_ptr 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; //发送 控制的任务答复 的时间 }; 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 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 m_dispatch_control_node_map; // // //硬件资源 // std::shared_ptr mp_catcher; //抓车器指针, 内存由Dispatch_manager来管理 // std::shared_ptr mp_catcher_task; //抓车器任务, 内存由本模块管理 // std::shared_ptr mp_carrier0; //搬运器指针, 内存由Dispatch_manager来管理 // std::shared_ptr mp_carrier_task0; //搬运器任务, 内存由本模块管理 // std::shared_ptr mp_carrier7; //搬运器指针, 内存由Dispatch_manager来管理 // std::shared_ptr mp_carrier_task7; //搬运器任务, 内存由本模块管理 // std::shared_ptr mp_carrier3; //搬运器指针, 内存由Dispatch_manager来管理 // std::shared_ptr mp_carrier_task3; //搬运器任务, 内存由本模块管理 // std::shared_ptr mp_passageway; //通道口指针, 内存由Dispatch_manager来管理 // std::shared_ptr mp_passageway_task; //通道口任务, 内存由本模块管理 //通信缓存 message::Dispatch_request_msg m_dispatch_request_msg; //1执行搬运请求(主控->调度管理) message::Dispatch_plan_request_msg m_dispatch_plan_request_msg; //2调度总规划的请求, 用于启动整个调度算法(调度管理->调度算法) // message::Dispatch_control_request_msg m_dispatch_control_request_msg_catcher; //3调度控制的任务请求(调度算法->调度管理) // message::Dispatch_control_response_msg m_dispatch_control_response_msg_catcher; //4调度控制的任务答复(调度管理->调度算法) // Point3D_tool::Point3D m_source_coordinates_catcher; //起点 的坐标 (Dispatch_control 长流程的起点) // Point3D_tool::Point3D m_destination_coordinates_catcher; //终点 的坐标 (Dispatch_control 长流程的终点) // // message::Dispatch_control_request_msg m_dispatch_control_request_msg_carrier0; //3调度控制的任务请求(调度算法->调度管理) // message::Dispatch_control_response_msg m_dispatch_control_response_msg_carrier0; //4调度控制的任务答复(调度管理->调度算法) // Point3D_tool::Point3D m_source_coordinates_carrier0; //起点 的坐标 (Dispatch_control 长流程的起点) // Point3D_tool::Point3D m_destination_coordinates_carrier0; //终点 的坐标 (Dispatch_control 长流程的终点) // // message::Dispatch_control_request_msg m_dispatch_control_request_msg_carrier7; //3调度控制的任务请求(调度算法->调度管理) // message::Dispatch_control_response_msg m_dispatch_control_response_msg_carrier7; //4调度控制的任务答复(调度管理->调度算法) // Point3D_tool::Point3D m_source_coordinates_carrier7; //起点 的坐标 (Dispatch_control 长流程的起点) // Point3D_tool::Point3D m_destination_coordinates_carrier7; //终点 的坐标 (Dispatch_control 长流程的终点) // // message::Dispatch_control_request_msg m_dispatch_control_request_msg_carrier3; //3调度控制的任务请求(调度算法->调度管理) // message::Dispatch_control_response_msg m_dispatch_control_response_msg_carrier3; //4调度控制的任务答复(调度管理->调度算法) // Point3D_tool::Point3D m_source_coordinates_carrier3; //起点 的坐标 (Dispatch_control 长流程的起点) // Point3D_tool::Point3D m_destination_coordinates_carrier3; //终点 的坐标 (Dispatch_control 长流程的终点) 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