dispatch_process.h 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265
  1. /*
  2. dispatch_source 和 dispatch_destination 的表示含义
  3. 1~165 2楼~12楼的停车位
  4. 1100 0号出口(在一楼)(目前没有)
  5. 1101~1106 1~6号出入口 (在一楼)
  6. 1107 7号出口(在一楼)
  7. 1201~1206 1~6号出入口上方2楼处
  8. 1301~1306 1~6号出入口上方3楼处
  9. 1401~1406 1~6号出入口上方4楼处
  10. 2101~2112 左侧电梯井(1楼~12楼)(一号搬运器的待机点)
  11. 2201~2212 右侧电梯井(1楼~12楼)(二号搬运器的待机点)
  12. */
  13. #ifndef NNXX_TESTS_DISPATCH_PROCESS_H
  14. #define NNXX_TESTS_DISPATCH_PROCESS_H
  15. #include <error_code.h>
  16. #include "../tool/thread_condition.h"
  17. #include "../tool/TaskQueue/BaseTask.h"
  18. #include "../tool/common_data.h"
  19. #include <glog/logging.h>
  20. #include "../dispatch/carrier.h"
  21. #include "../dispatch/catcher.h"
  22. #include "../dispatch/passageway.h"
  23. #include "../dispatch/dispatch_coordinates.h"
  24. #include "../message/dispatch_message.pb.h"
  25. #include "../message/dispatch_control.pb.h"
  26. #include "../tool/point3D_tool.h"
  27. //调度流程 存车
  28. class Dispatch_process : public tq::BaseTask
  29. {
  30. public:
  31. //调度指令超时时间 300000ms = 5min
  32. #define DISPATCH_PROCESS_TIMEOUT_MS 600000
  33. //调度指令超时时间 衰减值 在原有的基础上减少30秒
  34. #define DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS 30000
  35. //调度默认 差值 2mm
  36. #define DISPATCH_DEFAULT_DIFFERENCE 2
  37. //调度默认 比值 1%
  38. #define DISPATCH_DEFAULT_RATE 0.01
  39. //调度流程的状态, 控制步骤
  40. enum Dispatch_process_status
  41. {
  42. DISPATCH_PROCESS_STATUS_UNKNOW = 0, //未知
  43. DISPATCH_PROCESS_CREATED = 1, //流程创建,
  44. DISPATCH_PROCESS_READY = 2, //流程准备,待机
  45. DISPATCH_PROCESS_CONNECT_DEVICE = 3, //流程 连接设备
  46. DISPATCH_PROCESS_WORKING = 4, //流程工作正忙, 进行长流程
  47. DISPATCH_PROCESS_RESPONSE = 5, //流程 给调度控制答复
  48. DISPATCH_PROCESS_DISCONNECT_DEVICE = 6, //流程 解除设备
  49. DISPATCH_PROCESS_OVER = 7, //流程完成
  50. DISPATCH_PROCESS_RELEASE = 8, //流程释放
  51. DISPATCH_PROCESS_FAULT = 10, //故障
  52. DISPATCH_PROCESS_MOTION_FAILED = 11, //单个小动作执行失败
  53. };
  54. //调度任务类型
  55. enum Dispatch_process_type
  56. {
  57. DISPATCH_PROCESS_TYPE_UNKNOW = 0, //未知
  58. DISPATCH_PROCESS_STORE = 101, //存车
  59. DISPATCH_PROCESS_PICKUP = 102, //取车
  60. };
  61. //机器手的动作, 控制步骤
  62. enum Dispatch_process_catcher_motion
  63. {
  64. CATCHER_MOTION_UNKNOW = 0, //未知
  65. };
  66. //搬运器的动作, 控制步骤
  67. enum Dispatch_process_carrier_motion
  68. {
  69. CARRIER_MOTION_UNKNOW = 0, //未知
  70. };
  71. public:
  72. Dispatch_process();
  73. Dispatch_process(const Dispatch_process& other)= default;
  74. Dispatch_process& operator =(const Dispatch_process& other)= default;
  75. ~Dispatch_process();
  76. public://API functions
  77. //初始化, 就把主控发送的请求传入即可.
  78. Error_manager Dispatch_process_init(message::Dispatch_request_msg &dispatch_request_msg);
  79. //反初始化
  80. Error_manager Dispatch_process_uninit();
  81. //检查流程是否空闲待机
  82. Error_manager check_process_ready();
  83. public://get or set member variable
  84. protected://主流程
  85. //主线程
  86. virtual void Main();
  87. //检查调度请求
  88. Error_manager check_dispatch_request_msg();
  89. //发送调度总计划
  90. Error_manager send_dispatch_plan_request_msg();
  91. //等待控制指令
  92. Error_manager wait_dispatch_control_request_msg();
  93. //连接调度设备, 创建新的任务单 与设备建立连接
  94. Error_manager connect_dispatch_device();
  95. //执行调度控制指令, 并根据完成情况给答复
  96. Error_manager excute_dispatch_control();
  97. //发送调度控制答复, 发给调度控制的
  98. Error_manager send_dispatch_control_response_msg(message::Dispatch_device_task_status dispatch_device_task_status);
  99. //断开调度设备, 释放任务单 与设备解除连接
  100. Error_manager disconnect_dispatch_device();
  101. //等待调度总计划答复
  102. Error_manager wait_dispatch_plan_response_msg();
  103. //发送调度答复, 发给主控的
  104. Error_manager send_dispatch_response_msg();
  105. //通知调度管理, 释放资源,
  106. Error_manager release_resource();
  107. //异常处理
  108. Error_manager Exception_handling();
  109. protected://长流程
  110. //机器手去入口抓车(例如:机器手移动到2号入口上方,然后下降到一楼抓车,最后上升到最上方)
  111. Error_manager excute_robot_catch_car_from_inlet();
  112. //机器手把车放到中跑车上面(例如:机器手下降到中跑车上放车,最后上升到最上方)(通过目标点 指定放到哪一个中跑车上)
  113. Error_manager excute_robot_put_car_to_carrier();
  114. //机器手去中跑车上抓车(例如:机器手移动到1号中跑车上方,然后下降到中跑车抓车,最后上升到最上方)
  115. Error_manager excute_robot_catch_car_from_carrier();
  116. //机器手去出口放车(例如:机器手移动到3号出口上方,然后下降到一楼放车,最后上升到最上方)
  117. Error_manager excute_robot_put_car_to_outlet();
  118. //机器手的自由移动(例如:机器手移动到6号出入口的上方4楼处,给其他设备避让)(不进行抓车和放车的操作)
  119. Error_manager excute_robot_move();
  120. //搬运器从机器手上接车(例如:搬运器移动到2号入口的上方,然后等待机器手把车放到中跑车上,小跑车保持松夹杆状态)
  121. Error_manager excute_carrier_receive_car_from_robot();
  122. //搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至电梯井)
  123. Error_manager excute_carrier_store_car_to_parkingspace();
  124. //搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至56车位外面即可)
  125. Error_manager excute_carrier_store_car_to_parkingspace_ex();
  126. //搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至电梯井)
  127. Error_manager excute_carrier_pickup_car_from_parkingspace();
  128. //搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至56车位外面即可)
  129. Error_manager excute_carrier_pickup_car_from_parkingspace_ex();
  130. //搬运器把车交付给机器手(例如:搬运器移动到3号入口的上方,小跑车松夹杆,然后等待机器手把车从中跑车上取走)
  131. Error_manager excute_carrier_deliver_car_to_robot();
  132. //搬运器的自由移动(可以提前到2楼来准备接车,或者为了避让就退回至电梯井)(小跑车不进行取车和存车)
  133. Error_manager excute_carrier_move();
  134. //执行通道口动作
  135. Error_manager excute_passageway_motion();
  136. protected://短流程
  137. //检查机器手任务单
  138. Error_manager check_catcher_task();
  139. //检查搬运器任务单
  140. Error_manager check_carrier_task();
  141. //检查通道口任务单
  142. Error_manager check_passageway_task();
  143. //检查 任务单 是否完成任务, 里面会调整短步骤
  144. Error_manager check_task_ex(std::shared_ptr<Task_Base> p_task, int& step);
  145. //机器手调整到 正常待机的姿态(调节夹杆和轴距)
  146. Error_manager catcher_adjust_to_ready(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
  147. //机器手 移动z
  148. Error_manager catcher_move_z(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target);
  149. //机器手调整到 准备从地面抓车前的姿态
  150. Error_manager catcher_adjust_from_ground(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
  151. //机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
  152. Error_manager catcher_adjust_wheel_base(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
  153. //机器手 移动c轴 夹杆
  154. Error_manager catcher_move_c(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, Catcher_task::Clamp_motion target);
  155. //机器手调整到 对接搬运器的姿态
  156. Error_manager catcher_adjust_to_carrier(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, bool reverse_flag);
  157. //机器手调整到 准备把车放到地面的姿态
  158. Error_manager catcher_adjust_to_ground(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
  159. //机器手调整到 对接搬运器的姿态
  160. Error_manager catcher_adjust_from_carrier(Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
  161. //搬运器调整到 正常待机的姿态(调节夹杆和轴距)
  162. Error_manager carrier_adjust_to_ready(Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates);
  163. //搬运器 移动x
  164. Error_manager carrier_move_x(Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target);
  165. //搬运器 移动y
  166. Error_manager carrier_move_y(Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target);
  167. //搬运器 移动z
  168. Error_manager carrier_move_z(Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target);
  169. //搬运器 移动c轴 夹车杆
  170. Error_manager carrier_move_c(Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Clamp_motion target);
  171. //搬运器调整 水平的交接
  172. Error_manager carrier_joint_x(Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Joint_motion target);
  173. //搬运器 修正轴距
  174. Error_manager carrier_adjust_wheel_base(Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates);
  175. public://member variable
  176. Dispatch_process_status m_dispatch_process_status; //调度流程的状态, 控制步骤
  177. //流程总规划数据, 外部指令
  178. std::mutex m_lock;
  179. Dispatch_process_type m_dispatch_process_type; //调度任务类型
  180. int m_dispatch_source; //调度的起点,源头 (Dispatch_plan 总计划的起点)
  181. int m_dispatch_destination; //调度的终点,目的地(Dispatch_plan 总计划的终点)
  182. Common_data::Car_measure_information m_car_measure_information; //整车的测量信息
  183. Common_data::Parkspace_information m_parkspace_information; //单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
  184. std::string m_command_key; //任务唯一码
  185. int m_timeout_ms; //超时时间,单位ms
  186. std::chrono::system_clock::time_point m_start_time; //流程开始时间
  187. Error_manager m_result; //流程的执行结果
  188. float m_wheel_base; //轮距
  189. // Dispatch_process_catcher_motion m_dispatch_process_catcher_motion; //机器手的动作, 控制步骤
  190. // Dispatch_process_carrier_motion m_dispatch_process_carrier_motion; //搬运器的动作, 控制步骤
  191. //硬件资源
  192. std::shared_ptr<Dispatch_device_base> mp_catcher; //抓车器指针, 内存由Dispatch_manager来管理
  193. std::shared_ptr<Task_Base> mp_catcher_task; //抓车器任务, 内存由本模块管理
  194. std::shared_ptr<Dispatch_device_base> mp_carrier; //搬运器指针, 内存由Dispatch_manager来管理
  195. std::shared_ptr<Task_Base> mp_carrier_task; //搬运器任务, 内存由本模块管理
  196. std::shared_ptr<Dispatch_device_base> mp_passageway; //通道口指针, 内存由Dispatch_manager来管理
  197. std::shared_ptr<Task_Base> mp_passageway_task; //通道口任务, 内存由本模块管理
  198. //通信缓存
  199. message::Dispatch_request_msg m_dispatch_request_msg; //1执行搬运请求(主控->调度管理)
  200. message::Dispatch_plan_request_msg m_dispatch_plan_request_msg; //2调度总规划的请求, 用于启动整个调度算法(调度管理->调度算法)
  201. message::Dispatch_control_request_msg m_dispatch_control_request_msg_catcher; //3调度控制的任务请求(调度算法->调度管理)
  202. message::Dispatch_control_response_msg m_dispatch_control_response_msg_catcher; //4调度控制的任务答复(调度管理->调度算法)
  203. Point3D_tool::Point3D m_source_coordinates_catcher; //起点 的坐标 (Dispatch_control 长流程的起点)
  204. Point3D_tool::Point3D m_destination_coordinates_catcher; //终点 的坐标 (Dispatch_control 长流程的终点)
  205. message::Dispatch_control_request_msg m_dispatch_control_request_msg_carrier; //3调度控制的任务请求(调度算法->调度管理)
  206. message::Dispatch_control_response_msg m_dispatch_control_response_msg_carrier; //4调度控制的任务答复(调度管理->调度算法)
  207. Point3D_tool::Point3D m_source_coordinates_carrier; //起点 的坐标 (Dispatch_control 长流程的起点)
  208. Point3D_tool::Point3D m_destination_coordinates_carrier; //终点 的坐标 (Dispatch_control 长流程的终点)
  209. message::Dispatch_plan_response_msg m_dispatch_plan_response_msg; //5调度总规划的答复(调度算法->调度管理)
  210. message::Dispatch_response_msg m_dispatch_response_msg; //6搬运动作执行完成后反馈结果(调度管理->主控)
  211. //流程通信顺序为 收1->发2 ->收3->发4->收3->发4->收3->发4..... ->收5->发6
  212. std::chrono::system_clock::time_point m_time_to_send_control_response; //发送 控制的任务答复 的时间
  213. private:
  214. };
  215. #endif //NNXX_TESTS_DISPATCH_PROCESS_H