|
|
@@ -1,853 +0,0 @@
|
|
|
-//
|
|
|
-// Created by huli on 2020/7/20.
|
|
|
-//
|
|
|
-
|
|
|
-#include "carrier_base.h"
|
|
|
-#include "dispatch_communication.h"
|
|
|
-/*
|
|
|
-Carrier_base::Carrier_base()
|
|
|
-{
|
|
|
- m_carrier_status = E_UNKNOW;
|
|
|
- m_carrier_id = -1;
|
|
|
-
|
|
|
- m_request_x = 0; //搬运器坐标x轴, 中跑车控制横向移动
|
|
|
- m_request_y = 0; //搬运器坐标y轴, 小跑车控制纵向移动
|
|
|
- m_request_z = 0; //搬运器坐标z轴, 电梯控制上下移动
|
|
|
- m_request_space_id = 0; //搬运器空间位置的id.
|
|
|
- m_request_y1 = 0; //搬运器坐标y1轴, 小跑车控制纵向移动(前轮抓杆)
|
|
|
- m_request_y2 = 0; //搬运器坐标y2轴, 小跑车控制纵向移动(后轮抓杆)
|
|
|
- m_request_wheelbase = 0; //搬运器小跑车的抓车杆前后轮距.
|
|
|
- m_request_clamp_motion = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作，1=夹紧，2=松开
|
|
|
- m_request_joint_motion = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作，
|
|
|
-
|
|
|
- m_respons_status = RESPONS_WORKING;
|
|
|
- m_respons_x = 0; //搬运器坐标x轴, 中跑车控制横向移动
|
|
|
- m_respons_y = 0; //搬运器坐标y轴, 小跑车控制纵向移动
|
|
|
- m_respons_z = 0; //搬运器坐标z轴, 电梯控制上下移动
|
|
|
- m_respons_space_id = 0; //搬运器空间位置的id.
|
|
|
- m_respons_y1 = 0; //搬运器坐标y1轴, 小跑车控制纵向移动(前轮抓杆)
|
|
|
- m_respons_y2 = 0; //搬运器坐标y2轴, 小跑车控制纵向移动(后轮抓杆)
|
|
|
- m_respons_wheelbase = 0; //搬运器小跑车的抓车杆前后轮距.
|
|
|
- m_respons_clamp_motion = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作，1=夹紧，2=松开
|
|
|
- m_respons_joint_motion = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作，
|
|
|
-
|
|
|
- m_status_updata_time = std::chrono::system_clock::now();
|
|
|
- m_last_heartbeat = 0;
|
|
|
- m_device_status = DEVICE_UNKNOWN;
|
|
|
- m_load_status = LOAD_UNKNOWN;
|
|
|
- m_actual_x = 0; //搬运器坐标x轴, 中跑车控制横向移动
|
|
|
- m_actual_y = 0; //搬运器坐标y轴, 小跑车控制纵向移动
|
|
|
- m_actual_z = 0; //搬运器坐标z轴, 电梯控制上下移动
|
|
|
- m_actual_y1 = 0; //搬运器坐标y1轴, 小跑车控制纵向移动(前轮抓杆)
|
|
|
- m_actual_y2 = 0; //搬运器坐标y2轴, 小跑车控制纵向移动(后轮抓杆)
|
|
|
- m_actual_clamp_motion1 = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作，1=夹紧，2=松开
|
|
|
- m_actual_clamp_motion2 = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作，1=夹紧，2=松开
|
|
|
- m_actual_joint_motion1 = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作，
|
|
|
- m_actual_joint_motion2 = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作，
|
|
|
- m_actual_small_sports_car_motion = E_SMALL_SPORTS_NO_ACTION; //小跑车的位置,是否在中跑车上面
|
|
|
-
|
|
|
- memset(m_carrier_error_code, 0, 50); //搬运器设备的报警信息位
|
|
|
- memset(m_carrier_warning_code, 0, 50); //升降机设备的报警信息位
|
|
|
-
|
|
|
- mp_execute_thread = NULL;
|
|
|
-
|
|
|
-}
|
|
|
-
|
|
|
-Carrier_base::~Carrier_base()
|
|
|
-{
|
|
|
- carrier_base_uninit();
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-//搬运器 初始化
|
|
|
-Error_manager Carrier_base::carrier_base_init()
|
|
|
-{
|
|
|
-// 线程默认开启
|
|
|
- m_execute_condition.reset(false, true, false);
|
|
|
- mp_execute_thread = new std::thread(&Carrier_base::execute_thread_fun, this);
|
|
|
-
|
|
|
- m_carrier_status = E_READY;
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-//搬运器 反初始化
|
|
|
-Error_manager Carrier_base::carrier_base_uninit()
|
|
|
-{
|
|
|
- if (mp_execute_thread)
|
|
|
- {
|
|
|
- m_execute_condition.kill_all();
|
|
|
- }
|
|
|
- if (mp_execute_thread)
|
|
|
- {
|
|
|
- mp_execute_thread->join();
|
|
|
- delete mp_execute_thread;
|
|
|
- mp_execute_thread = NULL;
|
|
|
- }
|
|
|
-
|
|
|
- m_carrier_status = E_UNKNOW;
|
|
|
-
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-//处理一级任务, 普通任务
|
|
|
-Error_manager Carrier_base::execute_one_level_task(std::shared_ptr<Carrier_task> p_carrier_task)
|
|
|
-{
|
|
|
- Error_manager t_error;
|
|
|
- Error_manager t_result;
|
|
|
-
|
|
|
- //检查指针
|
|
|
- if (p_carrier_task == NULL) {
|
|
|
- return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
|
|
|
- "Carrier_base::execute_one_level_task failed, POINTER_IS_NULL");
|
|
|
- }
|
|
|
- //检查任务类型，
|
|
|
- if (p_carrier_task->get_task_type() != CARRIER_TASK)
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
|
|
|
- "Carrier_base::execute_one_level_task get_task_type() != CARRIER_TASK ");
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- //检查接收方的状态
|
|
|
- t_error = check_one_level_task();
|
|
|
- if ( t_error != SUCCESS )
|
|
|
- {
|
|
|
- t_result.compare_and_cover_error(t_error);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //接受任务，并将任务的状态改为TASK_SIGNED已签收
|
|
|
- mp_carrier_one_level_task = p_carrier_task;
|
|
|
- mp_carrier_one_level_task->set_task_statu(TASK_SIGNED);
|
|
|
-
|
|
|
- //这里不用检查任务内容, 直接下发给底层设备
|
|
|
- //启动定位管理模块，的核心工作线程
|
|
|
- m_carrier_status = E_ONE_LEVEL_WORK;
|
|
|
- m_execute_condition.notify_all(true);
|
|
|
- //通知 thread_work 子线程启动。
|
|
|
-
|
|
|
- //将任务的状态改为 TASK_WORKING 处理中
|
|
|
- //20200225, 在线程真正的执行的时候,才改为工作中, (执行线程可能会先处理更高优先级的任务单)
|
|
|
-// mp_carrier_one_level_task->set_task_statu(TASK_WORKING);
|
|
|
- }
|
|
|
-
|
|
|
- if ( t_result != Error_code::SUCCESS )
|
|
|
- {
|
|
|
- return t_result;
|
|
|
- }
|
|
|
-
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-//处理二级任务, 插队任务
|
|
|
-Error_manager Carrier_base::execute_two_level_task(std::shared_ptr<Carrier_task> p_carrier_task)
|
|
|
-{
|
|
|
- Error_manager t_error;
|
|
|
- Error_manager t_result;
|
|
|
-
|
|
|
- //检查指针
|
|
|
- if (p_carrier_task == NULL) {
|
|
|
- return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
|
|
|
- "Carrier_base::execute_two_level_task failed, POINTER_IS_NULL");
|
|
|
- }
|
|
|
- //检查任务类型，
|
|
|
- if (p_carrier_task->get_task_type() != CARRIER_TASK)
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
|
|
|
- "Carrier_base::execute_two_level_task get_task_type() != CARRIER_TASK ");
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- //检查接收方的状态
|
|
|
- t_error = check_two_level_task();
|
|
|
- if ( t_error != SUCCESS )
|
|
|
- {
|
|
|
- t_result.compare_and_cover_error(t_error);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //接受任务，并将任务的状态改为TASK_SIGNED已签收
|
|
|
- mp_carrier_two_level_task = p_carrier_task;
|
|
|
- mp_carrier_two_level_task->set_task_statu(TASK_SIGNED);
|
|
|
-
|
|
|
- //这里不用检查任务内容, 直接下发给底层设备
|
|
|
- //启动定位管理模块，的核心工作线程
|
|
|
- m_carrier_status = E_TWO_LEVEL_WORK;
|
|
|
- m_execute_condition.notify_all(true);
|
|
|
- //通知 thread_work 子线程启动。
|
|
|
-
|
|
|
- //将任务的状态改为 TASK_WORKING 处理中
|
|
|
- //20200225, 在线程真正的执行的时候,才改为工作中, (执行线程可能会先处理更高优先级的任务单)
|
|
|
-// mp_carrier_two_level_task->set_task_statu(TASK_WORKING);
|
|
|
- }
|
|
|
-
|
|
|
- if ( t_result != Error_code::SUCCESS )
|
|
|
- {
|
|
|
- return t_result;
|
|
|
- }
|
|
|
-
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-//处理三级任务, 核心任务
|
|
|
-Error_manager Carrier_base::execute_three_level_task(std::shared_ptr<Carrier_task> p_carrier_task)
|
|
|
-{
|
|
|
- Error_manager t_error;
|
|
|
- Error_manager t_result;
|
|
|
-
|
|
|
- //检查指针
|
|
|
- if (p_carrier_task == NULL) {
|
|
|
- return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
|
|
|
- "Carrier_base::execute_three_level_task failed, POINTER_IS_NULL");
|
|
|
- }
|
|
|
- //检查任务类型，
|
|
|
- if (p_carrier_task->get_task_type() != CARRIER_TASK)
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
|
|
|
- "Carrier_base::execute_three_level_task get_task_type() != CARRIER_TASK ");
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- //检查接收方的状态
|
|
|
- t_error = check_three_level_task();
|
|
|
- if ( t_error != SUCCESS )
|
|
|
- {
|
|
|
- t_result.compare_and_cover_error(t_error);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //接受任务，并将任务的状态改为TASK_SIGNED已签收
|
|
|
- mp_carrier_three_level_task = p_carrier_task;
|
|
|
- mp_carrier_three_level_task->set_task_statu(TASK_SIGNED);
|
|
|
-
|
|
|
- //这里不用检查任务内容, 直接下发给底层设备
|
|
|
- //启动定位管理模块，的核心工作线程
|
|
|
- m_carrier_status = E_THREE_LEVEL_WORK;
|
|
|
- m_execute_condition.notify_all(true);
|
|
|
- //通知 thread_work 子线程启动。
|
|
|
-
|
|
|
- //将任务的状态改为 TASK_WORKING 处理中
|
|
|
- //20200225, 在线程真正的执行的时候,才改为工作中, (执行线程可能会先处理更高优先级的任务单)
|
|
|
-// mp_carrier_three_level_task->set_task_statu(TASK_WORKING);
|
|
|
- }
|
|
|
-
|
|
|
- if ( t_result != Error_code::SUCCESS )
|
|
|
- {
|
|
|
- return t_result;
|
|
|
- }
|
|
|
-
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-//检查状态,是否正常运行
|
|
|
-Error_manager Carrier_base::check_status()
|
|
|
-{
|
|
|
- if ( m_carrier_status == E_READY )
|
|
|
- {
|
|
|
- return Error_code::SUCCESS;
|
|
|
- }
|
|
|
- else if ( m_carrier_status >= E_BUSY && m_carrier_status <= E_THREE_LEVEL_OVER)
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
|
|
|
- " Carrier_base::check_status is busy ");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_STATUS_ERROR, Error_level::MINOR_ERROR,
|
|
|
- " Carrier_base::check_status error ");
|
|
|
- }
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-//判断能否执行一级任务
|
|
|
-Error_manager Carrier_base::check_one_level_task()
|
|
|
-{
|
|
|
- if ( m_carrier_status == E_READY )
|
|
|
- {
|
|
|
- return Error_code::SUCCESS;
|
|
|
- }
|
|
|
- else if ( m_carrier_status >= E_BUSY && m_carrier_status <= E_THREE_LEVEL_OVER)
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
|
|
|
- " Carrier_base::check_status is busy ");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_STATUS_ERROR, Error_level::MINOR_ERROR,
|
|
|
- " Carrier_base::check_status error ");
|
|
|
- }
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-//判断能否执行二级任务
|
|
|
-Error_manager Carrier_base::check_two_level_task()
|
|
|
-{
|
|
|
- if ( m_carrier_status == E_READY ||
|
|
|
- m_carrier_status == E_ONE_LEVEL_WORK ||
|
|
|
- m_carrier_status == E_ONE_LEVEL_OVER )
|
|
|
- {
|
|
|
- return Error_code::SUCCESS;
|
|
|
- }
|
|
|
- else if ( m_carrier_status >= E_TWO_LEVEL_WORK && m_carrier_status <= E_THREE_LEVEL_OVER)
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
|
|
|
- " Carrier_base::check_status is busy ");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_STATUS_ERROR, Error_level::MINOR_ERROR,
|
|
|
- " Carrier_base::check_status error ");
|
|
|
- }
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-//判断能否执行三级任务
|
|
|
-Error_manager Carrier_base::check_three_level_task()
|
|
|
-{
|
|
|
- if ( m_carrier_status == E_READY ||
|
|
|
- m_carrier_status == E_ONE_LEVEL_WORK ||
|
|
|
- m_carrier_status == E_ONE_LEVEL_OVER )
|
|
|
- {
|
|
|
- return Error_code::SUCCESS;
|
|
|
- }
|
|
|
- else if ( m_carrier_status >= E_TWO_LEVEL_WORK && m_carrier_status <= E_THREE_LEVEL_OVER)
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
|
|
|
- " Carrier_base::check_status is busy ");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_STATUS_ERROR, Error_level::MINOR_ERROR,
|
|
|
- " Carrier_base::check_status error ");
|
|
|
- }
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-//结束任务单，里面会根据任务的故障等级修正 任务单的状态
|
|
|
-Error_manager Carrier_base::end_task(std::shared_ptr<Carrier_task> carrier_task)
|
|
|
-{
|
|
|
- LOG(INFO) << " ---Carrier_base::end_task run---"<< this;
|
|
|
-
|
|
|
-// carrier_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status;
|
|
|
-
|
|
|
- //注:这里只修改任务单的状态, 搬运器的状态不管
|
|
|
- //在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
|
|
|
- //判断任务单的错误等级，
|
|
|
- if ( carrier_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
|
|
|
- {
|
|
|
- //强制改为TASK_OVER，不管它当前在做什么。
|
|
|
- carrier_task->set_task_statu(TASK_OVER);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //强制改为 TASK_ERROR，不管它当前在做什么。
|
|
|
- carrier_task->set_task_statu(TASK_ERROR);
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-//取消任务单，由发送方提前取消任务单
|
|
|
-Error_manager Carrier_base::cancel_task(std::shared_ptr<Carrier_task> carrier_task)
|
|
|
-{
|
|
|
- //找到对应的任务单
|
|
|
- if ( carrier_task->get_task_id() == mp_carrier_one_level_task->get_task_id() )
|
|
|
- {
|
|
|
- if ( m_carrier_status == E_ONE_LEVEL_WORK || m_carrier_status == E_ONE_LEVEL_OVER )
|
|
|
- {
|
|
|
- //如果正在执行一级任务, 那么取消当前指令, 然后降级
|
|
|
- m_execute_condition.notify_all(false);
|
|
|
- //确保内部线程已经停下
|
|
|
- while (m_execute_condition.is_working())
|
|
|
- {
|
|
|
-
|
|
|
- }
|
|
|
- cancel_command();
|
|
|
- mp_carrier_one_level_task.reset();
|
|
|
- m_carrier_status = E_READY;
|
|
|
- m_execute_condition.notify_all(true);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //否则直接销毁任务单
|
|
|
- mp_carrier_one_level_task.reset();
|
|
|
- }
|
|
|
- }
|
|
|
- else if ( carrier_task->get_task_id() == mp_carrier_two_level_task->get_task_id() )
|
|
|
- {
|
|
|
- if ( m_carrier_status == E_TWO_LEVEL_WORK || m_carrier_status == E_TWO_LEVEL_OVER )
|
|
|
- {
|
|
|
- //如果正在执行二级任务, 那么取消当前指令, 然后降级
|
|
|
- m_execute_condition.notify_all(false);
|
|
|
- //确保内部线程已经停下
|
|
|
- while (m_execute_condition.is_working())
|
|
|
- {
|
|
|
-
|
|
|
- }
|
|
|
- cancel_command();
|
|
|
- mp_carrier_two_level_task.reset();
|
|
|
- m_carrier_status = E_ONE_LEVEL_WORK;
|
|
|
- m_execute_condition.notify_all(true);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //否则直接销毁任务单
|
|
|
- mp_carrier_two_level_task.reset();
|
|
|
- }
|
|
|
- }
|
|
|
- else if ( carrier_task->get_task_id() == mp_carrier_three_level_task->get_task_id() )
|
|
|
- {
|
|
|
- if ( m_carrier_status == E_THREE_LEVEL_WORK || m_carrier_status == E_THREE_LEVEL_OVER )
|
|
|
- {
|
|
|
- //如果正在执行三级任务, 那么取消当前指令, 然后降级
|
|
|
- m_execute_condition.notify_all(false);
|
|
|
- //确保内部线程已经停下
|
|
|
- while (m_execute_condition.is_working())
|
|
|
- {
|
|
|
-
|
|
|
- }
|
|
|
- cancel_command();
|
|
|
- mp_carrier_three_level_task.reset();
|
|
|
- m_carrier_status = E_TWO_LEVEL_WORK;
|
|
|
- m_execute_condition.notify_all(true);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //否则直接销毁任务单
|
|
|
- mp_carrier_two_level_task.reset();
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- return Error_manager(Error_code::CARRIER_TASK_NOTHINGNESS, Error_level::MINOR_ERROR,
|
|
|
- " carrier_task->get_task_id() is nothingness ");
|
|
|
- }
|
|
|
-
|
|
|
- carrier_task->set_task_statu(TASK_DEAD);
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-//判断是否为待机，如果已经准备好，则可以执行任务。
|
|
|
-bool Carrier_base::is_ready()
|
|
|
-{
|
|
|
- return (m_carrier_status == E_READY);
|
|
|
-}
|
|
|
-
|
|
|
-Carrier_base::Carrier_status Carrier_base::get_carrier_status()
|
|
|
-{
|
|
|
- return m_carrier_status;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-//执行外界任务的执行函数
|
|
|
-void Carrier_base::execute_thread_fun()
|
|
|
-{
|
|
|
- LOG(INFO) << " execute_thread_fun start "<< this;
|
|
|
-
|
|
|
-// while (1);
|
|
|
-
|
|
|
- Error_manager t_error;
|
|
|
-
|
|
|
- while (m_execute_condition.is_alive())
|
|
|
- {
|
|
|
- m_execute_condition.wait();
|
|
|
- if ( m_execute_condition.is_alive() )
|
|
|
- {
|
|
|
- std::this_thread::sleep_for(std::chrono::microseconds(1));
|
|
|
- std::this_thread::yield();
|
|
|
-
|
|
|
- switch ( (Carrier_status)m_carrier_status )
|
|
|
- {
|
|
|
- //核心任务, (三级任务)
|
|
|
- case E_THREE_LEVEL_WORK:
|
|
|
- {
|
|
|
- if ( mp_carrier_three_level_task.get() != NULL )
|
|
|
- {
|
|
|
- mp_carrier_three_level_task->set_task_statu(TASK_WORKING);
|
|
|
- //执行三级任务
|
|
|
- std::unique_lock<std::mutex> t_lock(mp_carrier_three_level_task->m_lock);
|
|
|
- m_request_key = mp_carrier_three_level_task->m_request_key;
|
|
|
- m_request_x = mp_carrier_three_level_task->m_request_x;
|
|
|
- m_request_y = mp_carrier_three_level_task->m_request_y;
|
|
|
- m_request_z = mp_carrier_three_level_task->m_request_z;
|
|
|
- m_request_space_id = mp_carrier_three_level_task->m_request_space_id;
|
|
|
- m_request_y1 = mp_carrier_three_level_task->m_request_y1;
|
|
|
- m_request_y2 = mp_carrier_three_level_task->m_request_y2;
|
|
|
- m_request_wheelbase = mp_carrier_three_level_task->m_request_wheelbase;
|
|
|
- m_request_clamp_motion = (Carrier_base::Clamp_motion)mp_carrier_three_level_task->m_request_clamp_motion;
|
|
|
- m_request_joint_motion = (Carrier_base::Joint_motion)mp_carrier_three_level_task->m_request_joint_motion;
|
|
|
-
|
|
|
- //更新通信
|
|
|
- update_device_communication();
|
|
|
-
|
|
|
- //检查plc的答复状态
|
|
|
- if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
|
|
|
- {
|
|
|
- mp_carrier_three_level_task->m_respons_key = m_respons_key;
|
|
|
- mp_carrier_three_level_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status;
|
|
|
- mp_carrier_three_level_task->m_respons_x = m_respons_x;
|
|
|
- mp_carrier_three_level_task->m_respons_y = m_respons_y;
|
|
|
- mp_carrier_three_level_task->m_respons_z = m_respons_z;
|
|
|
- mp_carrier_three_level_task->m_respons_space_id = m_respons_space_id;
|
|
|
- mp_carrier_three_level_task->m_respons_y1 = m_respons_y1;
|
|
|
- mp_carrier_three_level_task->m_respons_y2 = m_respons_y2;
|
|
|
- mp_carrier_three_level_task->m_respons_wheelbase = m_respons_wheelbase;
|
|
|
- mp_carrier_three_level_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion;
|
|
|
- mp_carrier_three_level_task->m_respons_joint_motion = (Carrier_task::Joint_motion)m_respons_joint_motion;
|
|
|
-
|
|
|
- //如果故障,则添加错误码
|
|
|
- if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_three_level_task->set_task_error_manager(t_error);
|
|
|
- }
|
|
|
-
|
|
|
- end_task(mp_carrier_three_level_task);
|
|
|
- m_carrier_status = E_THREE_LEVEL_OVER;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //设备正常运行
|
|
|
- //延时1ms, snap7的通信效率偏慢, 不需要高频率检查
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
- }
|
|
|
- //else继续等待,直到任务完成
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //直接降级
|
|
|
- m_carrier_status = E_TWO_LEVEL_WORK;
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
- case E_THREE_LEVEL_OVER:
|
|
|
- {
|
|
|
- if ( mp_carrier_three_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //检查任务状态,
|
|
|
- //在 E_THREE_LEVEL_WORK 里面, 已经 设为 TASK_OVER 了.
|
|
|
- //等待发送方的新指令, (发送方会把状态改为回收, 表示这个任务结束)
|
|
|
- if ( mp_carrier_three_level_task->get_task_statu() == TASK_WITHDRAW )
|
|
|
- {
|
|
|
- //这里会通知任务已经释放, 然后销毁任务单, 并降级
|
|
|
- mp_carrier_three_level_task->set_task_statu(TASK_FREE);
|
|
|
- mp_carrier_three_level_task.reset();
|
|
|
- m_carrier_status = E_TWO_LEVEL_WORK;
|
|
|
- }
|
|
|
- //任务单重新创建, 调度创建的新的任务,那么回去继续工作
|
|
|
- else if ( mp_carrier_three_level_task->get_task_statu() == TASK_CREATED )
|
|
|
- {
|
|
|
- mp_carrier_three_level_task->set_task_statu(TASK_SIGNED);
|
|
|
- m_carrier_status = E_THREE_LEVEL_WORK;
|
|
|
- }
|
|
|
- //else //保持不动, 直到发送方给定新的任务,
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //直接降级
|
|
|
- m_carrier_status = E_TWO_LEVEL_WORK;
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
- case E_TWO_LEVEL_WORK:
|
|
|
- {
|
|
|
- if ( mp_carrier_two_level_task.get() != NULL )
|
|
|
- {
|
|
|
- mp_carrier_two_level_task->set_task_statu(TASK_WORKING);
|
|
|
- //执行二级任务
|
|
|
- std::unique_lock<std::mutex> t_lock(mp_carrier_two_level_task->m_lock);
|
|
|
- m_request_key = mp_carrier_two_level_task->m_request_key;
|
|
|
- m_request_x = mp_carrier_two_level_task->m_request_x;
|
|
|
- m_request_y = mp_carrier_two_level_task->m_request_y;
|
|
|
- m_request_z = mp_carrier_two_level_task->m_request_z;
|
|
|
- m_request_space_id = mp_carrier_two_level_task->m_request_space_id;
|
|
|
- m_request_y1 = mp_carrier_two_level_task->m_request_y1;
|
|
|
- m_request_y2 = mp_carrier_two_level_task->m_request_y2;
|
|
|
- m_request_wheelbase = mp_carrier_two_level_task->m_request_wheelbase;
|
|
|
- m_request_clamp_motion = (Carrier_base::Clamp_motion)mp_carrier_two_level_task->m_request_clamp_motion;
|
|
|
- m_request_joint_motion = (Carrier_base::Joint_motion)mp_carrier_two_level_task->m_request_joint_motion;
|
|
|
-
|
|
|
- //更新通信
|
|
|
- update_device_communication();
|
|
|
-
|
|
|
- //检查plc的答复状态
|
|
|
- if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
|
|
|
- {
|
|
|
- mp_carrier_two_level_task->m_respons_key = m_respons_key;
|
|
|
- mp_carrier_two_level_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status;
|
|
|
- mp_carrier_two_level_task->m_respons_x = m_respons_x;
|
|
|
- mp_carrier_two_level_task->m_respons_y = m_respons_y;
|
|
|
- mp_carrier_two_level_task->m_respons_z = m_respons_z;
|
|
|
- mp_carrier_two_level_task->m_respons_space_id = m_respons_space_id;
|
|
|
- mp_carrier_two_level_task->m_respons_y1 = m_respons_y1;
|
|
|
- mp_carrier_two_level_task->m_respons_y2 = m_respons_y2;
|
|
|
- mp_carrier_two_level_task->m_respons_wheelbase = m_respons_wheelbase;
|
|
|
- mp_carrier_two_level_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion;
|
|
|
- mp_carrier_two_level_task->m_respons_joint_motion = (Carrier_task::Joint_motion)m_respons_joint_motion;
|
|
|
-
|
|
|
- //如果故障,则添加错误码
|
|
|
- if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_two_level_task->set_task_error_manager(t_error);
|
|
|
- }
|
|
|
-
|
|
|
- end_task(mp_carrier_two_level_task);
|
|
|
- m_carrier_status = E_THREE_LEVEL_OVER;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //设备正常运行
|
|
|
- //延时1ms, snap7的通信效率偏慢, 不需要高频率检查
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
- }
|
|
|
- //else继续等待,直到任务完成
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //直接降级
|
|
|
- m_carrier_status = E_ONE_LEVEL_WORK;
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
- case E_TWO_LEVEL_OVER:
|
|
|
- {
|
|
|
- if ( mp_carrier_two_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //检查任务状态,
|
|
|
- //在 E_TWO_LEVEL_WORK 里面, 已经 设为 TASK_OVER 了.
|
|
|
- //等待发送方的新指令, (发送方会把状态改为回收, 表示这个任务结束)
|
|
|
- if ( mp_carrier_two_level_task->get_task_statu() == TASK_WITHDRAW )
|
|
|
- {
|
|
|
- //这里会通知任务已经释放, 然后销毁任务单, 并降级
|
|
|
- mp_carrier_two_level_task->set_task_statu(TASK_FREE);
|
|
|
- mp_carrier_two_level_task.reset();
|
|
|
- m_carrier_status = E_ONE_LEVEL_WORK;
|
|
|
- }
|
|
|
- //任务单重新创建, 调度创建的新的任务,那么回去继续工作
|
|
|
- else if ( mp_carrier_two_level_task->get_task_statu() == TASK_CREATED )
|
|
|
- {
|
|
|
- mp_carrier_two_level_task->set_task_statu(TASK_SIGNED);
|
|
|
- m_carrier_status = E_TWO_LEVEL_WORK;
|
|
|
- }
|
|
|
- //else //保持不动, 直到发送方给定新的任务,
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //直接降级
|
|
|
- m_carrier_status = E_ONE_LEVEL_WORK;
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
- case E_ONE_LEVEL_WORK:
|
|
|
- {
|
|
|
- if ( mp_carrier_one_level_task.get() != NULL )
|
|
|
- {
|
|
|
- mp_carrier_one_level_task->set_task_statu(TASK_WORKING);
|
|
|
- //执行一级任务,
|
|
|
- std::unique_lock<std::mutex> t_lock(mp_carrier_one_level_task->m_lock);
|
|
|
- m_request_key = mp_carrier_one_level_task->m_request_key;
|
|
|
- m_request_x = mp_carrier_one_level_task->m_request_x;
|
|
|
- m_request_y = mp_carrier_one_level_task->m_request_y;
|
|
|
- m_request_z = mp_carrier_one_level_task->m_request_z;
|
|
|
- m_request_space_id = mp_carrier_one_level_task->m_request_space_id;
|
|
|
- m_request_y1 = mp_carrier_one_level_task->m_request_y1;
|
|
|
- m_request_y2 = mp_carrier_one_level_task->m_request_y2;
|
|
|
- m_request_wheelbase = mp_carrier_one_level_task->m_request_wheelbase;
|
|
|
- m_request_clamp_motion = (Carrier_base::Clamp_motion)mp_carrier_one_level_task->m_request_clamp_motion;
|
|
|
- m_request_joint_motion = (Carrier_base::Joint_motion)mp_carrier_one_level_task->m_request_joint_motion;
|
|
|
-
|
|
|
- //更新通信
|
|
|
- update_device_communication();
|
|
|
-
|
|
|
- //检查plc的答复状态
|
|
|
- if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
|
|
|
- {
|
|
|
- mp_carrier_one_level_task->m_respons_x = m_respons_x;
|
|
|
- mp_carrier_one_level_task->m_respons_y = m_respons_y;
|
|
|
- mp_carrier_one_level_task->m_respons_z = m_respons_z;
|
|
|
- mp_carrier_one_level_task->m_respons_space_id = m_respons_space_id;
|
|
|
- mp_carrier_one_level_task->m_respons_y1 = m_respons_y1;
|
|
|
- mp_carrier_one_level_task->m_respons_y2 = m_respons_y2;
|
|
|
- mp_carrier_one_level_task->m_respons_wheelbase = m_respons_wheelbase;
|
|
|
- mp_carrier_one_level_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion;
|
|
|
- mp_carrier_one_level_task->m_respons_joint_motion = (Carrier_task::Joint_motion)m_respons_joint_motion;
|
|
|
-
|
|
|
- //如果故障,则添加错误码
|
|
|
- if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_one_level_task->set_task_error_manager(t_error);
|
|
|
- }
|
|
|
-
|
|
|
- end_task(mp_carrier_one_level_task);
|
|
|
- m_carrier_status = E_ONE_LEVEL_OVER;
|
|
|
-
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //设备正常运行
|
|
|
- //延时1ms, snap7的通信效率偏慢, 不需要高频率检查
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
- }
|
|
|
- //else继续等待,直到任务完成
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //直接降级
|
|
|
- m_carrier_status = E_READY;
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
- case E_ONE_LEVEL_OVER:
|
|
|
- {
|
|
|
- if ( mp_carrier_one_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //检查任务状态,
|
|
|
- //在 E_ONE_LEVEL_WORK 里面, 已经 设为 TASK_OVER 了.
|
|
|
- //等待发送方的新指令, (发送方会把状态改为回收, 表示这个任务结束)
|
|
|
- if ( mp_carrier_one_level_task->get_task_statu() == TASK_WITHDRAW )
|
|
|
- {
|
|
|
- //这里会通知任务已经释放, 然后销毁任务单, 并降级
|
|
|
- mp_carrier_one_level_task->set_task_statu(TASK_FREE);
|
|
|
- mp_carrier_one_level_task.reset();
|
|
|
- m_carrier_status = E_READY;
|
|
|
- }
|
|
|
- //任务单重新创建, 调度创建的新的任务,那么回去继续工作
|
|
|
- else if ( mp_carrier_one_level_task->get_task_statu() == TASK_CREATED )
|
|
|
- {
|
|
|
- mp_carrier_one_level_task->set_task_statu(TASK_SIGNED);
|
|
|
- m_carrier_status = E_ONE_LEVEL_WORK;
|
|
|
- }
|
|
|
- //else //保持不动, 直到发送方给定新的任务,
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //直接降级
|
|
|
- m_carrier_status = E_READY;
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
- case E_FAULT:
|
|
|
- {
|
|
|
- //更新通信
|
|
|
- update_device_communication();
|
|
|
-
|
|
|
- //所有任务报错, 并销毁任务.
|
|
|
- if ( mp_carrier_one_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_STATUS_ERROR, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_one_level_task->set_task_error_manager(t_error);
|
|
|
- end_task(mp_carrier_one_level_task);
|
|
|
- mp_carrier_one_level_task.reset();
|
|
|
- }
|
|
|
- if ( mp_carrier_two_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_STATUS_ERROR, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_two_level_task->set_task_error_manager(t_error);
|
|
|
- end_task(mp_carrier_two_level_task);
|
|
|
- mp_carrier_two_level_task.reset();
|
|
|
- }
|
|
|
- if ( mp_carrier_three_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_STATUS_ERROR, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_three_level_task->set_task_error_manager(t_error);
|
|
|
- end_task(mp_carrier_three_level_task);
|
|
|
- mp_carrier_three_level_task.reset();
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- break;
|
|
|
- }
|
|
|
- case E_DISCONNECT:
|
|
|
- {
|
|
|
- //更新通信
|
|
|
- update_device_communication();
|
|
|
- //所有任务报错, 并销毁任务.
|
|
|
- if ( mp_carrier_one_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_STATUS_DISCONNECT, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_one_level_task->set_task_error_manager(t_error);
|
|
|
- end_task(mp_carrier_one_level_task);
|
|
|
- mp_carrier_one_level_task.reset();
|
|
|
- }
|
|
|
- if ( mp_carrier_two_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_STATUS_DISCONNECT, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_two_level_task->set_task_error_manager(t_error);
|
|
|
- end_task(mp_carrier_two_level_task);
|
|
|
- mp_carrier_two_level_task.reset();
|
|
|
- }
|
|
|
- if ( mp_carrier_three_level_task.get() != NULL )
|
|
|
- {
|
|
|
- //添加错误码
|
|
|
- Error_manager t_error(CARRIER_STATUS_DISCONNECT, MINOR_ERROR, "m_respons_status is error");
|
|
|
- mp_carrier_three_level_task->set_task_error_manager(t_error);
|
|
|
- end_task(mp_carrier_three_level_task);
|
|
|
- mp_carrier_three_level_task.reset();
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- break;
|
|
|
- }
|
|
|
- case E_READY:
|
|
|
- {
|
|
|
- //更新通信
|
|
|
- update_device_communication();
|
|
|
- break;
|
|
|
- }
|
|
|
- default:
|
|
|
- {
|
|
|
-
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- }
|
|
|
- }
|
|
|
- LOG(INFO) << " execute_thread_fun end "<< this;
|
|
|
- return;
|
|
|
-}
|
|
|
-
|
|
|
-//执行线程工作函数, 正在执行任务单.
|
|
|
-Error_manager Carrier_base::execute_thread_working(std::shared_ptr<Carrier_task> p_carrier_task, Carrier_status & carrier_status)
|
|
|
-{
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-//执行线程工作函数, 已经完成任务单. 等待新的指令
|
|
|
-Error_manager Carrier_base::execute_thread_over(std::shared_ptr<Carrier_task> p_carrier_task, Carrier_status & carrier_status)
|
|
|
-{
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-//更新设备底层通信数据
|
|
|
-Error_manager Carrier_base::update_device_communication()
|
|
|
-{
|
|
|
-
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-//取消下发的指令
|
|
|
-Error_manager Carrier_base::cancel_command()
|
|
|
-{
|
|
|
- //以后再写 need programe
|
|
|
- //目前调度和plc的通信指令做的很简单,没有暂停和急停 复位等操作.
|
|
|
- //这里先空着,以后再写.
|
|
|
- //调度模块单方面销毁任务, 不管底层plc的执行情况, 也不去告知plc任务取消.
|
|
|
-
|
|
|
- return Error_code::SUCCESS;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-*/
|
|
|
-
|
