|
@@ -59,10 +59,17 @@ public:
|
|
|
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, //流程释放
|
|
|
|
|
@@ -76,13 +83,69 @@ public:
|
|
|
//调度控制的状态, 控制步骤(对于一个设备的长流程的控制)
|
|
|
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_CONTROL_WORKING = 4, //流程工作正忙, 进行长流程
|
|
|
- DISPATCH_CONTROL_TASK_WITHDRAW = 5, //流程 收回任务单
|
|
|
- DISPATCH_CONTROL_RESPONSE = 6, //流程 给调度控制答复
|
|
|
+
|
|
|
+ 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_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_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_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_CONTROL_DISCONNECT_DEVICE = 7, //流程 解除设备
|
|
|
DISPATCH_CONTROL_OVER = 8, //流程完成
|
|
|
DISPATCH_CONTROL_RELEASE = 9, //流程释放
|
|
@@ -107,29 +170,39 @@ public:
|
|
|
std::chrono::system_clock::time_point m_time_to_send_control_response; //发送 控制的任务答复 的时间 注释
|
|
|
};
|
|
|
|
|
|
+
|
|
|
//调度设备机器人节点
|
|
|
struct Dispatch_catcher_node
|
|
|
{
|
|
|
- Dispatch_control_status m_dispatch_control_status; //调度控制的状态, 控制步骤(对于一个设备的长流程的控制)
|
|
|
- //核心机器人, 2号
|
|
|
+ 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; //调度任务指针, 内存由本模块管理
|
|
|
//跟随搬运器, 3号搬运器
|
|
|
+ bool m_following_flag; //3号搬运器是否跟随机器人
|
|
|
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; //调度控制的状态, 控制步骤(对于一个设备的长流程的控制)
|
|
|
- //核心搬运器, 1号或者2号搬运器
|
|
|
+ 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; //调度任务指针, 内存由本模块管理
|
|
|
//给机器人发送避让指令, 2号机器人(可选)
|
|
|
std::shared_ptr<Dispatch_device_base> mp_avoid_catcher; //调度设备指针, 内存由Dispatch_manager来管理
|
|
|
std::shared_ptr<Task_Base> mp_avoid_catcher_task; //调度任务指针, 内存由本模块管理
|
|
|
+
|
|
|
+ Error_manager m_error; //错误码
|
|
|
+
|
|
|
};
|
|
|
|
|
|
|
|
@@ -161,6 +234,8 @@ public://API functions
|
|
|
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://主流程
|
|
|
|
|
|
//主线程
|
|
@@ -168,13 +243,30 @@ protected://主流程
|
|
|
|
|
|
//检查调度请求
|
|
|
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 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);
|
|
|
//连接调度设备, 创建新的任务单 与设备建立连接
|
|
@@ -229,6 +321,8 @@ protected://长流程
|
|
|
Error_manager excute_passageway_motion(Dispatch_control_node & dispatch_control_node);
|
|
|
|
|
|
protected://短流程
|
|
|
+ //检查 任务单 是否完成任务, 里面会调整短步骤
|
|
|
+ Error_manager check_task(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);
|
|
|
|
|
@@ -264,6 +358,39 @@ protected://短流程
|
|
|
//搬运器 修正轴距
|
|
|
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_adjust_to_ready(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates);
|
|
|
+ //机器手 移动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);
|
|
|
+ //机器手调整到 准备从地面抓车前的姿态
|
|
|
+ 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_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);
|
|
|
+ //搬运器 移动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);
|
|
|
+ //搬运器 移动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);
|
|
|
+ //搬运器 移动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; //调度流程的状态, 控制步骤
|
|
|
|