// // Created by huli on 2021/3/22. // #include "dispatch_process.h" #include "../system/system_communication.h" #include "../dispatch/dispatch_manager.h" Dispatch_process::Dispatch_process() { m_dispatch_process_status = DISPATCH_PROCESS_STATUS_UNKNOW; m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW; m_dispatch_source = 0; m_dispatch_destination = 0; m_wheel_base = 0; } Dispatch_process::~Dispatch_process() { Dispatch_process_uninit(); } //初始化, 就把主控发送的请求传入即可. Error_manager Dispatch_process::Dispatch_process_init(message::Dispatch_request_msg &dispatch_request_msg) { //设定超时时间, 默认比任务指令里面的时间少10秒, if ( dispatch_request_msg.base_info().has_timeout_ms() ) { m_timeout_ms = dispatch_request_msg.base_info().timeout_ms() - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS; } else { m_timeout_ms = DISPATCH_PROCESS_TIMEOUT_MS - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS; } m_command_key = dispatch_request_msg.command_key(); m_start_time = std::chrono::system_clock::now(); //检查调度请求消息 if ( dispatch_request_msg.terminal_id() < Dispatch_coordinates::get_instance_references().m_passageway_terminal_id_min || dispatch_request_msg.terminal_id() > Dispatch_coordinates::get_instance_references().m_passageway_terminal_id_max) { return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR, " dispatch_request_msg.terminal_id() is error "); } //解析调度请求消息 if ( dispatch_request_msg.dispatch_motion_direction() == message::E_STORE_CAR ) { //检查调度请求消息 if ( Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_INLET && Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_BIDIRECTION ) { return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR, " m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] is error "); } //存车的车位可以是1~3个 if ( dispatch_request_msg.parkspace_info_ex_size() <=0 || dispatch_request_msg.parkspace_info_ex_size() >3 ) { return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR, " dispatch_request_msg.parkspace_info_ex_size() is error "); } //检查定位信息 if ( dispatch_request_msg.has_locate_information() == false ) { return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR, " dispatch_request_msg.has_locate_information() is error "); } m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE; m_dispatch_source = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ; //终点在运动过程中动态分配//存车需要在完全确定车位之后, 才能判断车位类型 // m_dispatch_destination = dispatch_request_msg.parkspace_info().parkingspace_index_id() + PARKSPACE_ID_BASE; for (int i = 0; i < dispatch_request_msg.parkspace_info_ex_size(); ++i) { if(dispatch_request_msg.parkspace_info_ex(i).parkspace_path() == message::Parkspace_path::LEFT_PATH) { Common_data::copy_data(m_parkspace_information_left, dispatch_request_msg.parkspace_info_ex(i)); Common_data::scaling(m_parkspace_information_left, 1000); } if(dispatch_request_msg.parkspace_info_ex(i).parkspace_path() == message::Parkspace_path::RIGHT_PATH) { Common_data::copy_data(m_parkspace_information_right, dispatch_request_msg.parkspace_info_ex(i)); Common_data::scaling(m_parkspace_information_right, 1000); } if(dispatch_request_msg.parkspace_info_ex(i).parkspace_path() == message::Parkspace_path::TEMPORARY_CACHE_PATH) { Common_data::copy_data(m_parkspace_information_temporary, dispatch_request_msg.parkspace_info_ex(i)); Common_data::scaling(m_parkspace_information_temporary, 1000); } } // Common_data::copy_data(m_parkspace_information, dispatch_request_msg.parkspace_info()); // Common_data::scaling(m_parkspace_information, 1000); Common_data::copy_data(m_car_measure_information, dispatch_request_msg.locate_information()); Common_data::scaling(m_car_measure_information, 1000); m_wheel_base = m_car_measure_information.car_wheel_base; } else if( dispatch_request_msg.dispatch_motion_direction() == message::E_PICKUP_CAR ) { //检查调度请求消息 if ( Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_OUTLET && Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_BIDIRECTION ) { return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR, " m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] is error "); } //取车的车位必须是1个 if ( dispatch_request_msg.parkspace_info_ex_size() != 1 ) { return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR, " dispatch_request_msg.parkspace_info_ex_size() is error "); } m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP; m_dispatch_source = dispatch_request_msg.parkspace_info_ex(0).parkingspace_index_id() + PARKSPACE_ID_BASE; m_parkspace_type = (Common_data::Parkspace_type)( dispatch_request_msg.parkspace_info_ex(0).parkingspace_type() ); //终点在运动过程中动态分配 //目前不控制门, 所以出口有主控决定. 如果后续控制门, 那么出口可以由调度决定. //目前取车还是固定出口. m_dispatch_destination = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ; Common_data::copy_data(m_parkspace_information_optimal, dispatch_request_msg.parkspace_info_ex(0)); Common_data::scaling(m_parkspace_information_optimal, 1000); m_wheel_base = m_parkspace_information_optimal.car_information.car_wheel_base; } else { m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW; return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR, " Dispatch_process::Dispatch_process_init ERROR "); } //轮距 if ( m_wheel_base < Dispatch_coordinates::get_instance_references().m_car_wheel_base_min || m_wheel_base > Dispatch_coordinates::get_instance_references().m_car_wheel_base_max) { return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR, " m_wheel_base < 1000 ERROR "); } m_car_type = (Common_data::Car_type)( dispatch_request_msg.car_type() ); m_temporary_cache_flag = false; m_dispatch_request_msg = dispatch_request_msg; m_dispatch_process_status = DISPATCH_PROCESS_CREATED; return Error_code::SUCCESS; } //反初始化 Error_manager Dispatch_process::Dispatch_process_uninit() { std::unique_lock t_lock(m_lock); for (auto iter = m_dispatch_control_node_map.begin(); iter != m_dispatch_control_node_map.end(); ++iter) { if ( iter->second.mp_dispatch_task.get() != NULL ) { iter->second.mp_dispatch_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW); iter->second.mp_dispatch_task.reset(); iter->second.mp_dispatch_device.reset(); } } return Error_code::SUCCESS; } //检查流程是否空闲待机 Error_manager Dispatch_process::check_process_ready() { return Error_code::SUCCESS; if ( m_dispatch_process_status == DISPATCH_PROCESS_READY ) { return Error_code::SUCCESS; } else { return Error_manager(Error_code::DISPATCH_PROCESS_IS_NOT_READY, Error_level::MINOR_ERROR, " Dispatch_process::check_process_ready() fun error "); } } //判断是否为待机， bool Dispatch_process::is_ready() { // std::unique_lock t_lock(m_lock); if ( m_dispatch_process_status == DISPATCH_PROCESS_READY && m_dispatch_carrier_node.mp_main_carrier.get() == NULL) { return true; } else { return false; } } //能否在左侧路径存车 bool Dispatch_process::is_able_store_left_path() { if ( m_parkspace_information_left.parkingspace_index_id !=0 ) { return true; } return false; } //能否在右侧路径存车 bool Dispatch_process::is_able_store_right_path() { if ( m_parkspace_information_right.parkingspace_index_id !=0 ) { return true; } return false; } //能否在3楼路径存车 bool Dispatch_process::is_able_store_temporary_cache_path() { if ( m_parkspace_information_temporary.parkingspace_index_id !=0 ) { return true; } return false; } //能否在临时缓存位存车 bool Dispatch_process::is_able_store_temporary_cache_path_2nd_floor() { if ( m_parkspace_information_temporary.parkingspace_index_id >= 4 && m_parkspace_information_temporary.parkingspace_index_id <= 12 ) { return true; } return false; } //能否在临时缓存位存车 bool Dispatch_process::is_able_store_temporary_cache_path_3rd_floor() { if ( m_parkspace_information_temporary.parkingspace_index_id >= 19 && m_parkspace_information_temporary.parkingspace_index_id <= 27 ) { return true; } return false; } void Dispatch_process::set_main_carrier(std::shared_ptr p_carrier) { std::unique_lock t_lock(m_lock); m_dispatch_carrier_node.mp_main_carrier = p_carrier; } std::string Dispatch_process::get_dispatch_control_command_key(Dispatch_catcher_node & dispatch_catcher_node) { char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", m_command_key.c_str(), m_dispatch_process_type, dispatch_catcher_node.m_dispatch_control_status); std::string t_str = t_key; return t_str; } std::string Dispatch_process::get_dispatch_control_command_key(Dispatch_carrier_node & dispatch_carrier_node) { char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", m_command_key.c_str(), m_dispatch_process_type, dispatch_carrier_node.m_dispatch_control_status); std::string t_str = t_key; return t_str; } void Dispatch_process::Main() { Error_manager t_error; //主流程, 循环执行 while ( std::chrono::system_clock::now() - m_start_time < std::chrono::milliseconds(m_timeout_ms) ) { std::this_thread::sleep_for(std::chrono::microseconds(1)); std::this_thread::sleep_for(std::chrono::milliseconds(1)); // std::this_thread::sleep_for(std::chrono::milliseconds(100)); #ifdef TIME_TEST if ( mcarrier == m_dispatch_carrier_node.m_dispatch_control_status && mcatcher == m_dispatch_catcher_node.m_dispatch_control_status ) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); // std::this_thread::sleep_for(std::chrono::seconds(1)); Dispatch_coordinates::get_instance_references().cout_space_lock(); } else { mcarrier = m_dispatch_carrier_node.m_dispatch_control_status; mcatcher = m_dispatch_catcher_node.m_dispatch_control_status; } #endif std::cout << " huli test ::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: " << " m_dispatch_process_status = " << m_dispatch_process_status << " , m_command_key = " << m_command_key << std::endl; // std::cout << " huli test :::: " << " Dispatch_manager::get_instance_references().m_carrier_map[2]->m_device_one_level_command_key = " << Dispatch_manager::get_instance_references().m_carrier_map[2]->m_device_one_level_command_key << std::endl; // std::cout << " huli test :::: " << " Dispatch_manager::get_instance_references().m_carrier_map[2]->m_device_two_level_command_key = " << Dispatch_manager::get_instance_references().m_carrier_map[2]->m_device_two_level_command_key << std::endl; // std::cout << " huli test :::: " << " Dispatch_manager::get_instance_references().m_carrier_map[2]->m_device_three_level_command_key = " << Dispatch_manager::get_instance_references().m_carrier_map[2]->m_device_three_level_command_key << std::endl; switch ( (Dispatch_process_status)m_dispatch_process_status ) { case DISPATCH_PROCESS_CREATED://流程创建, { //检查调度请求 m_result = check_dispatch_request_msg(); if ( m_result !=Error_code::SUCCESS) { m_dispatch_process_status = DISPATCH_PROCESS_FAULT; break; } //发送调度总计划 // m_result = send_dispatch_plan_request_msg(); // if ( m_result !=Error_code::SUCCESS) // { // m_dispatch_process_status = DISPATCH_PROCESS_FAULT; // break; // } //流程正常, 就进入等待状态, 等待调度控制发送动作指令 m_dispatch_process_status = DISPATCH_PROCESS_READY; break; } case DISPATCH_PROCESS_READY://流程准备，待机 { m_result = wait_resource_allocation(); if ( m_result == Error_code::SUCCESS) { if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP ) { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY; m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY; m_dispatch_process_status = DISPATCH_PROCESS_WORKING_PICKUP; } else if(m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE) { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY; m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY; m_dispatch_process_status = DISPATCH_PROCESS_WORKING_STORE; //根据设备选择, 来设置终点 if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2 ) { m_parkspace_information_optimal = m_parkspace_information_temporary; } else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 0 && m_temporary_cache_flag == false ) { m_parkspace_information_optimal = m_parkspace_information_left; } else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 0 && m_temporary_cache_flag == true) { m_parkspace_information_optimal = m_parkspace_information_temporary; } else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 1 && m_temporary_cache_flag == false ) { m_parkspace_information_optimal = m_parkspace_information_right; } else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 1 && m_temporary_cache_flag == true) { m_parkspace_information_optimal = m_parkspace_information_temporary; } } } //else 一直等待资源分配 break; //调度控制, 并根据完成情况给答复 // dispatch_control_motion(); // // //等待调度总计划答复 // m_result = wait_dispatch_plan_response_msg(); // if ( m_result ==Error_code::SUCCESS) // { // //流程正常, 就进入完成状态, // m_dispatch_process_status = DISPATCH_PROCESS_OVER; // break; // } // else if ( m_result !=Error_code::NODATA ) // { // m_dispatch_process_status = DISPATCH_PROCESS_FAULT; // break; // } //else nodata, 就表示没有新的指令, 那么什么都不做, 原地待命 break; } case DISPATCH_PROCESS_WORKING_PICKUP://流程完成 { //调度控制取车 m_result = dispatch_control_motion_pickup(); if ( m_result ==Error_code::SUCCESS) { //流程正常, 就进入完成状态, m_dispatch_process_status = DISPATCH_PROCESS_OVER; break; } else if ( m_result !=Error_code::NODATA ) { m_dispatch_process_status = DISPATCH_PROCESS_FAULT; break; } //else nodata, 就表示没有新的指令, 那么什么都不做, 原地待命 break; } case DISPATCH_PROCESS_WORKING_STORE://流程完成 { //调度控制存车 m_result = dispatch_control_motion_store(); if ( m_result ==Error_code::SUCCESS) { //流程正常, 就进入完成状态, m_dispatch_process_status = DISPATCH_PROCESS_OVER; break; } else if ( m_result !=Error_code::NODATA ) { m_dispatch_process_status = DISPATCH_PROCESS_FAULT; break; } //else nodata, 就表示没有新的指令, 那么什么都不做, 原地待命 break; } case DISPATCH_PROCESS_OVER://流程完成 { //发送调度答复, 发给主控的 m_result = send_dispatch_response_msg(); if ( m_result !=Error_code::SUCCESS) { m_dispatch_process_status = DISPATCH_PROCESS_FAULT; break; } //流程正常, 就进入等待状态, 等待调度控制发送动作指令 m_dispatch_process_status = DISPATCH_PROCESS_RELEASE; break; } case DISPATCH_PROCESS_RELEASE://流程释放 { //通知调度管理, 释放资源, m_result = release_resource(); if ( m_result !=Error_code::SUCCESS) { m_dispatch_process_status = DISPATCH_PROCESS_FAULT; break; } //在这里, 整个流程彻底结束, 之后线程池会自动回收 这个流程对象的资源 return; break; } case DISPATCH_PROCESS_FAULT://故障 { LOG(INFO) << " m_result = "<< m_result << " " << this; release_resource(); Exception_handling(); //在这里, 整个流程彻底结束, 之后线程池会自动回收 这个流程对象的资源 return; break; } default: { break; } } } //任务超时 LOG(INFO) << " Dispatch_process::Main() time out "<< this; return; } //检查调度请求 Error_manager Dispatch_process::check_dispatch_request_msg() { // std::unique_lock t_lock(m_lock); return Error_code::SUCCESS; } //等待资源分配 Error_manager Dispatch_process::wait_resource_allocation() { //只判断搬运器的指针是否有效. if ( m_dispatch_carrier_node.mp_main_carrier.get() == NULL ) { return Error_code::NODATA; } else { return Error_code::SUCCESS; } } //调度控制取车 Error_manager Dispatch_process::dispatch_control_motion_pickup() { std::unique_lock t_lock(m_lock); Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); Error_manager t_carrier_error; Carrier * tp_main_carrier = NULL; Carrier_task * tp_main_carrier_task = NULL; if ( m_dispatch_carrier_node.mp_main_carrier.get() != NULL && m_dispatch_carrier_node.mp_main_carrier_task.get() != NULL ) { tp_main_carrier = (Carrier *)m_dispatch_carrier_node.mp_main_carrier.get(); tp_main_carrier_task = (Carrier_task *)m_dispatch_carrier_node.mp_main_carrier_task.get(); //一级任务被打断后, 重新执行 if ( tp_main_carrier->get_break_flag() ) { if ( m_dispatch_carrier_node.m_main_carrier_task_level == Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL ) { m_dispatch_carrier_node.m_dispatch_control_status = DISPATCH_CARRIER_PICKUP_START; } tp_main_carrier->set_break_flag(false); } } Catcher * tp_avoid_catcher = NULL; Catcher_task * tp_avoid_catcher_task = NULL; if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL ) { tp_avoid_catcher = (Catcher *)m_dispatch_carrier_node.mp_avoid_catcher.get(); tp_avoid_catcher_task = (Catcher_task *)m_dispatch_carrier_node.mp_avoid_catcher_task.get(); } Carrier * tp_avoid_carrier = NULL; Carrier_task * tp_avoid_carrier_task = NULL; if ( m_dispatch_carrier_node.mp_avoid_carrier.get() != NULL && m_dispatch_carrier_node.mp_avoid_carrier_task.get() != NULL ) { tp_avoid_carrier = (Carrier *)m_dispatch_carrier_node.mp_avoid_carrier.get(); tp_avoid_carrier_task = (Carrier_task *)m_dispatch_carrier_node.mp_avoid_carrier_task.get(); } std::cout << " huli test :::: " << " ===================================================================================================================== = " << "carrier start" << std::endl; std::cout << " huli test :::: " << " m_dispatch_carrier_node.m_dispatch_control_status = " << m_dispatch_carrier_node.m_dispatch_control_status << std::endl; if ( m_dispatch_carrier_node.mp_main_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_main_carrier->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_main_carrier->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_main_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL ) { std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_catcher->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_avoid_catcher->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_avoid_catcher->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_carrier_node.mp_avoid_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_avoid_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_carrier->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_avoid_carrier->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_avoid_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_avoid_carrier->get_device_id() = " << -8888 << std::endl; } //搬运器的控制动作 switch ( m_dispatch_carrier_node.m_dispatch_control_status ) { case DISPATCH_CONTROL_CREATED: case DISPATCH_CONTROL_READY://等待调度管理进行资源分配 { m_dispatch_carrier_node.m_error = check_main_carrier(m_dispatch_carrier_node); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP ) { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_START; } else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE ) { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_START; } } //else 原地等待 break; } case DISPATCH_CARRIER_PICKUP_START://连接搬运器, 创建新的任务单 与设备建立连接 { //注意了:3楼搬运器必须要和机器人联动, 所以先通知机器人连接 if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2 ) { //通知机器人提前开始 m_dispatch_catcher_node.m_dispatch_control_start_flag = true; m_dispatch_catcher_node.m_following_flag = false; //等待机器人正常连接之后, 搬运器才正式开始 if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_2 ) { if ( m_dispatch_carrier_node.mp_main_carrier_task.get() == NULL ) { //连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理 m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = connect_dispatch_carrier(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_main_carrier_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL; m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } } //else 无限等待 } else { if ( m_dispatch_carrier_node.mp_main_carrier_task.get() == NULL ) { //连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理 m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = connect_dispatch_carrier(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_main_carrier_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL; m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } } break; } case DISPATCH_CARRIER_PICKUP_1: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_2://搬运器 准备开始, 需要同步任务单和设备真实数据 { //注意了:3楼搬运器必须要和机器人联动, 所以必须等机器人连接上之后,搬运器才开始启动 if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2 ) { //等待机器人正常连接之后, 搬运器才正式开始 if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_2 ) { //搬运器 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步 carrier_ready_to_start(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } else { //搬运器 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步 carrier_ready_to_start(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_3: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); //注意了:同步失败,需要重新同步. if ( m_dispatch_carrier_node.m_error != Error_code::SUCCESS ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_2); } break; } case DISPATCH_CARRIER_PICKUP_4://搬运器检查姿态 //搬运器调整到 正常待机的姿态(调节夹杆和轴距) { if ( tp_main_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_main_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE && Common_data::approximate_difference(m_wheel_base, tp_main_carrier->m_actual_y1-tp_main_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE)) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { //搬运器调整到 正常待机的姿态(调节夹杆和轴距) carrier_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_4 tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error "); } break; } case DISPATCH_CARRIER_PICKUP_5: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_6://让小跑车回到中跑车上, y轴移动 { if ( Common_data::approximate_difference(tp_main_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_7: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_8://让中跑车回到电梯井, 进行x轴路径检查 { float t_z = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].z; if ( Common_data::approximate_difference(tp_main_carrier->m_actual_z, t_z, DISPATCH_DEFAULT_DIFFERENCE) ) { //直接到 20步 , 准备同层移动到车位 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_20); if ( tp_main_carrier->get_device_id() == 2 ) { //3楼的搬运器直接到 22步 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_22); } } else { //如果是3楼, 直接报错, 因为3楼搬运器只能在3楼移动, z轴真实值和目标值 一定是相同的 if ( tp_main_carrier->get_device_id() !=0 && tp_main_carrier->get_device_id() !=1 ) { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_8 tp_main_carrier->get_device_id() PARAMRTER ERROR "); } else { int t_avoid_catcher_id = -1; //需要避让的机器人 //空间锁判断, 是否需要机器人避让 if ( tp_main_carrier->get_device_id() == 0 ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 1 ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //成功则直接跳转, 不用避让, 直接控制搬运器直到电梯井 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_14); } else if(t_avoid_catcher_id != -1) { if ( m_dispatch_carrier_node.mp_avoid_catcher.get() == NULL ) { m_dispatch_carrier_node.mp_avoid_catcher = Dispatch_manager::get_instance_references().m_catcher_map[t_avoid_catcher_id]; } if ( m_dispatch_carrier_node.mp_avoid_catcher_task.get() == NULL ) { //连接避让设备 使得避让机器人进行避让 m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_avoid_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_8 carrier_try_space_lock fun ERROR "); } } } break; } case DISPATCH_CARRIER_PICKUP_9: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_10://机器人避让到最高点, 机器人上升不需要判断空间锁 { catcher_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_11://机器人释放 空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_avoid_catcher->m_actual_coordinates_id, tp_avoid_catcher->get_device_id()); } break; } case DISPATCH_CARRIER_PICKUP_12://搬运器空间锁判断, 必须成功 { int t_avoid_catcher_id = -1; //需要避让的机器人 if ( tp_main_carrier->get_device_id() == 0 ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 1 ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } break; } case DISPATCH_CARRIER_PICKUP_13://避让机器人 断连 { if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL ) { disconnect_dispatch_device(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task); } m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_14://让中跑车回到电梯井, x轴移动 { if ( tp_main_carrier->get_device_id() == 0 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 1 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_14 tp_main_carrier->get_device_id() PARAMRTER ERROR "); } break; } case DISPATCH_CARRIER_PICKUP_15://搬运器 释放空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id()); } break; } case DISPATCH_CARRIER_PICKUP_16://中跑车收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_17: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_18: //电梯移动到对应的楼层, 搬运器在电梯里面z轴移动, 不需要判断空间锁 { float t_z = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].z; carrier_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_z); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_19: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_20://伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_main_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK ) { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } break; } case DISPATCH_CARRIER_PICKUP_21: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_22://中跑车 x轴移动到车位, 进行x轴路径检查 { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x; int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns; if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) ) { //搬运器不用x轴运动, 直接到29步, 去车位取车 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_30); } else { int t_avoid_catcher_id = -1; //需要避让的机器人 if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, t_column+1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 2 ) { //3楼搬运器不用加锁, 直接x轴移动 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_28); break;//切换流程 } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_22 tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR "); break;//切换流程 } //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //左右搬运器加锁成功, 直接x轴移动 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_28); } else if(t_avoid_catcher_id != -1 && (tp_main_carrier->m_actual_coordinates_rows == 2 || tp_main_carrier->m_actual_coordinates_rows == 1))//4楼取车, 机器人进行避让 { if ( m_dispatch_carrier_node.mp_avoid_catcher.get() == NULL ) { m_dispatch_carrier_node.mp_avoid_catcher = Dispatch_manager::get_instance_references().m_catcher_map[t_avoid_catcher_id]; } if ( m_dispatch_carrier_node.mp_avoid_carrier.get() == NULL ) { m_dispatch_carrier_node.mp_avoid_carrier = Dispatch_manager::get_instance_references().m_carrier_map[2]; } //连接4楼机器人和3楼搬运器 Error_manager t_catcher_error; Error_manager t_carrier_error; if ( m_dispatch_carrier_node.mp_avoid_catcher_task.get() == NULL && m_dispatch_carrier_node.mp_avoid_carrier_task.get() == NULL) { //连接避让设备 使得避让机器人进行避让 t_catcher_error = m_dispatch_carrier_node.mp_avoid_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); t_carrier_error = m_dispatch_carrier_node.mp_avoid_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); if ( t_catcher_error == Error_code::SUCCESS && t_carrier_error == Error_code::SUCCESS) { t_catcher_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); if ( t_catcher_error == Error_code::SUCCESS ) { t_carrier_error = connect_dispatch_carrier(m_dispatch_carrier_node.mp_avoid_carrier, m_dispatch_carrier_node.mp_avoid_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); if ( t_carrier_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_22 connect_dispatch_carrier ERROR "); break;//切换流程 } } } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } else if ( m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL && m_dispatch_carrier_node.mp_avoid_carrier_task.get() != NULL ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_22 m_dispatch_carrier_node.mp_avoid_catcher_task.get() m_dispatch_carrier_node.mp_avoid_carrier_task.get() ERROR "); break;//切换流程 } } //else 无限等待 } break; } case DISPATCH_CARRIER_PICKUP_23: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_24://让4楼机器人进行避让, 先加锁, 在避让 { //机器人避让距离, 4楼避让4个车位, 3楼避让2个车位 int m_avoid_distance = 0; int t_row = (m_parkspace_information_optimal.parkingspace_index_id-1) / tp_dispatch_coordinates->m_space_lock_columns; if ( t_row == 1 ) { m_avoid_distance = 3; } else { m_avoid_distance = 3; } int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns; //4楼搬运器需要避让4个车位 if ( tp_main_carrier->get_device_id() == 0 && tp_avoid_catcher->m_actual_coordinates_columns < t_column+m_avoid_distance ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, tp_avoid_catcher->m_actual_coordinates_columns, 2, t_column+m_avoid_distance+1, tp_avoid_catcher->get_device_id()); //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //机器人移到42号口 float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id+m_avoid_distance].x; catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, t_x); carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_carrier, tp_avoid_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } else if ( tp_main_carrier->get_device_id() == 1 && tp_avoid_catcher->m_actual_coordinates_columns > t_column-m_avoid_distance) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, t_column-m_avoid_distance-1, 2, tp_avoid_catcher->m_actual_coordinates_columns, tp_avoid_catcher->get_device_id()); //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //机器人移到34号口 float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id-m_avoid_distance].x; catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, t_x); carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_carrier, tp_avoid_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_25://机器人释放 空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task); m_dispatch_carrier_node.m_error.compare_and_cover_error(check_task_status(m_dispatch_carrier_node.mp_avoid_carrier_task)); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_avoid_catcher->m_actual_coordinates_id, tp_avoid_catcher->get_device_id()); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_26://搬运器空间锁判断, 必须成功 { int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns; int t_avoid_catcher_id = -1; //需要避让的机器人 if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, t_column+1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } break; } case DISPATCH_CARRIER_PICKUP_27://避让机器人 断连 { if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL ) { //避让机器人 断开连接 disconnect_dispatch_device(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task); } if ( m_dispatch_carrier_node.mp_avoid_carrier.get() != NULL && m_dispatch_carrier_node.mp_avoid_carrier_task.get() != NULL ) { //避让机器人 断开连接 disconnect_dispatch_device(m_dispatch_carrier_node.mp_avoid_carrier, m_dispatch_carrier_node.mp_avoid_carrier_task); } m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_28://搬运器x轴移动到车位 { //3楼交接时, 机器人和3楼搬运器一起运动 if ( tp_main_carrier->get_device_id() == 2 ) { if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_13 ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } else { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_29: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_30://把任务从一级升到三级, 准备取车 { //3楼交接时, 机器人先升级, 搬运器再升级 if ( tp_main_carrier->get_device_id() == 2 ) { if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_16 ) { m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_main_carrier_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL; m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); if ( m_dispatch_destination != 1107 && m_dispatch_destination != 1100 ) { //如不是7号出口, 那么开启 抓取机器人 //注意了, 取车流程只有在这里, 抓取机器人的流程才开始 m_dispatch_catcher_node.m_dispatch_control_start_flag = true; if ( tp_main_carrier->get_device_id() != 2 ) { m_dispatch_catcher_node.m_following_flag = true; } else { m_dispatch_catcher_node.m_following_flag = false; } } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } //else 无限等待 } else { m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_main_carrier_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL; m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); if ( m_dispatch_destination != 1107 && m_dispatch_destination != 1100 ) { //如不是7号出口, 那么开启 抓取机器人 //注意了, 取车流程只有在这里, 抓取机器人的流程才开始 m_dispatch_catcher_node.m_dispatch_control_start_flag = true; if ( tp_main_carrier->get_device_id() != 2 ) { m_dispatch_catcher_node.m_following_flag = true; } else { m_dispatch_catcher_node.m_following_flag = false; } } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } break; } case DISPATCH_CARRIER_PICKUP_31://小跑车 进入车位 { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_32: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_33://小跑车 夹车 { carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_34: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_35://小跑车 回到中跑车 { //注意了, 3楼的搬运器取车时, 机器人的位置必须在搬运器的上方. if ( tp_main_carrier->get_device_id() == 2 ) { //等待机器人调整到3楼搬运器的上方位置 if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_16 ) { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } else { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_36: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_37://让中跑车回到电梯井 { //如果不在2楼, 那么就要退回电梯井 if ( tp_main_carrier->get_device_id() == 0 && (tp_main_carrier->m_actual_coordinates_rows != 0 || m_dispatch_destination == 1100)) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 1 && (tp_main_carrier->m_actual_coordinates_rows != 0 || m_dispatch_destination == 1107)) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 2 ) { //3楼搬运器取车后, 不用x轴移动, 直接准备交接 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_51); } else { //不用回电梯井, 直接准备x轴移动到出口上方 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_43); } break; } case DISPATCH_CARRIER_PICKUP_38://搬运器 释放空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id()); } break; } case DISPATCH_CARRIER_PICKUP_39://收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_40: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_41://电梯移动到2楼, (7号出口移动到1楼), 搬运器在电梯里面z轴移动, 不需要判断空间锁 { float t_z =0; if ( (m_dispatch_destination == 1107 && tp_main_carrier->get_device_id() == 1) || (m_dispatch_destination == 1100 && tp_main_carrier->get_device_id() == 0) )//1楼出口 { t_z = tp_dispatch_coordinates->m_carrier_1th_floor_z; } else if ( m_dispatch_destination >1100 && m_dispatch_destination<1107 ) { t_z = tp_dispatch_coordinates->m_carrier_2th_floor_z; } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; break;//直接跳转到故障 } carrier_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_z); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_42: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_43://伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_main_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK ) { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } break; } case DISPATCH_CARRIER_PICKUP_44: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_45://中跑车 x轴移动, 进行x轴路径检查, 加空间锁失败就进入待机位 { //7号出口, 就不用判断空间锁了, 直接x轴移动 if ( m_dispatch_destination == 1100 || m_dispatch_destination == 1107 ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_80); } else { //等待机器人开始流程任务 if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_2 ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; int t_terminal = m_dispatch_destination - PASSAGEWAY_ID_BASE; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) ) { //搬运器不用x轴运动, 直接到50步, 准备交接 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_51); } else { int t_avoid_catcher_id = -1; //需要避让的机器人 if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, t_column+1-t_parity_flag, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-t_parity_flag, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 2 ) { //3楼搬运器不用加锁, 直接交接 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_51); break;//切换流程 } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_45 tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR "); break;//切换流程 } //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //左右搬运器加锁成功, 直接x轴移动 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_49); } else { //加锁失败, 就移动到待机位. m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } } } //没有抢到机器人的资源, 也要进入待机位 else { //移动到待机位. m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } } break; } case DISPATCH_CARRIER_PICKUP_46://中跑车 x轴移动, 移动到待机位 { if ( tp_main_carrier->get_device_id() == 0 ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[2].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 1 ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[14].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_47: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id()); } break; } case DISPATCH_CARRIER_PICKUP_48://中跑车 x轴移动, 进行x轴路径检查, 加空间锁失败就 只能无限等待 { //等待机器人开始流程任务 if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_2 ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; int t_terminal = m_dispatch_destination - PASSAGEWAY_ID_BASE; int t_parity_flag = (t_terminal - 1) % 2; int t_column = 4 + ((t_terminal - 1) / 2) * 3 + t_parity_flag * 2 - 1; if (Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE)) { //搬运器不用x轴运动, 准备交接 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_51); } else { int t_avoid_catcher_id = -1; //需要避让的机器人 if (tp_main_carrier->get_device_id() == 0 && t_column < 13 - 1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock( tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, t_column + 1 -t_parity_flag, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if (tp_main_carrier->get_device_id() == 1 && t_column > 3 - 1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock( tp_main_carrier->m_actual_coordinates_rows, t_column - t_parity_flag, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns - 1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if (tp_main_carrier->get_device_id() == 2) { //3楼搬运器不用加锁, 直接交接 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_51); break;//切换流程 } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_48 tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR "); break;//切换流程 } //判断结果 if (m_dispatch_carrier_node.m_error == Error_code::SUCCESS) { //左右搬运器加锁成功, 直接x轴移动 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) ( m_dispatch_carrier_node.m_dispatch_control_status + 1); } //else 无限等待 } } break; } case DISPATCH_CARRIER_PICKUP_49://中跑车 x轴移动, 移动到取车口的上方2楼处 { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_50: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_51://小跑车 松开夹杆 { carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_52: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_53://等待机器人把车从搬运器上面取走 { // 等待机器人把车从搬运器上面取走 if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_25 ) { if ( tp_main_carrier->get_device_id() == 2 ) { //进入3楼搬运器取车的特殊流程, 跟随和避让 m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_70; } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } } //else 无限等待 break; } case DISPATCH_CARRIER_PICKUP_54://搬运器退回电梯井 { if ( tp_main_carrier->get_device_id() == 0 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 1 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_PICKUP_54 tp_main_carrier->get_device_id() PARAMRTER ERROR "); } break; } case DISPATCH_CARRIER_PICKUP_55://搬运器 释放空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id()); } break; } case DISPATCH_CARRIER_PICKUP_56://收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { //7号出口, 必须收回对接 if ( m_dispatch_destination == 1100 || m_dispatch_destination == 1107 ) { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //如果后面有一级存车任务, 那么就跳过 else if ( tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL) || tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL)) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_57: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_58://主搬运器 断连 { disconnect_dispatch_device(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_END); break; } case DISPATCH_CARRIER_PICKUP_70://3楼搬运器跟随, 到机器人旁边一个车位 { // 3楼搬运器 最好和机器人一起移动 if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_26 ) { //3楼搬运器跟随, 到机器人旁边一个车位 float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; float t_following_x = 0; if ( m_dispatch_destination == 1101 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1102].x; } else if ( m_dispatch_destination == 1106 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1105].x; } else if ( tp_main_carrier->m_actual_x < t_x) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination-1].x; } else { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination+1].x; } carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_following_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_PICKUP_71: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_72://3楼搬运器移到机器人下方 { //如果后面有一级存车任务, 那么就跳过 //注意了:如果主搬运器是3楼的, 那么也要判断机器人 if ( tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL) || tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL) || m_dispatch_catcher_node.mp_main_catcher->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL) || m_dispatch_catcher_node.mp_main_catcher->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL) ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { if ( m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_PICKUP_38 ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } break; } case DISPATCH_CARRIER_PICKUP_73: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_74://主搬运器 断连 { disconnect_dispatch_device(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_END); break; } case DISPATCH_CARRIER_PICKUP_80://中跑车 x轴移动, 移动到7号取车口 { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_81: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_82://小跑车 进入车位 { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_83: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_84://小跑车 松开车 { carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_85: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_86://小跑车 回到中跑车 { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_PICKUP_87: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_PICKUP_88: { //回到正常流程 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_54); break; } case DISPATCH_CARRIER_PICKUP_END: { m_dispatch_carrier_node.m_error = Error_code::SUCCESS; break; } default: { break; } } std::cout << " huli test :::: " << " ======================================================================================== = " << "carrier end" << std::endl; std::cout << " huli test :::: " << " m_dispatch_carrier_node.m_dispatch_control_status = " << m_dispatch_carrier_node.m_dispatch_control_status << std::endl; if ( m_dispatch_carrier_node.mp_main_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_main_carrier->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_main_carrier->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_main_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL ) { std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_catcher->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_avoid_catcher->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_avoid_catcher->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_carrier_node.mp_avoid_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_avoid_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_carrier->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_avoid_carrier->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_avoid_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_avoid_carrier->get_device_id() = " << -8888 << std::endl; } #ifdef PROCESS_TEST char cr; std::cin >> cr ; #endif std::cout << " huli test :::: " << " ----------------------------------------------------------------------------------------- = " << "catcher start" << std::endl; std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_dispatch_control_status = " << m_dispatch_catcher_node.m_dispatch_control_status << std::endl; if ( m_dispatch_catcher_node.mp_main_catcher.get() != NULL ) { std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << m_dispatch_catcher_node.mp_main_catcher->get_device_id() <<" + "<< m_dispatch_catcher_node.mp_main_catcher->get_current_command_key() <<" + "<< m_dispatch_catcher_node.mp_main_catcher->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_catcher_node.mp_following_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << m_dispatch_catcher_node.mp_following_carrier->get_device_id() <<" + " << m_dispatch_catcher_node.mp_following_carrier->get_current_command_key()<<" + "<< m_dispatch_catcher_node.mp_following_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << -8888 << std::endl; } Error_manager t_catcher_error; Catcher * tp_main_catcher = NULL; Catcher_task * tp_main_catcher_task = NULL; if ( m_dispatch_catcher_node.mp_main_catcher.get() != NULL && m_dispatch_catcher_node.mp_main_catcher_task.get() != NULL ) { tp_main_catcher = (Catcher *)m_dispatch_catcher_node.mp_main_catcher.get(); tp_main_catcher_task = (Catcher_task *)m_dispatch_catcher_node.mp_main_catcher_task.get(); //一级任务被打断后, 重新执行 if ( tp_main_catcher->get_break_flag() ) { if ( m_dispatch_catcher_node.m_main_catcher_task_level == Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL ) { m_dispatch_catcher_node.m_dispatch_control_status = DISPATCH_CATCHER_PICKUP_START; } tp_main_catcher->set_break_flag(false); } } Carrier * tp_following_carrier = NULL; Carrier_task * tp_following_carrier_task = NULL; if ( m_dispatch_catcher_node.mp_following_carrier.get() != NULL && m_dispatch_catcher_node.mp_following_carrier_task.get() != NULL ) { tp_following_carrier = (Carrier *)m_dispatch_catcher_node.mp_following_carrier.get(); tp_following_carrier_task = (Carrier_task *)m_dispatch_catcher_node.mp_following_carrier_task.get(); } //抓取器的控制动作 switch ( m_dispatch_catcher_node.m_dispatch_control_status ) { case DISPATCH_CONTROL_CREATED: case DISPATCH_CONTROL_READY: { m_dispatch_catcher_node.m_error = check_main_catcher(m_dispatch_catcher_node); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP ) { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_START; } else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE ) { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_START; } } //else 原地等待 break; } case DISPATCH_CATCHER_PICKUP_START://连接机器人, 创建新的任务单 与设备建立连接 { if ( m_dispatch_catcher_node.mp_main_catcher.get() == NULL ) { m_dispatch_catcher_node.mp_main_catcher = Dispatch_manager::get_instance_references().m_catcher_map[1]; } if ( m_dispatch_catcher_node.mp_main_catcher_task.get() == NULL ) { m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_error = connect_dispatch_catcher(m_dispatch_catcher_node.mp_main_catcher, m_dispatch_catcher_node.mp_main_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_main_catcher_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL; m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_PICKUP_1: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_2://机器人 准备开始, 需要同步任务单和设备真实数据 { //机器人 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步 catcher_ready_to_start(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_PICKUP_3: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); //注意了:同步失败,需要重新同步. if ( m_dispatch_catcher_node.m_error != Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_2); } break; } case DISPATCH_CATCHER_PICKUP_4://机器手调整到 正常待机的姿态(调节夹杆和轴距) { if ( tp_main_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_main_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE || tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance > tp_dispatch_coordinates->m_catcher_wheel_base_limit || tp_main_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW ) { catcher_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } } else { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_catcher_node.m_error = Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CATCHER_PICKUP_4 Dispatch_process::excute_robot_catch_car_from_carrier() fun error "); } break; } case DISPATCH_CATCHER_PICKUP_5: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_6://机器手 调整z轴, (z轴上升到4楼. 不需要判断空间锁) { if ( tp_main_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z ) { //检测正常, 直接跳过即可 m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } else { catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } break; } case DISPATCH_CATCHER_PICKUP_7://机器人释放 空间锁 { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id()); } break; } case DISPATCH_CATCHER_PICKUP_8://连接3楼的跟随机器人 { if ( m_dispatch_catcher_node.m_following_flag ) { if ( m_dispatch_catcher_node.mp_following_carrier.get() == NULL ) { m_dispatch_catcher_node.mp_following_carrier = Dispatch_manager::get_instance_references().m_carrier_map[2]; } if ( m_dispatch_catcher_node.mp_following_carrier_task.get() == NULL ) { m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_following_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_error = connect_dispatch_carrier(m_dispatch_catcher_node.mp_following_carrier, m_dispatch_catcher_node.mp_following_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_PICKUP_9: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_following_carrier_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_10://搬运器 准备开始, 需要同步任务单和设备真实数据 { if ( m_dispatch_catcher_node.m_following_flag ) { //搬运器 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步 carrier_ready_to_start(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_following_carrier, tp_following_carrier_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_PICKUP_11: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_following_carrier_task, m_dispatch_catcher_node.m_dispatch_control_status); //注意了:同步失败,需要重新同步. if ( m_dispatch_catcher_node.m_error != Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_10); } break; } case DISPATCH_CATCHER_PICKUP_12://机器人 x轴移动之前, 判断空间锁. { int t_column = 0; // 2楼交接, 在出口终点x交接 3楼交接, 在取车车位起点x交接, if ( m_dispatch_catcher_node.m_following_flag ) { int t_terminal = m_dispatch_destination - 1100; int t_parity_flag = (t_terminal-1)%2; t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; } else { t_column = (m_parkspace_information_optimal.parkingspace_index_id -1) % tp_dispatch_coordinates->m_space_lock_columns; } if ( tp_main_catcher->m_actual_coordinates_columns < t_column ) { m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, tp_main_catcher->m_actual_coordinates_columns, 2, t_column+1, tp_main_catcher->get_device_id()); } else { m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, t_column-1, 2, tp_main_catcher->m_actual_coordinates_columns, tp_main_catcher->get_device_id()); } //判断结果 if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CATCHER_PICKUP_13://机器手调整到 对接搬运器的姿态 { float t_x = 0; if ( m_dispatch_catcher_node.m_following_flag ) { // 2楼交接, 在出口终点x交接 t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; float t_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); //机器手调整到 对接搬运器的姿态 catcher_adjust_from_carrier(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_x); //3楼搬运器跟随, 到机器人旁边一个车位 float t_following_x = 0; if ( m_dispatch_destination == 1101 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1102].x; } else if ( m_dispatch_destination == 1106 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1105].x; } else if ( tp_following_carrier->m_actual_x < t_x) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination-1].x; } else { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination+1].x; } carrier_move_x(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_following_carrier, tp_following_carrier_task, tp_dispatch_coordinates, t_following_x); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_28 ) { //3楼交接, 在取车车位起点x交接, t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x; float t_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); //机器手调整到 对接搬运器的姿态 catcher_adjust_from_carrier(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_x); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 } break; } case DISPATCH_CATCHER_PICKUP_14://机器人释放 空间锁 { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task); if ( m_dispatch_catcher_node.m_following_flag ) { m_dispatch_catcher_node.m_error.compare_and_cover_error(check_task_status(m_dispatch_catcher_node.mp_following_carrier_task)); } if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id()); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } break; } case DISPATCH_CATCHER_PICKUP_15://机器手 准备下降抓车之前, 等待搬运器就位, 然后把一级任务升到三级 { //2楼交接时, 机器人先等待, 再升级, 3楼交接时, 机器人先升级, 再等待, if ( m_dispatch_catcher_node.m_following_flag ) { // 2楼交接, 等待搬运器准备好, 机器人再升级 //等待搬运器就位, 把一级任务升到三级 if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_51 ) { m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL); m_dispatch_catcher_node.m_error.compare_and_cover_error(m_dispatch_catcher_node.mp_following_carrier->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL)); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_main_catcher_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL; m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } //else 无限等待 } else { //3楼交接, 机器人直接升级, 需要锁定位置, 不能为其他流程避让 m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_main_catcher_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL; m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } break; } case DISPATCH_CATCHER_PICKUP_16://机器手 z轴下降, 空间加锁 { //2楼交接时, 机器人先等待, 再升级, 3楼交接时, 机器人先升级, 再等待, if ( m_dispatch_catcher_node.m_following_flag ) { //2楼交接的空间已经由搬运器加锁, 机器人下降时就不判断了, 强制加锁即可. if ( m_dispatch_catcher_node.m_following_flag )//去3楼取车不用加锁 { int t_terminal = m_dispatch_destination - 1100; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_force_space_lock(0, t_column-t_parity_flag, 2, t_column+1-t_parity_flag, tp_main_catcher->get_device_id()); } m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_51 ) { //2楼交接的空间已经由搬运器加锁, 机器人下降时就不判断了, 强制加锁即可. if ( m_dispatch_catcher_node.m_following_flag )//去3楼取车不用加锁 { int t_terminal = m_dispatch_destination - 1100; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_force_space_lock(0, t_column-t_parity_flag, 2, t_column+1-t_parity_flag, tp_main_catcher->get_device_id()); } m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 } break; } case DISPATCH_CATCHER_PICKUP_17://机器手 z轴下降 { float t_z = 0; if ( m_dispatch_catcher_node.m_following_flag ) { //下降到2楼抓车 t_z = tp_dispatch_coordinates->m_catcher_2th_floor_z; } else { //下降到3楼抓车 t_z = tp_dispatch_coordinates->m_catcher_3th_floor_z; } catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_PICKUP_18: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_19://交接车辆检查//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. { //检查姿态(注注注注注意了, 调试阶段是无车, 真实环境是有车的) if ( tp_main_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR && tp_main_catcher->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE) { float temp_wheel_base = tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance; //修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } else { catcher_adjust_wheel_base(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } } else { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_catcher_node.m_error = Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CATCHER_PICKUP_19 Dispatch_process:: tp_main_catcher->m_actual_load_status error "); } break; } case DISPATCH_CATCHER_PICKUP_20: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_21://机器手 夹车 { //等待小跑车松开夹车杆 if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_53 ) { catcher_move_c(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CATCHER_PICKUP_22: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_23://机器手 z轴上升, 上升190mm就足以 { // float t_z = 0; // if ( m_dispatch_catcher_node.m_following_flag ) // { // t_z = tp_dispatch_coordinates->m_catcher_2th_floor_z + tp_dispatch_coordinates->m_separated_distance_z; // } // else // { // t_z = tp_dispatch_coordinates->m_catcher_3th_floor_z + tp_dispatch_coordinates->m_separated_distance_z; // } //只能在最高处交接 float t_z = tp_dispatch_coordinates->m_catcher_4th_floor_z; catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_PICKUP_24: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_25://3楼机器人移动前加锁 { int t_terminal = m_dispatch_destination - 1100; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; if ( tp_main_catcher->m_actual_coordinates_columns < t_column ) { m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, tp_main_catcher->m_actual_coordinates_columns, 2, t_column+1, tp_main_catcher->get_device_id()); } else { m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, t_column-1, 2, tp_main_catcher->m_actual_coordinates_columns, tp_main_catcher->get_device_id()); } //判断结果 if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CATCHER_PICKUP_26://机器手调整到 准备把车放到地面的姿态 { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; float t_y = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].y; if ( Common_data::approximate_difference(tp_main_catcher->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) && Common_data::approximate_difference(tp_main_catcher->m_actual_y, t_y, DISPATCH_DEFAULT_DIFFERENCE)) { //检测正常, 直接跳过即可 m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } else { catcher_adjust_to_ground(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_x, t_y); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } break; } case DISPATCH_CATCHER_PICKUP_27://机器人 释放空间锁 { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); //注:只有3楼取车, 才会进行x轴移动, 需要解锁. 其他时候机器人在2楼, 不要释放空间锁 if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS && m_dispatch_catcher_node.m_following_flag == false) { tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id()); } break; } case DISPATCH_CATCHER_PICKUP_28://等待搬运器离开后, 机器人就可以下降了 { //等待搬运器离开后, 机器人就可以下降了 if ( (m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_56 && m_dispatch_carrier_node.m_dispatch_control_status <= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_58) || (m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_72 && m_dispatch_carrier_node.m_dispatch_control_status <= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_74) || (m_dispatch_carrier_node.m_dispatch_control_status == Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_END) ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else if(m_dispatch_carrier_node.m_dispatch_control_status == Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_55 && (Common_data::approximate_difference(tp_main_carrier->m_actual_x, tp_main_catcher->m_actual_x, tp_dispatch_coordinates->m_separated_distance_x) == false) ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CATCHER_PICKUP_29://机器人z轴下降, 判断空间锁 { //2楼交接这里就不加锁了, 因为前面已经加了, 但是3楼前面没有提前加锁, 这里就要加锁了 if ( m_dispatch_catcher_node.m_following_flag ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { int t_terminal = m_dispatch_destination - 1100; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(0, t_column-t_parity_flag, 2, t_column+1-t_parity_flag, tp_main_catcher->get_device_id()); //判断结果 if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 } break; } case DISPATCH_CATCHER_PICKUP_30://机器人下降到地面放车 { catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_1th_floor_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_PICKUP_31: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_32://机器手 松开夹杆 { catcher_move_c(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_PICKUP_33: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_34://机器手调整到 正常待机的姿态(调节夹杆和轴距) { if ( tp_main_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE || tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance > tp_dispatch_coordinates->m_catcher_wheel_base_limit || tp_main_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW ) { catcher_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_PICKUP_35: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_36://机器手 调整z轴, z轴上升到4楼 { catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_PICKUP_37://机器人释放 空间锁 { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id()); } break; } case DISPATCH_CATCHER_PICKUP_38://3楼的搬运器跟随到机器人下方 { //如果后面有一级存车任务, 那么就跳过 if ( m_dispatch_catcher_node.m_following_flag && tp_main_catcher->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL) == false && tp_main_catcher->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL) == false) { float t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_following_carrier, tp_following_carrier_task, tp_dispatch_coordinates, t_following_x); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_PICKUP_39: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_following_carrier_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_PICKUP_40://主机器人 断连 { disconnect_dispatch_device(m_dispatch_catcher_node.mp_main_catcher, m_dispatch_catcher_node.mp_main_catcher_task); if ( m_dispatch_catcher_node.m_following_flag ) { disconnect_dispatch_device(m_dispatch_catcher_node.mp_following_carrier, m_dispatch_catcher_node.mp_following_carrier_task); } m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_END); break; } case DISPATCH_CATCHER_PICKUP_END: { m_dispatch_catcher_node.m_error = Error_code::SUCCESS; break; } default: { break; } } std::cout << " huli test :::: " << " ---------------------------------------------------------------------------------------------------------------------------- = " << "catcher end" << std::endl; std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_dispatch_control_status = " << m_dispatch_catcher_node.m_dispatch_control_status << std::endl; if ( m_dispatch_catcher_node.mp_main_catcher.get() != NULL ) { std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << m_dispatch_catcher_node.mp_main_catcher->get_device_id() <<" + "<< m_dispatch_catcher_node.mp_main_catcher->get_current_command_key() <<" + "<< m_dispatch_catcher_node.mp_main_catcher->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_catcher_node.mp_following_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << m_dispatch_catcher_node.mp_following_carrier->get_device_id() <<" + " << m_dispatch_catcher_node.mp_following_carrier->get_current_command_key()<<" + "<< m_dispatch_catcher_node.mp_following_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << -8888 << std::endl; } std::cout << " huli test :::: " << " +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ = " << 3 << std::endl; std::cout << " huli test :::: " << " m_parkspace_information_optimal.parkingspace_index_id = " << m_parkspace_information_optimal.parkingspace_index_id << std::endl; std::cout << " huli test :::: " << " m_dispatch_destination = " << m_dispatch_destination << std::endl; #ifdef PROCESS_TEST char ch; std::cin >> ch ; #endif //结果汇总 if ( m_dispatch_carrier_node.m_dispatch_control_status == DISPATCH_CARRIER_PICKUP_END && (m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CATCHER_PICKUP_END || m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CONTROL_READY) ) { return Error_code::SUCCESS; } else if ( m_dispatch_carrier_node.m_dispatch_control_status == DISPATCH_CONTROL_FAULT ) { return m_dispatch_carrier_node.m_error; } else if(m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CONTROL_FAULT) { return m_dispatch_catcher_node.m_error; } else { return Error_code::NODATA; } } //调度控制存车 Error_manager Dispatch_process::dispatch_control_motion_store() { std::unique_lock t_lock(m_lock); Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); Error_manager t_carrier_error; Carrier * tp_main_carrier = NULL; Carrier_task * tp_main_carrier_task = NULL; if ( m_dispatch_carrier_node.mp_main_carrier.get() != NULL && m_dispatch_carrier_node.mp_main_carrier_task.get() != NULL ) { tp_main_carrier = (Carrier *)m_dispatch_carrier_node.mp_main_carrier.get(); tp_main_carrier_task = (Carrier_task *)m_dispatch_carrier_node.mp_main_carrier_task.get(); //一级任务被打断后, 重新执行 if ( tp_main_carrier->get_break_flag() ) { if ( m_dispatch_carrier_node.m_main_carrier_task_level == Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL ) { m_dispatch_carrier_node.m_dispatch_control_status = DISPATCH_CARRIER_STORE_START; } tp_main_carrier->set_break_flag(false); } } Catcher * tp_avoid_catcher = NULL; Catcher_task * tp_avoid_catcher_task = NULL; if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL ) { tp_avoid_catcher = (Catcher *)m_dispatch_carrier_node.mp_avoid_catcher.get(); tp_avoid_catcher_task = (Catcher_task *)m_dispatch_carrier_node.mp_avoid_catcher_task.get(); } Carrier * tp_avoid_carrier = NULL; Carrier_task * tp_avoid_carrier_task = NULL; if ( m_dispatch_carrier_node.mp_avoid_carrier.get() != NULL && m_dispatch_carrier_node.mp_avoid_carrier_task.get() != NULL ) { tp_avoid_carrier = (Carrier *)m_dispatch_carrier_node.mp_avoid_carrier.get(); tp_avoid_carrier_task = (Carrier_task *)m_dispatch_carrier_node.mp_avoid_carrier_task.get(); } std::cout << " huli test :::: " << " ===================================================================================================================== = " << "carrier start" << std::endl; std::cout << " huli test :::: " << " m_dispatch_carrier_node.m_dispatch_control_status = " << m_dispatch_carrier_node.m_dispatch_control_status << std::endl; if ( m_dispatch_carrier_node.mp_main_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_main_carrier->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_main_carrier->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_main_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL ) { std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_catcher->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_avoid_catcher->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_avoid_catcher->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_carrier_node.mp_avoid_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_avoid_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_carrier->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_avoid_carrier->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_avoid_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_avoid_carrier->get_device_id() = " << -8888 << std::endl; } //搬运器的控制动作 switch ( m_dispatch_carrier_node.m_dispatch_control_status ) { case DISPATCH_CONTROL_CREATED: case DISPATCH_CONTROL_READY://等待调度管理进行资源分配 { m_dispatch_carrier_node.m_error = check_main_carrier(m_dispatch_carrier_node); if (m_dispatch_carrier_node.m_error == Error_code::SUCCESS) { if (m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP) { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_START; } else if (m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE) { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_START; } } //else 原地等待 break; } case DISPATCH_CARRIER_STORE_START://连接搬运器, 创建新的任务单 与设备建立连接 { //注意了:3楼的存车流程, 需要机器人提前启动 if (m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2) { //通知机器人提前开始 m_dispatch_catcher_node.m_dispatch_control_start_flag = true; m_dispatch_catcher_node.m_following_flag = false; //等待机器人正常连接之后, 搬运器才正式开始 if (m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_STORE_2) { if ( m_dispatch_carrier_node.mp_main_carrier_task.get() == NULL ) { //连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理 m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->check_task_level( Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if (m_dispatch_carrier_node.m_error == Error_code::SUCCESS) { m_dispatch_carrier_node.m_error = connect_dispatch_carrier(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if (m_dispatch_carrier_node.m_error == Error_code::SUCCESS) { m_dispatch_carrier_node.m_main_carrier_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL; m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) ( m_dispatch_carrier_node.m_dispatch_control_status + 1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) (m_dispatch_carrier_node.m_dispatch_control_status + 2); } } //else 无限等待 } else { if ( m_dispatch_carrier_node.mp_main_carrier_task.get() == NULL ) { //连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理 m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->check_task_level( Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if (m_dispatch_carrier_node.m_error == Error_code::SUCCESS) { m_dispatch_carrier_node.m_error = connect_dispatch_carrier(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if (m_dispatch_carrier_node.m_error == Error_code::SUCCESS) { m_dispatch_carrier_node.m_main_carrier_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL; m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) ( m_dispatch_carrier_node.m_dispatch_control_status + 1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) (m_dispatch_carrier_node.m_dispatch_control_status + 2); } } break; } case DISPATCH_CARRIER_STORE_1: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_2://搬运器 准备开始, 需要同步任务单和设备真实数据 { //注意了:3楼搬运器必须要和机器人联动, 所以必须等机器人连接上之后,搬运器才开始启动 if (m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2) { //等待机器人正常连接之后, 搬运器才正式开始 if (m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_STORE_2) { //搬运器 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步 carrier_ready_to_start(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) (m_dispatch_carrier_node.m_dispatch_control_status + 1); } //else 无限等待 } else { //通知机器人提前开始 m_dispatch_catcher_node.m_dispatch_control_start_flag = true; m_dispatch_catcher_node.m_following_flag = true; //搬运器 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步 carrier_ready_to_start(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) (m_dispatch_carrier_node.m_dispatch_control_status + 1); } break; } case DISPATCH_CARRIER_STORE_3: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); //注意了:同步失败,需要重新同步. if ( m_dispatch_carrier_node.m_error != Error_code::SUCCESS ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_2); } break; } case DISPATCH_CARRIER_STORE_4://搬运器检查姿态 //搬运器调整到 正常待机的姿态(调节夹杆和轴距) { if ( tp_main_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_main_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE && Common_data::approximate_difference(m_wheel_base, tp_main_carrier->m_actual_y1-tp_main_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE)) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { //搬运器调整到 正常待机的姿态(调节夹杆和轴距) carrier_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_4 tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error "); } break; } case DISPATCH_CARRIER_STORE_5: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_6://让小跑车回到中跑车上, y轴移动 { if ( Common_data::approximate_difference(tp_main_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_STORE_7: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_8://让中跑车回到电梯井, 进行x轴路径检查 { if ( tp_main_carrier->get_device_id() == 2 ) { //3楼的搬运器直接到 入口旁边一个车位 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_17); } else { //其他搬运器到2楼入口去接车 float t_z = tp_dispatch_coordinates->m_carrier_2th_floor_z; if ( Common_data::approximate_difference(tp_main_carrier->m_actual_z, t_z, DISPATCH_DEFAULT_DIFFERENCE) ) { //就在2楼, 那么准备同层移动到入口接车 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_15); } else { //搬运器不在2楼或者3楼, 搬运器在4楼及以上, 那么直接返回电梯井即可. m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } } break; } case DISPATCH_CARRIER_STORE_9://让中跑车回到电梯井, x轴移动 { if ( tp_main_carrier->get_device_id() == 0 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 1 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_9 tp_main_carrier->get_device_id() PARAMRTER ERROR "); } break; } case DISPATCH_CARRIER_STORE_10://搬运器 释放空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id()); } break; } case DISPATCH_CARRIER_STORE_11://中跑车收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_12: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_13: //电梯移动到对应的楼层, 搬运器在电梯里面z轴移动, 不需要判断空间锁 { //其他搬运器到2楼入口去接车 float t_z = tp_dispatch_coordinates->m_carrier_2th_floor_z; carrier_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_z); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_14: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_15://伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_main_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK ) { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } break; } case DISPATCH_CARRIER_STORE_16: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_17://中跑车 x轴移动到入口, 进行x轴路径检查 { //如果机器人已经把车抓起来了, 那么搬运器直接去入口接车即可 if ( m_dispatch_catcher_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_25 ) { // 搬运器直接去入口接车即可 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_20); } if ( tp_main_carrier->get_device_id() == 2 ) { if ( m_dispatch_catcher_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_8 ) { //3楼搬运器不用加锁, 直接去待机位即可 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break;//切换流程 } //else 无限等待 } else { //等待机器人升到三级任务, 搬运器移到待机位, (机器人到一楼抓车还需要一段时间.) if ( m_dispatch_catcher_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_16 ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x; int t_terminal = m_dispatch_source - PASSAGEWAY_ID_BASE; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; //注意了:待机点在入口旁边一个大车位, 空间锁判断时, 距离2个小车位即可. int t_avoid_catcher_id = -1; //需要避让的机器人 if ( tp_main_carrier->get_device_id() == 0 ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, t_column-2, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 1 ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column+2, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 2 ) { //3楼搬运器不用加锁, 直接去待机位即可 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break;//切换流程 } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_17 tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR "); break;//切换流程 } //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //左右搬运器加锁成功, 直接x轴移动到待机位 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 在电梯井 无限等待 } //else 在电梯井 无限等待 } break; } case DISPATCH_CARRIER_STORE_18://中跑车 x轴移动, 移动到待机位, (待机点在入口旁边一个大车位) { //3楼搬运器跟随, 到机器人旁边一个车位 float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x; float t_following_x = 0; //3楼搬运器特殊处理, // 注意了: 机器人必须等待3楼搬运器执行完这一步后, 才能到一楼抓车 if ( tp_main_carrier->get_device_id() == 2 ) { if ( m_dispatch_catcher_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_13 ) { if ( m_dispatch_source == 1101 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1102].x; } else if ( m_dispatch_source == 1106 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1105].x; } else if ( tp_main_carrier->m_actual_x < t_x) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source-1].x; } else { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source+1].x; } } else //无限等待 { break; } } else if ( tp_main_carrier->get_device_id() == 0 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x - tp_dispatch_coordinates->m_separated_distance_x; } else if ( tp_main_carrier->get_device_id() == 1 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x + tp_dispatch_coordinates->m_separated_distance_x; } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_18 tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR "); break;//切换流程 } carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_following_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_19: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_20://搬运器 准备接车之前, 等待机器人就位, 然后把一级任务升到三级 { //等待机器人把车抓起来, 如果机器人已经把车抓起来了, 那么搬运器把一级任务升到三级 if ( m_dispatch_catcher_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_25 ) { m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_main_carrier_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL; m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } //else 无限等待 break; } case DISPATCH_CARRIER_STORE_21://搬运器移到入口接车, 对路径强制加锁, (机器人已经加空间锁了) { //2楼交接的空间已经由机器人加锁, 搬运器移到入口接车就不判断了, 强制加锁即可. if ( tp_main_carrier->get_device_id() == 0 ) { int t_terminal = m_dispatch_source - 1100; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_force_space_lock(0, 0, 0, t_column+1-t_parity_flag, tp_main_carrier->get_device_id()); } else if ( tp_main_carrier->get_device_id() == 1 ) { int t_terminal = m_dispatch_source - 1100; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_force_space_lock(0, t_column-t_parity_flag, 0, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id()); } //去3楼取车不用加锁 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_22://中跑车 x轴移动, 移动到存车入口上方准备接车 { //注意了: 3楼搬运器在停车位交接, 2楼搬运器在入口上方交接 float t_x = 0; if ( tp_main_carrier->get_device_id() == 2 ) { //3楼搬运器 最好和机器人一起运动 if ( m_dispatch_catcher_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_26 ) { //need program , 这里设计到机器人和3楼搬运器的同步问题, 以后在写 t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } else { t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_STORE_23: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_24://检查汽车交接//修正轴距 { //检查姿态(注注注注注意了, 调试阶段是无车, 真实环境是有车的) if ( tp_main_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR && tp_main_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE) { float temp_wheel_base = tp_main_carrier->m_actual_y1 - tp_main_carrier->m_actual_y2; if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { carrier_adjust_wheel_base(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } } else { return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_24 Dispatch_process:: tp_main_carrier->m_actual_load_status() fun error "); } break; } case DISPATCH_CARRIER_STORE_25: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_26://让小跑车 夹车 { //等待机器人把车放到搬运器上面 if (m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_STORE_34) { carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CARRIER_STORE_27: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_28://让中跑车回到电梯井 { //等待机器人离开 if (m_dispatch_catcher_node.m_dispatch_control_status >= DISPATCH_CATCHER_STORE_38) { //判断z轴 是否同层 float t_z = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].z; if ( Common_data::approximate_difference(tp_main_carrier->m_actual_z, t_z, DISPATCH_DEFAULT_DIFFERENCE) ) { //存车到2楼或3楼, 那么直接去停车位 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_36); } else { if ( tp_main_carrier->get_device_id() == 0 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 1 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_28 tp_main_carrier->get_device_id() PARAMRTER ERROR "); } } } //else 无限等待 break; } case DISPATCH_CARRIER_STORE_29://搬运器 释放空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id()); } break; } case DISPATCH_CARRIER_STORE_30://中跑车收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_31: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_32: //电梯移动到对应的楼层, 搬运器在电梯里面z轴移动, 不需要判断空间锁 { float t_z = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].z; carrier_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_z); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_33: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_34://伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_main_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK ) { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } break; } case DISPATCH_CARRIER_STORE_35: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_36://中跑车 x轴移动到车位, 进行x轴路径检查 { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x; int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns; if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) ) { //搬运器不用x轴运动, 直接去车位存车, (注:3楼的搬运器已经提前去停车位了, x值一定相同) m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_44); } else { int t_avoid_catcher_id = -1; //需要避让的机器人 if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, t_column+1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_36 tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR "); break;//切换流程 } //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //左右搬运器加锁成功, 直接x轴移动 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_42); } else if(t_avoid_catcher_id != -1 && (tp_main_carrier->m_actual_coordinates_rows == 2 || tp_main_carrier->m_actual_coordinates_rows == 1))//4楼取车, 机器人进行避让 { if ( m_dispatch_carrier_node.mp_avoid_catcher.get() == NULL ) { m_dispatch_carrier_node.mp_avoid_catcher = Dispatch_manager::get_instance_references().m_catcher_map[t_avoid_catcher_id]; } if ( m_dispatch_carrier_node.mp_avoid_carrier.get() == NULL ) { m_dispatch_carrier_node.mp_avoid_carrier = Dispatch_manager::get_instance_references().m_carrier_map[2]; } //连接4楼机器人和3楼搬运器 Error_manager t_catcher_error; Error_manager t_carrier_error; if ( m_dispatch_carrier_node.mp_avoid_catcher_task.get() == NULL && m_dispatch_carrier_node.mp_avoid_carrier_task.get() == NULL) { //连接避让设备 使得避让机器人进行避让 t_catcher_error = m_dispatch_carrier_node.mp_avoid_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); t_carrier_error = m_dispatch_carrier_node.mp_avoid_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); if ( t_catcher_error == Error_code::SUCCESS && t_carrier_error == Error_code::SUCCESS) { t_catcher_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); if ( t_catcher_error == Error_code::SUCCESS ) { t_carrier_error = connect_dispatch_carrier(m_dispatch_carrier_node.mp_avoid_carrier, m_dispatch_carrier_node.mp_avoid_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL); if ( t_carrier_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_36 connect_dispatch_carrier ERROR "); break;//切换流程 } } } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } else if ( m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL && m_dispatch_carrier_node.mp_avoid_carrier_task.get() != NULL ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_36 m_dispatch_carrier_node.mp_avoid_catcher_task.get() m_dispatch_carrier_node.mp_avoid_carrier_task.get() ERROR "); break;//切换流程 } } //else 无限等待 } break; } case DISPATCH_CARRIER_STORE_37: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_38://让4楼机器人进行避让, 先加锁, 在避让 { //机器人避让距离, 4楼避让4个车位, 3楼避让2个车位 int m_avoid_distance = 0; int t_row = (m_parkspace_information_optimal.parkingspace_index_id-1) / tp_dispatch_coordinates->m_space_lock_columns; if ( t_row == 1 ) { m_avoid_distance = 3; } else { m_avoid_distance = 3; } int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns; //4楼搬运器需要避让4个车位 if ( tp_main_carrier->get_device_id() == 0 && tp_avoid_catcher->m_actual_coordinates_columns < t_column+m_avoid_distance ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, tp_avoid_catcher->m_actual_coordinates_columns, 2, t_column+m_avoid_distance+1, tp_avoid_catcher->get_device_id()); //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //机器人移到42号口 float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id+m_avoid_distance].x; catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, t_x); carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_carrier, tp_avoid_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } else if ( tp_main_carrier->get_device_id() == 1 && tp_avoid_catcher->m_actual_coordinates_columns > t_column-m_avoid_distance) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, t_column-m_avoid_distance-1, 2, tp_avoid_catcher->m_actual_coordinates_columns, tp_avoid_catcher->get_device_id()); //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { //机器人移到34号口 float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id-m_avoid_distance].x; catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, t_x); carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_carrier, tp_avoid_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } //else 无限等待 } else { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_STORE_39://机器人释放 空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task); m_dispatch_carrier_node.m_error.compare_and_cover_error(check_task_status(m_dispatch_carrier_node.mp_avoid_carrier_task)); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_avoid_catcher->m_actual_coordinates_id, tp_avoid_catcher->get_device_id()); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } break; } case DISPATCH_CARRIER_STORE_40://搬运器空间锁判断, 必须成功 { int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns; int t_avoid_catcher_id = -1; //需要避让的机器人 if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, 0, tp_main_carrier->m_actual_coordinates_rows, t_column+1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1, tp_main_carrier->m_actual_coordinates_rows, tp_dispatch_coordinates->m_space_lock_columns-1, tp_main_carrier->get_device_id(), t_avoid_catcher_id); } //判断结果 if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } break; } case DISPATCH_CARRIER_STORE_41://避让机器人 断连 { if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL ) { //避让机器人 断开连接 disconnect_dispatch_device(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task); } if ( m_dispatch_carrier_node.mp_avoid_carrier.get() != NULL && m_dispatch_carrier_node.mp_avoid_carrier_task.get() != NULL ) { //避让机器人 断开连接 disconnect_dispatch_device(m_dispatch_carrier_node.mp_avoid_carrier, m_dispatch_carrier_node.mp_avoid_carrier_task); } m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_42://搬运器x轴移动到车位 { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_43: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_44://小跑车 进入车位 { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_45: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_46://小跑车 松开 { carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_47: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_48://小跑车 回到中跑车 { carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_49: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_50://让中跑车回到电梯井 { if ( tp_main_carrier->get_device_id() == 2 ) { //3楼的流程 到此结束了 m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_54); } //2楼和4楼 必须回电梯井 else if ( tp_main_carrier->m_actual_coordinates_rows == 0 || tp_main_carrier->m_actual_coordinates_rows == 2 ) { if ( tp_main_carrier->get_device_id() == 0 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 1 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_50 tp_main_carrier->get_device_id() PARAMRTER ERROR "); } } //如果后面有一级存车任务, 那么就跳过, 否则让中跑车回到电梯井 else if ( tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL) || tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL)) { m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_54); } else { if ( tp_main_carrier->get_device_id() == 0 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else if ( tp_main_carrier->get_device_id() == 1 ) { carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); } else { m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CARRIER_STORE_50 tp_main_carrier->get_device_id() PARAMRTER ERROR "); } } break; } case DISPATCH_CARRIER_STORE_51://搬运器 释放空间锁 { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS ) { m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id()); } break; } case DISPATCH_CARRIER_STORE_52://收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1); break; } case DISPATCH_CARRIER_STORE_53: { m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status); break; } case DISPATCH_CARRIER_STORE_54://主搬运器 断连 { disconnect_dispatch_device(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task); m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_END); break; } } std::cout << " huli test :::: " << " ======================================================================================== = " << "carrier end" << std::endl; std::cout << " huli test :::: " << " m_dispatch_carrier_node.m_dispatch_control_status = " << m_dispatch_carrier_node.m_dispatch_control_status << std::endl; if ( m_dispatch_carrier_node.mp_main_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_main_carrier->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_main_carrier->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_main_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL ) { std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_catcher->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_avoid_catcher->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_avoid_catcher->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_carrier_node.mp_avoid_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_avoid_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_carrier->get_device_id() <<" + "<< m_dispatch_carrier_node.mp_avoid_carrier->get_current_command_key() <<" + "<< m_dispatch_carrier_node.mp_avoid_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_avoid_carrier->get_device_id() = " << -8888 << std::endl; } #ifdef PROCESS_TEST char cr; std::cin >> cr ; #endif std::cout << " huli test :::: " << " ----------------------------------------------------------------------------------------- = " << "catcher start" << std::endl; std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_dispatch_control_status = " << m_dispatch_catcher_node.m_dispatch_control_status << std::endl; if ( m_dispatch_catcher_node.mp_main_catcher.get() != NULL ) { std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << m_dispatch_catcher_node.mp_main_catcher->get_device_id() <<" + "<< m_dispatch_catcher_node.mp_main_catcher->get_current_command_key() <<" + "<< m_dispatch_catcher_node.mp_main_catcher->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_catcher_node.mp_following_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << m_dispatch_catcher_node.mp_following_carrier->get_device_id() <<" + " << m_dispatch_catcher_node.mp_following_carrier->get_current_command_key()<<" + "<< m_dispatch_catcher_node.mp_following_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << -8888 << std::endl; } Error_manager t_catcher_error; Catcher * tp_main_catcher = NULL; Catcher_task * tp_main_catcher_task = NULL; if ( m_dispatch_catcher_node.mp_main_catcher.get() != NULL && m_dispatch_catcher_node.mp_main_catcher_task.get() != NULL ) { tp_main_catcher = (Catcher *)m_dispatch_catcher_node.mp_main_catcher.get(); tp_main_catcher_task = (Catcher_task *)m_dispatch_catcher_node.mp_main_catcher_task.get(); //一级任务被打断后, 重新执行 if ( tp_main_catcher->get_break_flag() ) { if ( m_dispatch_catcher_node.m_main_catcher_task_level == Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL ) { m_dispatch_catcher_node.m_dispatch_control_status = DISPATCH_CATCHER_STORE_START; } tp_main_catcher->set_break_flag(false); } } Carrier * tp_following_carrier = NULL; Carrier_task * tp_following_carrier_task = NULL; if ( m_dispatch_catcher_node.mp_following_carrier.get() != NULL && m_dispatch_catcher_node.mp_following_carrier_task.get() != NULL ) { tp_following_carrier = (Carrier *)m_dispatch_catcher_node.mp_following_carrier.get(); tp_following_carrier_task = (Carrier_task *)m_dispatch_catcher_node.mp_following_carrier_task.get(); } //抓取器的控制动作 switch ( m_dispatch_catcher_node.m_dispatch_control_status ) { case DISPATCH_CONTROL_CREATED: case DISPATCH_CONTROL_READY: { m_dispatch_catcher_node.m_error = check_main_catcher(m_dispatch_catcher_node); if (m_dispatch_catcher_node.m_error == Error_code::SUCCESS) { if (m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP) { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_START; } else if (m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE) { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_START; } } //else 原地等待 break; } case DISPATCH_CATCHER_STORE_START://连接机器人, 创建新的任务单 与设备建立连接 { if (m_dispatch_catcher_node.mp_main_catcher.get() == NULL) { m_dispatch_catcher_node.mp_main_catcher = Dispatch_manager::get_instance_references().m_catcher_map[1]; } if ( m_dispatch_catcher_node.mp_main_catcher_task.get() == NULL ) { m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->check_task_level( Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if (m_dispatch_catcher_node.m_error == Error_code::SUCCESS) { m_dispatch_catcher_node.m_error = connect_dispatch_catcher(m_dispatch_catcher_node.mp_main_catcher, m_dispatch_catcher_node.mp_main_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if (m_dispatch_catcher_node.m_error == Error_code::SUCCESS) { m_dispatch_catcher_node.m_main_catcher_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL; m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) (m_dispatch_catcher_node.m_dispatch_control_status + 1); } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) (m_dispatch_catcher_node.m_dispatch_control_status + 2); } break; } case DISPATCH_CATCHER_STORE_1: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_2://机器人 准备开始, 需要同步任务单和设备真实数据 { //机器人 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步 catcher_ready_to_start(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) (m_dispatch_catcher_node.m_dispatch_control_status + 1); break; } case DISPATCH_CATCHER_STORE_3: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); //注意了:同步失败,需要重新同步. if ( m_dispatch_catcher_node.m_error != Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_2); } break; } case DISPATCH_CATCHER_STORE_4://机器手调整到 正常待机的姿态(调节夹杆和轴距) { if ( tp_main_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_main_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE || tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance > tp_dispatch_coordinates->m_catcher_wheel_base_limit || tp_main_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW ) { catcher_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } } else { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; m_dispatch_catcher_node.m_error = Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR, "DISPATCH_CATCHER_STORE_4 Dispatch_process::excute_robot_catch_car_from_carrier() fun error "); } break; } case DISPATCH_CATCHER_STORE_5: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_6://机器手 调整z轴, (z轴上升到4楼. 不需要判断空间锁) { if ( tp_main_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z ) { //检测正常, 直接跳过即可 m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } else { catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } break; } case DISPATCH_CATCHER_STORE_7://机器人释放 空间锁 { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id()); } break; } case DISPATCH_CATCHER_STORE_8://连接3楼的跟随机器人 { if ( m_dispatch_catcher_node.m_following_flag ) { if ( m_dispatch_catcher_node.mp_following_carrier.get() == NULL ) { m_dispatch_catcher_node.mp_following_carrier = Dispatch_manager::get_instance_references().m_carrier_map[2]; } if ( m_dispatch_catcher_node.mp_following_carrier_task.get() == NULL ) { m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_following_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_error = connect_dispatch_carrier(m_dispatch_catcher_node.mp_following_carrier, m_dispatch_catcher_node.mp_following_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } //else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接 } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_STORE_9: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_following_carrier_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_10://搬运器 准备开始, 需要同步任务单和设备真实数据 { if ( m_dispatch_catcher_node.m_following_flag ) { //搬运器 准备开始, 需要同步任务单和设备真实数据.//注意了:连接设备只是预约, 设备不一定立刻执行.所以需要在连接成功之后进行数据同步 carrier_ready_to_start(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_following_carrier, tp_following_carrier_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_STORE_11: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_following_carrier_task, m_dispatch_catcher_node.m_dispatch_control_status); //注意了:同步失败,需要重新同步. if ( m_dispatch_catcher_node.m_error != Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_10); } break; } case DISPATCH_CATCHER_STORE_12://机器人 x轴移动之前, 判断空间锁. { int t_terminal = m_dispatch_source - 1100; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; if ( tp_main_catcher->m_actual_coordinates_columns < t_column ) { m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, tp_main_catcher->m_actual_coordinates_columns, 2, t_column+1, tp_main_catcher->get_device_id()); } else { m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, t_column-1, 2, tp_main_catcher->m_actual_coordinates_columns, tp_main_catcher->get_device_id()); } //判断结果 if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CATCHER_STORE_13://机器手调整到 对接地面的姿态 { if ( m_dispatch_catcher_node.m_following_flag ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x; float t_y = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].y; //机器手调整到 对接地面的姿态 catcher_adjust_from_ground(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); //3楼搬运器跟随, 到机器人旁边一个车位 float t_following_x = 0; if ( m_dispatch_source == 1101 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1102].x; } else if ( m_dispatch_source == 1106 ) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[1105].x; } else if ( tp_following_carrier->m_actual_x < t_x) { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source-1].x; } else { t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source+1].x; } carrier_move_x(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_following_carrier, tp_following_carrier_task, tp_dispatch_coordinates, t_following_x); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_18 ) { float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x; float t_y = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].y; //机器手调整到 对接地面的姿态 catcher_adjust_from_ground(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 } break; } case DISPATCH_CATCHER_STORE_14://机器人释放 空间锁 { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task); if ( m_dispatch_catcher_node.m_following_flag ) { m_dispatch_catcher_node.m_error.compare_and_cover_error(check_task_status(m_dispatch_catcher_node.mp_following_carrier_task)); } if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id()); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } break; } case DISPATCH_CATCHER_STORE_15://机器手 准备下降抓车之前, 把一级任务升到三级 { //2楼交接则直接下降抓车, 3楼交接,就需要等待3楼搬运器进入待机位, if ( m_dispatch_catcher_node.m_following_flag ) { m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->change_task_level( Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL); m_dispatch_catcher_node.m_error.compare_and_cover_error(m_dispatch_catcher_node.mp_following_carrier->change_task_level( Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL)); if (m_dispatch_catcher_node.m_error == Error_code::SUCCESS) { m_dispatch_catcher_node.m_main_catcher_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL; m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) ( m_dispatch_catcher_node.m_dispatch_control_status + 1); } else if(m_dispatch_catcher_node.m_error == Error_code::NODATA) { //无限等待 } else { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } //等待搬运器就位, 把一级任务升到三级 else if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_20 ) { m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->change_task_level( Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL); if (m_dispatch_catcher_node.m_error == Error_code::SUCCESS) { m_dispatch_catcher_node.m_main_catcher_task_level = Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL; m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status) ( m_dispatch_catcher_node.m_dispatch_control_status + 1); } else if(m_dispatch_catcher_node.m_error == Error_code::NODATA) { //无限等待 } else { m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT; } } break; } case DISPATCH_CATCHER_STORE_16://机器手 z轴下降, 空间加锁 { int t_terminal = m_dispatch_source - 1100; int t_parity_flag = (t_terminal-1)%2; int t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(0, t_column-t_parity_flag, 2, t_column+1-t_parity_flag, tp_main_catcher->get_device_id()); //判断结果 if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { //左右搬运器加锁成功, 直接x轴移动 m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CATCHER_STORE_17://机器手 z轴下降 { float t_z = tp_dispatch_coordinates->m_catcher_1th_floor_z; catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_STORE_18: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_19://修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. { float temp_wheel_base = tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance; //修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } else { catcher_adjust_wheel_base(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } break; } case DISPATCH_CATCHER_STORE_20: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_21://机器手 夹车 { catcher_move_c(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_STORE_22: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_23://机器手 z轴上升, 上升到交接为上方190mm就足以 { // float t_z = 0; // if ( m_dispatch_catcher_node.m_following_flag ) // { // t_z = tp_dispatch_coordinates->m_catcher_2th_floor_z + tp_dispatch_coordinates->m_separated_distance_z; // } // else // { // t_z = tp_dispatch_coordinates->m_catcher_3th_floor_z + tp_dispatch_coordinates->m_separated_distance_z; // } //只能在最高处交接 float t_z = tp_dispatch_coordinates->m_catcher_4th_floor_z; catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_STORE_24: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_25://机器人 x轴移动之前, 判断空间锁. { int t_column = 0; // 2楼交接, 在出口终点x交接 3楼交接, 在取车车位起点x交接, if ( m_dispatch_catcher_node.m_following_flag ) { int t_terminal = m_dispatch_source - 1100; int t_parity_flag = (t_terminal-1)%2; t_column = 4 + ((t_terminal-1)/2)*3 + t_parity_flag*2 -1; } else { t_column = (m_parkspace_information_optimal.parkingspace_index_id -1) % tp_dispatch_coordinates->m_space_lock_columns; } if ( tp_main_catcher->m_actual_coordinates_columns < t_column ) { m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, tp_main_catcher->m_actual_coordinates_columns, 2, t_column+1, tp_main_catcher->get_device_id()); } else { m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(1, t_column-1, 2, tp_main_catcher->m_actual_coordinates_columns, tp_main_catcher->get_device_id()); } //判断结果 if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CATCHER_STORE_26://机器手调整到 准备把车放到搬运器的姿态 { float t_x = 0; // 2楼交接, 在入口起点x交接 3楼交接, 在车位x交接, if ( m_dispatch_catcher_node.m_following_flag ) { t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x; } else { t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x; } float t_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); //注:3楼流程 可能会出现x平移的同时 进行旋转掉头 catcher_adjust_to_carrier(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_x, true); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_STORE_27://机器人 释放空间锁 { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id()); } break; } case DISPATCH_CATCHER_STORE_28://等待搬运器过来接车后, 机器人就可以下降了 { //等待搬运器过来接车后, 机器人就可以下降了 if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_24 ) { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } //else 无限等待 break; } case DISPATCH_CATCHER_STORE_29://机器人z轴下降, 判断空间锁 { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_STORE_30://机器手 z轴下降 到搬运器放车 { float t_z = 0; if ( m_dispatch_catcher_node.m_following_flag ) { //下降到2楼 t_z = tp_dispatch_coordinates->m_catcher_2th_floor_z; } else { //下降到3楼 t_z = tp_dispatch_coordinates->m_catcher_3th_floor_z; } catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, t_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_STORE_31: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_32://机器手 松开夹杆 { catcher_move_c(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_STORE_33: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_34://机器手调整到 正常待机的姿态(调节夹杆和轴距) { if ( tp_main_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE || tp_main_catcher->m_actual_d1 + tp_main_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance > tp_dispatch_coordinates->m_catcher_wheel_base_limit || tp_main_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW ) { catcher_adjust_to_ready(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_STORE_35: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_36://机器手 调整z轴, z轴上升到4楼 { catcher_move_z(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_main_catcher, tp_main_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); break; } case DISPATCH_CATCHER_STORE_37://机器人释放 空间锁 { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status); if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS ) { tp_dispatch_coordinates->catcher_space_unlock(tp_main_catcher->m_actual_coordinates_id, tp_main_catcher->get_device_id()); } break; } case DISPATCH_CATCHER_STORE_38://3楼的搬运器跟随到机器人下方 { //如果后面有一级存车任务, 那么就跳过 if ( m_dispatch_catcher_node.m_following_flag && tp_main_catcher->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL) == false && tp_main_catcher->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL) == false) { float t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_source].x; carrier_move_x(get_dispatch_control_command_key(m_dispatch_catcher_node), tp_following_carrier, tp_following_carrier_task, tp_dispatch_coordinates, t_following_x); m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1); } else { m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+2); } break; } case DISPATCH_CATCHER_STORE_39: { m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_catcher_node.mp_following_carrier_task, m_dispatch_catcher_node.m_dispatch_control_status); break; } case DISPATCH_CATCHER_STORE_40://主机器人 断连 { disconnect_dispatch_device(m_dispatch_catcher_node.mp_main_catcher, m_dispatch_catcher_node.mp_main_catcher_task); if ( m_dispatch_catcher_node.m_following_flag ) { disconnect_dispatch_device(m_dispatch_catcher_node.mp_following_carrier, m_dispatch_catcher_node.mp_following_carrier_task); } m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_END); break; } case DISPATCH_CATCHER_STORE_END: { m_dispatch_catcher_node.m_error = Error_code::SUCCESS; break; } default: { break; } } std::cout << " huli test :::: " << " ---------------------------------------------------------------------------------------------------------------------------- = " << "catcher end" << std::endl; std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_dispatch_control_status = " << m_dispatch_catcher_node.m_dispatch_control_status << std::endl; if ( m_dispatch_catcher_node.mp_main_catcher.get() != NULL ) { std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << m_dispatch_catcher_node.mp_main_catcher->get_device_id() <<" + "<< m_dispatch_catcher_node.mp_main_catcher->get_current_command_key() <<" + "<< m_dispatch_catcher_node.mp_main_catcher->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << -8888 << std::endl; } if ( m_dispatch_catcher_node.mp_following_carrier.get() != NULL ) { std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << m_dispatch_catcher_node.mp_following_carrier->get_device_id() <<" + " << m_dispatch_catcher_node.mp_following_carrier->get_current_command_key()<<" + "<< m_dispatch_catcher_node.mp_following_carrier->get_dispatch_device_status() << std::endl; } else { std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << -8888 << std::endl; } std::cout << " huli test :::: " << " +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ = " << 3 << std::endl; std::cout << " huli test :::: " << " m_dispatch_source = " << m_dispatch_source << std::endl; std::cout << " huli test :::: " << " m_parkspace_information_optimal.parkingspace_index_id = " << m_parkspace_information_optimal.parkingspace_index_id << std::endl; #ifdef PROCESS_TEST char ch; std::cin >> ch ; #endif //结果汇总 if ( m_dispatch_carrier_node.m_dispatch_control_status == DISPATCH_CARRIER_STORE_END && m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CATCHER_STORE_END ) { return Error_code::SUCCESS; } else if ( m_dispatch_carrier_node.m_dispatch_control_status == DISPATCH_CONTROL_FAULT ) { return m_dispatch_carrier_node.m_error; } else if(m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CONTROL_FAULT) { return m_dispatch_catcher_node.m_error; } else { return Error_code::NODATA; } } //检查核心搬运器 Error_manager Dispatch_process::check_main_carrier(Dispatch_carrier_node & dispatch_carrier_node) { //只判断搬运器的指针是否有效. if ( dispatch_carrier_node.mp_main_carrier.get() != NULL ) { // if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP ) // { // m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_START; // } // else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE ) // { // m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_STORE_START; // } return Error_code::SUCCESS; } else { return Error_code::NODATA; } } //检查核心抓取器 Error_manager Dispatch_process::check_main_catcher(Dispatch_catcher_node & dispatch_catcher_node) { //只判断搬 调度设备机器人启动标志位 if ( dispatch_catcher_node.m_dispatch_control_start_flag ) { //为抓取器分配对应的设备, 目前只有2号机器人 // if ( dispatch_catcher_node.mp_main_catcher.get() ==NULL ) // { // dispatch_catcher_node.mp_main_catcher = Dispatch_manager::get_instance_references().m_catcher_map[1]; // } // // if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP ) // { // dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_START; // } // else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE ) // { // dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_START; // } return Error_code::SUCCESS; } else { return Error_code::NODATA; } } //连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理 Error_manager Dispatch_process::connect_dispatch_carrier(std::shared_ptr & p_carrier, std::shared_ptr & p_carrier_task, Dispatch_device_base::Dispatch_task_level dispatch_task_level) { Error_manager t_error; if ( p_carrier.get() != NULL ) { Carrier* tp_carrier = (Carrier*)p_carrier.get(); //检查设备能否执行任务, 主搬运器是管理层分配好的, 一般是可以直接连接. t_error = tp_carrier->check_task_level(dispatch_task_level); if ( t_error == Error_code::SUCCESS ) { //创建任务单 p_carrier_task = std::shared_ptr(new Carrier_task); p_carrier_task->task_init(NULL,std::chrono::milliseconds(15000)); Carrier_task * tp_carrier_task = (Carrier_task *)p_carrier_task.get(); //第一次发送 空的唯一码, 可以和设备建立联系 tp_carrier_task->m_request_key = ""; tp_carrier_task->m_request_x = tp_carrier->m_actual_x; tp_carrier_task->m_request_y = tp_carrier->m_actual_y; tp_carrier_task->m_request_z = tp_carrier->m_actual_z; tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y1; tp_carrier_task->m_request_y2 = tp_carrier->m_actual_y2; tp_carrier_task->m_request_clamp_motion = (Carrier_task::Clamp_motion)tp_carrier->m_actual_clamp_motion1; tp_carrier_task->m_request_joint_motion_x = (Carrier_task::Joint_motion)tp_carrier->m_actual_joint_motion_x1; tp_carrier_task->m_request_joint_motion_y = Carrier_task::Joint_motion::E_JOINT_NO_ACTION; tp_carrier_task->m_request_space_id = 0; tp_carrier_task->m_request_floor_id = 0; tp_carrier_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase; t_error = tp_carrier->execute_task(p_carrier_task, dispatch_task_level, m_command_key, m_dispatch_process_type); } } else { t_error.error_manager_reset(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " Dispatch_process::connect_dispatch_carrier() POINTER IS NULL "); } return t_error; } //连接抓取器, 创建新的任务单 与设备建立连接, 可以失败, 失败就重新执行 Error_manager Dispatch_process::connect_dispatch_catcher(std::shared_ptr & p_catcher, std::shared_ptr & p_catcher_task, Dispatch_device_base::Dispatch_task_level dispatch_task_level) { Error_manager t_error; if ( p_catcher.get() != NULL ) { Catcher* tp_catcher = (Catcher*)p_catcher.get(); //检查设备能否执行任务, 主抓取器是需要调度流程去争抢的. t_error = tp_catcher->check_task_level(dispatch_task_level); if ( t_error == Error_code::SUCCESS ) { //创建任务单 p_catcher_task = std::shared_ptr(new Catcher_task); p_catcher_task->task_init(NULL,std::chrono::milliseconds(15000)); Catcher_task * tp_catcher_task = (Catcher_task *)p_catcher_task.get(); //第一次发送 空的唯一码, 可以和设备建立联系 tp_catcher_task->m_request_key = ""; tp_catcher_task->m_request_x = tp_catcher->m_actual_x; tp_catcher_task->m_request_y = tp_catcher->m_actual_y; tp_catcher_task->m_request_b = tp_catcher->m_actual_b; tp_catcher_task->m_request_z = tp_catcher->m_actual_z; tp_catcher_task->m_request_d1 = tp_catcher->m_actual_d1; tp_catcher_task->m_request_d2 = tp_catcher->m_actual_d2; tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase; tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)tp_catcher->m_actual_clamp_motion1; t_error = tp_catcher->execute_task(p_catcher_task, dispatch_task_level, m_command_key, m_dispatch_process_type); } } else { t_error.error_manager_reset(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " Dispatch_process::connect_dispatch_catcher() POINTER IS NULL "); } return t_error; } //断开调度设备, 收回任务单, 释放任务单 与设备解除连接 Error_manager Dispatch_process::disconnect_dispatch_device(std::shared_ptr & p_device, std::shared_ptr & p_device_task) { if ( p_device.get() != NULL && p_device_task.get() != NULL ) { p_device_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW); p_device.reset(); p_device_task.reset(); return Error_code::SUCCESS; } else { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } } //发送调度总计划 Error_manager Dispatch_process::send_dispatch_plan_request_msg() { std::unique_lock t_lock(m_lock); m_dispatch_plan_request_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_plan_request_msg); m_dispatch_plan_request_msg.mutable_base_info()->set_timeout_ms(m_timeout_ms); m_dispatch_plan_request_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager); m_dispatch_plan_request_msg.mutable_base_info()->set_receiver(message::Communicator::eDispatch_control); m_dispatch_plan_request_msg.set_command_key(m_dispatch_request_msg.command_key()); m_dispatch_plan_request_msg.set_dispatch_task_type((message::Dispatch_task_type)m_dispatch_process_type); m_dispatch_plan_request_msg.set_dispatch_source(m_dispatch_source); m_dispatch_plan_request_msg.set_dispatch_destination(m_dispatch_destination); //这里不写错误码 std::string t_msg = m_dispatch_plan_request_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); return Error_code::SUCCESS; } //执行调度控制指令, 并根据完成情况给答复 Error_manager Dispatch_process::dispatch_control_motion() { Error_manager t_error; std::unique_lock t_lock(m_lock); // m_result.error_manager_clear_all(); for (auto iter = m_dispatch_control_node_map.begin(); iter != m_dispatch_control_node_map.end(); ++iter) { switch ( iter->second.m_dispatch_control_status ) { case DISPATCH_CONTROL_STATUS_UNKNOW: case DISPATCH_CONTROL_CREATED: case DISPATCH_CONTROL_READY: { t_error = wait_dispatch_control_request_msg(iter->first, iter->second); if ( t_error == Error_code::SUCCESS) { //流程正常, 就进入完成状态, iter->second.m_dispatch_control_status = DISPATCH_CONTROL_CONNECT_DEVICE; } //else 原地待命 break; } case DISPATCH_CONTROL_CONNECT_DEVICE: { //连接调度设备 t_error = connect_dispatch_device(iter->first, iter->second); if ( t_error !=Error_code::SUCCESS) { iter->second.m_error = t_error; iter->second.m_dispatch_control_status = DISPATCH_CONTROL_RESPONSE; } else { //流程正常, 就进入工作状态, iter->second.m_dispatch_control_status = DISPATCH_CONTROL_WORKING; iter->second.m_time_to_send_control_response = std::chrono::system_clock::now(); } break; } case DISPATCH_CONTROL_WORKING: { //执行调度控制指令, 并根据完成情况给答复 t_error = excute_dispatch_control(iter->first, iter->second); if ( t_error == Error_code::NODATA ) { //继续 长流程, 什么也不做 if ( std::chrono::system_clock::now() - iter->second.m_time_to_send_control_response >= std::chrono::seconds(1) ) { //发送调度控制答复, 发给调度控制的 t_error = send_dispatch_control_response_msg(iter->first, iter->second, message::E_TASK_WORKING); iter->second.m_time_to_send_control_response = std::chrono::system_clock::now(); } } else { //长流程结束, 就答复 control_response_msg // 注:这里执行调度控制, 即使报错了 也要答复给调度控制, 交给调度控制来进行决策. iter->second.m_error = t_error; iter->second.m_dispatch_control_status = DISPATCH_CONTROL_TASK_WITHDRAW; } break; } case DISPATCH_CONTROL_TASK_WITHDRAW://流程 收回任务单 { //发送调度控制答复, 发给调度控制的 t_error = dispatch_control_withdraw_task(iter->first, iter->second); //流程正常, 就进入等待状态, 等待调度控制发送动作指令 iter->second.m_dispatch_control_status = DISPATCH_CONTROL_RESPONSE; break; } case DISPATCH_CONTROL_RESPONSE://流程 给调度控制答复 { //发送调度控制答复, 发给调度控制的 t_error = send_dispatch_control_response_msg(iter->first, iter->second, message::E_TASK_OVER); //流程正常, 就进入等待状态, 等待调度控制发送动作指令 iter->second.m_dispatch_control_status = DISPATCH_CONTROL_DISCONNECT_DEVICE; break; } case DISPATCH_CONTROL_DISCONNECT_DEVICE://流程 解除设备 { //断开调度设备, 释放任务单 与设备解除连接 t_error = disconnect_dispatch_device(iter->first, iter->second); if ( t_error == Error_code::SUCCESS ) { //流程正常, 就回到 等待状态, 等待调度控制发送动作指令 iter->second.m_error.error_manager_clear_all(); iter->second.m_dispatch_control_status = DISPATCH_CONTROL_READY; } //else 保持不变继续等待 break; } default: { break; } } } return Error_code::SUCCESS; } //等待控制指令 Error_manager Dispatch_process::wait_dispatch_control_request_msg(int dispatch_device_type, Dispatch_control_node & dispatch_control_node) { //key不相等 就表示 收到了新的控制指令 if ( dispatch_control_node.m_dispatch_control_request_msg.command_key() != dispatch_control_node.m_dispatch_control_response_msg.command_key() ) { return Error_code::SUCCESS; } else { return Error_code::NODATA; } return Error_code::SUCCESS; } //连接调度设备, 创建新的任务单 与设备建立连接 Error_manager Dispatch_process::connect_dispatch_device(int dispatch_device_type, Dispatch_control_node & dispatch_control_node) { Error_manager t_error; if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 || dispatch_device_type == message::Dispatch_device_type::ROBOT_2 ) { //找到对应的设备 if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 ) { dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_catcher_map[0]; } if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_2 ) { dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_catcher_map[1]; } Catcher* tp_catcher = (Catcher*)dispatch_control_node.mp_dispatch_device.get(); //检查设备状态 if ( tp_catcher->check_status() == Error_code::SUCCESS && tp_catcher->m_actual_device_status == Dispatch_device_base::HARDWARE_DEVICE_READY ) { //创建任务单 dispatch_control_node.mp_dispatch_task = std::shared_ptr(new Catcher_task); dispatch_control_node.mp_dispatch_task->task_init(NULL,std::chrono::milliseconds(15000)); Catcher_task * tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get(); //第一次发送 空的唯一码, 可以和设备建立联系 tp_catcher_task->m_request_key = ""; tp_catcher_task->m_request_x = tp_catcher->m_actual_x; tp_catcher_task->m_request_y = tp_catcher->m_actual_y; tp_catcher_task->m_request_b = tp_catcher->m_actual_b; tp_catcher_task->m_request_z = tp_catcher->m_actual_z; tp_catcher_task->m_request_d1 = tp_catcher->m_actual_d1; tp_catcher_task->m_request_d2 = tp_catcher->m_actual_d2; tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase; tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)tp_catcher->m_actual_clamp_motion1; t_error = tp_catcher->execute_task(dispatch_control_node.mp_dispatch_task, Dispatch_device_base::DISPATCH_TASK_ONE_LEVEL); if ( t_error != Error_code::SUCCESS ) { dispatch_control_node.m_error = t_error; return t_error; } } else { std::cout << " huli test :::: " << " 123123123123123123123 = " << 1233 << std::endl; std::cout << " huli test :::: " << " tp_catcher->check_status() = " << tp_catcher->check_status().to_string() << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_actual_device_status = " << tp_catcher->m_actual_device_status << std::endl; t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR, " tp_catcher->m_actual_device_status device_status error "); dispatch_control_node.m_error = t_error; return t_error; } //设置起点 dispatch_control_node.m_source_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates_map[dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()]; //设置终点 dispatch_control_node.m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates_map[dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination()]; std::cout << " huli test :::: " << " ---------------------------------------- = " << 123 << std::endl; std::cout << " huli test :::: " << " dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() = " << dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() << std::endl; std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.x = " << dispatch_control_node.m_destination_coordinates.x << std::endl; std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.y = " << dispatch_control_node.m_destination_coordinates.y << std::endl; std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.z = " << dispatch_control_node.m_destination_coordinates.z << std::endl; std::cout << " huli test :::: " << "*******************************************" << 123 << std::endl; } //搬运器的配置 准备工作 else if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 || dispatch_device_type == message::Dispatch_device_type::CARRIER_2 || dispatch_device_type == message::Dispatch_device_type::CARRIER_3 ) { //找到对应的设备 if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 ) { dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[0]; } if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_2 ) { dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[1]; } if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_3 ) { dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[2]; } Carrier* tp_carrier = (Carrier*)dispatch_control_node.mp_dispatch_device.get(); //检查设备状态 if ( tp_carrier->check_status() == Error_code::SUCCESS && tp_carrier->m_actual_device_status == Dispatch_device_base::HARDWARE_DEVICE_READY ) { //创建任务单 dispatch_control_node.mp_dispatch_task = std::shared_ptr(new Carrier_task); dispatch_control_node.mp_dispatch_task->task_init(NULL,std::chrono::milliseconds(15000)); Carrier_task * tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get(); //第一次发送 空的唯一码, 可以和设备建立联系 tp_carrier_task->m_request_key = ""; tp_carrier_task->m_request_x = tp_carrier->m_actual_x; tp_carrier_task->m_request_y = tp_carrier->m_actual_y; tp_carrier_task->m_request_z = tp_carrier->m_actual_z; tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y1; tp_carrier_task->m_request_y2 = tp_carrier->m_actual_y2; tp_carrier_task->m_request_clamp_motion = (Carrier_task::Clamp_motion)tp_carrier->m_actual_clamp_motion1; tp_carrier_task->m_request_joint_motion_x = (Carrier_task::Joint_motion)tp_carrier->m_actual_joint_motion_x1; tp_carrier_task->m_request_joint_motion_y = Carrier_task::Joint_motion::E_JOINT_NO_ACTION; tp_carrier_task->m_request_space_id = 0; tp_carrier_task->m_request_floor_id = 0; tp_carrier_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase; t_error = tp_carrier->execute_task(dispatch_control_node.mp_dispatch_task, Dispatch_device_base::DISPATCH_TASK_ONE_LEVEL); if ( t_error != Error_code::SUCCESS ) { dispatch_control_node.m_error = t_error; return t_error; } } else { std::cout << " huli test :::: " << " 123123123123123123123 = " << 4566 << std::endl; std::cout << " huli test :::: " << " tp_carrier->check_status() = " << tp_carrier->check_status().to_string() << std::endl; std::cout << " huli test :::: " << " tp_carrier->m_actual_device_status = " << tp_carrier->m_actual_device_status << std::endl; t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR, " tp_carrier->m_actual_device_status device_status error "); dispatch_control_node.m_error = t_error; return t_error; } //设置起点 dispatch_control_node.m_source_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates_map[dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()]; //设置终点 dispatch_control_node.m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates_map[dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination()]; std::cout << " huli test :::: " << " ---------------------------------------- = " << 123 << std::endl; std::cout << " huli test :::: " << " dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() = " << dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() << std::endl; std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.x = " << dispatch_control_node.m_destination_coordinates.x << std::endl; std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.y = " << dispatch_control_node.m_destination_coordinates.y << std::endl; std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.z = " << dispatch_control_node.m_destination_coordinates.z << std::endl; std::cout << " huli test :::: " << "*******************************************" << 123 << std::endl; } else { t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_TYPE_ERROR, Error_level::MINOR_ERROR, " DISPATCH_PROCESS_DEVICE_TYPE_ERROR fun error "); dispatch_control_node.m_error = t_error; return t_error; } return Error_code::SUCCESS; } //执行调度控制指令, 并根据完成情况给答复 Error_manager Dispatch_process::excute_dispatch_control(int dispatch_device_type, Dispatch_control_node & dispatch_control_node) { Error_manager t_error; if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 || dispatch_device_type == message::Dispatch_device_type::ROBOT_2 ) { Catcher_task * tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get(); // while (1) // { // char in; // std::cin >> in ; // if ( in == 'p' ) // { // break; // } // } // std::cout << " huli test :::: " << "ppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppp = " << tp_catcher_task->m_step << std::endl; // std::cout << " huli test :::: " << "1111111 tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl; //设备的动作也使用外部的Main()的线程来循环 switch ( dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type() ) { case message::Dispatch_task_type::ROBOT_CATCH_CAR_FROM_INLET: { t_error = excute_robot_catch_car_from_inlet(dispatch_control_node); break; } case message::Dispatch_task_type::ROBOT_PUT_CAR_TO_CARRIER: { t_error = excute_robot_put_car_to_carrier(dispatch_control_node); break; } case message::Dispatch_task_type::ROBOT_CATCH_CAR_FROM_CARRIER: { t_error = excute_robot_catch_car_from_carrier(dispatch_control_node); break; } case message::Dispatch_task_type::ROBOT_PUT_CAR_TO_OUTLET: { t_error = excute_robot_put_car_to_outlet(dispatch_control_node); break; } case message::Dispatch_task_type::ROBOT_MOVE: { t_error = excute_robot_move(dispatch_control_node); break; } default: { return Error_manager(Error_code::DISPATCH_PROCESS_TASK_STATUS_ERROR, Error_level::MINOR_ERROR, "Dispatch_process::excute_dispatch_control() fun error "); break; } } // std::cout << " huli test :::: " << "222222 tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl; // std::cout << " huli test :::: " << " t_error_catcher = " << t_error.to_string() << std::endl; } else if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 || dispatch_device_type == message::Dispatch_device_type::CARRIER_2 || dispatch_device_type == message::Dispatch_device_type::CARRIER_3 ) { Catcher_task *tp_carrier_task = (Catcher_task *) dispatch_control_node.mp_dispatch_task.get(); // while (1) // { // char in; // std::cin >> in; // if (in == 'l') // { // break; // } // } // std::cout << " huli test :::: " // << "lllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll = " // << tp_carrier_task->m_step << std::endl; // std::cout << " huli test :::: " << "3333333 tp_carrier_task->m_step = " << tp_carrier_task->m_step // << std::endl; //设备的动作也使用外部的Main()的线程来循环 switch (dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type()) { case message::Dispatch_task_type::CARRIER_RECEIVE_CAR_FROM_ROBOT: { t_error = excute_carrier_receive_car_from_robot(dispatch_control_node); break; } case message::Dispatch_task_type::CARRIER_STORE_CAR_TO_PARKINGSPACE: { t_error = excute_carrier_store_car_to_parkingspace(dispatch_control_node); break; } case message::Dispatch_task_type::CARRIER_STORE_CAR_TO_PARKINGSPACE_EX: { t_error = excute_carrier_store_car_to_parkingspace_ex(dispatch_control_node); break; } case message::Dispatch_task_type::CARRIER_PICKUP_CAR_FROM_PARKINGSPACE: { t_error = excute_carrier_pickup_car_from_parkingspace(dispatch_control_node); break; } case message::Dispatch_task_type::CARRIER_PICKUP_CAR_FROM_PARKINGSPACE_EX: { t_error = excute_carrier_pickup_car_from_parkingspace_ex(dispatch_control_node); break; } case message::Dispatch_task_type::CARRIER_DELIVER_CAR_TO_ROBOT: { t_error = excute_carrier_deliver_car_to_robot(dispatch_control_node); break; } case message::Dispatch_task_type::CARRIER_MOVE: { t_error = excute_carrier_move(dispatch_control_node); break; } default: { return Error_manager(Error_code::DISPATCH_PROCESS_TASK_STATUS_ERROR, Error_level::MINOR_ERROR, "Dispatch_process::excute_dispatch_control() fun error "); break; } } // std::cout << " huli test :::: " << "44444444 tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl; // std::cout << " huli test :::: " << " t_error_carrier = " << t_error.to_string() << std::endl; } dispatch_control_node.m_error = t_error; return t_error; } //流程通知设备 收回任务单 Error_manager Dispatch_process::dispatch_control_withdraw_task(int dispatch_device_type, Dispatch_control_node & dispatch_control_node) { if ( dispatch_control_node.mp_dispatch_device.get() != NULL && dispatch_control_node.mp_dispatch_task.get() != NULL ) { dispatch_control_node.mp_dispatch_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW); } return Error_code::SUCCESS; } //发送调度控制答复, 发给调度控制的 Error_manager Dispatch_process::send_dispatch_control_response_msg(int dispatch_device_type, Dispatch_control_node & dispatch_control_node, message::Dispatch_device_task_status dispatch_device_task_status) { dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_control_response_msg); dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_timeout_ms(dispatch_control_node.m_dispatch_control_request_msg.base_info().timeout_ms()); dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager); dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_receiver(message::Communicator::eDispatch_control); dispatch_control_node.m_dispatch_control_response_msg.set_command_key(dispatch_control_node.m_dispatch_control_request_msg.command_key()); dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_task_type(dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type()); dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_type(dispatch_control_node.m_dispatch_control_request_msg.dispatch_device_type()); dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_source(dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()); dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_destination(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination()); dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_code(dispatch_control_node.m_error.get_error_code()); dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_level((message::Error_level)dispatch_control_node.m_error.get_error_level()); dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_description(dispatch_control_node.m_error.get_error_description()); dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_target_status(dispatch_control_node.m_dispatch_control_request_msg.dispatch_device_target_status()); dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_task_status(dispatch_device_task_status); std::string t_msg = dispatch_control_node.m_dispatch_control_response_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); if ( dispatch_device_task_status == message::Dispatch_device_task_status::E_TASK_OVER ) { LOG(INFO) << " dispatch_control_node.m_dispatch_control_response_msg = "<< dispatch_control_node.m_dispatch_control_response_msg.DebugString() << " " << this; } return Error_code::SUCCESS; } //断开调度设备, 释放任务单 与设备解除连接 Error_manager Dispatch_process::disconnect_dispatch_device(int dispatch_device_type, Dispatch_control_node & dispatch_control_node) { if ( dispatch_control_node.mp_dispatch_device.get() != NULL && dispatch_control_node.mp_dispatch_task.get() != NULL ) { if ( dispatch_control_node.mp_dispatch_task->get_task_statu() == Task_Base::Task_statu::TASK_FREE ) { dispatch_control_node.mp_dispatch_task.reset(); dispatch_control_node.mp_dispatch_device.reset(); return Error_code::SUCCESS; } else { return Error_code::NODATA; } } return Error_code::SUCCESS; } //等待调度总计划答复 Error_manager Dispatch_process::wait_dispatch_plan_response_msg() { std::unique_lock t_lock(m_lock); //key 相等 就表示 收到了总计划答复 if ( m_dispatch_plan_request_msg.command_key() == m_dispatch_plan_response_msg.command_key() ) { return Error_code::SUCCESS; } else { return Error_code::NODATA; } } //发送调度答复, 发给主控的 Error_manager Dispatch_process::send_dispatch_response_msg() { std::unique_lock t_lock(m_lock); m_dispatch_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_response_msg); m_dispatch_response_msg.mutable_base_info()->set_timeout_ms(m_dispatch_request_msg.base_info().timeout_ms()); m_dispatch_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager); m_dispatch_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain); m_dispatch_response_msg.set_command_key(m_dispatch_request_msg.command_key()); m_dispatch_response_msg.set_dispatch_motion_direction(m_dispatch_request_msg.dispatch_motion_direction()); m_dispatch_response_msg.set_terminal_id(m_dispatch_request_msg.terminal_id()); m_dispatch_response_msg.mutable_locate_information()->CopyFrom(m_dispatch_request_msg.locate_information()); m_dispatch_response_msg.mutable_parkspace_info_ex()->CopyFrom(m_dispatch_request_msg.parkspace_info_ex()); m_dispatch_response_msg.set_car_type(m_dispatch_request_msg.car_type()); if ( m_dispatch_response_msg.dispatch_motion_direction() == message::E_STORE_CAR ) { for (int i = 0; i < m_dispatch_response_msg.parkspace_info_ex_size(); ++i) { if ( m_dispatch_destination == m_dispatch_response_msg.parkspace_info_ex(i).parkingspace_index_id() ) { m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_occupied); } else { m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_empty); } } } else if( m_dispatch_response_msg.dispatch_motion_direction() == message::E_PICKUP_CAR ) { for (int i = 0; i < m_dispatch_response_msg.parkspace_info_ex_size(); ++i) { if ( m_dispatch_source == m_dispatch_response_msg.parkspace_info_ex(i).parkingspace_index_id() ) { m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_occupied); } else { m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_empty); } } } m_dispatch_response_msg.mutable_error_manager()->set_error_code(m_result.get_error_code()); m_dispatch_response_msg.mutable_error_manager()->set_error_level((message::Error_level)m_result.get_error_level()); m_dispatch_response_msg.mutable_error_manager()->set_error_description(m_result.get_error_description()); std::string t_msg = m_dispatch_response_msg.SerializeAsString(); System_communication::get_instance_references().encapsulate_msg(t_msg); return Error_code::SUCCESS; } //通知调度管理, 释放资源, Error_manager Dispatch_process::release_resource() { return Dispatch_manager::get_instance_references().release_dispatch_process(m_command_key); } //异常处理 Error_manager Dispatch_process::Exception_handling() { LOG(INFO) << " -------------Dispatch_process::Exception_handling------------------- "<< this; LOG(INFO) << " m_result = "<< m_result.to_string() << " " << this; return Error_code::SUCCESS; } //机器手去入口抓车(例如:机器手移动到2号入口上方,然后下降到一楼抓车,最后上升到最上方) Error_manager Dispatch_process::excute_robot_catch_car_from_inlet(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Catcher * tp_catcher = NULL; Catcher_task * tp_catcher_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get(); tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl; // getchar(); if ( tp_catcher_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 1 )//检查姿态 { if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE || tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance > tp_dispatch_coordinates->m_catcher_wheel_base_limit || tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW ) { catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } else { tp_catcher_task->m_step +=2; } } else { return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() fun error "); } } if ( tp_catcher_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 3 )//机器手z轴移到最高点. { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 5 )//机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车. { catcher_adjust_from_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 7 )//机器手z轴移到最低点. { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_1th_floor_z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 9 )//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. { float temp_wheel_base = tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance; //修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_catcher_task->m_step +=2; } else { catcher_adjust_wheel_base(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } } if ( tp_catcher_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 11 )//机器手 夹车 { catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 13 )//机器手 z轴上升到最高处 { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 14 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 15 )//机器手调整到 准备把车放到搬运器的姿态 { //这里机器手要反向, catcher_adjust_to_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, true); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 16 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 17 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //机器手把车放到中跑车上面(例如:机器手下降到中跑车上放车,最后上升到最上方)(通过目标点 指定放到哪一个中跑车上) Error_manager Dispatch_process::excute_robot_put_car_to_carrier(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Catcher * tp_catcher = NULL; Catcher_task * tp_catcher_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get(); tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl; // getchar(); if ( tp_catcher_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 1 )//检查姿态 { if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR && tp_catcher->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT && (Common_data::approximate_difference(tp_catcher->m_actual_b, 90, DISPATCH_DEFAULT_DIFFERENCE) || Common_data::approximate_difference(tp_catcher->m_actual_b, 270, DISPATCH_DEFAULT_DIFFERENCE) ) ) { //检测正常, 直接跳过即可 tp_catcher_task->m_step +=2; } else { return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_put_car_to_carrier() fun error "); } } if ( tp_catcher_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴 { if ( tp_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z ) { //检测正常, 直接跳过即可 tp_catcher_task->m_step +=2; } else { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); tp_catcher_task->m_step++; } } if ( tp_catcher_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到搬运器的姿态 { float temp_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) && Common_data::approximate_difference(tp_catcher->m_actual_y, temp_y, DISPATCH_DEFAULT_DIFFERENCE)) { //检测正常, 直接跳过即可 tp_catcher_task->m_step +=2; } else { catcher_adjust_to_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, false); tp_catcher_task->m_step++; } } if ( tp_catcher_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降 { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 9 )//机器手 松开夹杆 { catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 11 )//机器手 z轴上升 { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 13 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //机器手去中跑车上抓车(例如:机器手移动到1号中跑车上方,然后下降到中跑车抓车,最后上升到最上方) Error_manager Dispatch_process::excute_robot_catch_car_from_carrier(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Catcher * tp_catcher = NULL; Catcher_task * tp_catcher_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get(); tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl; // getchar(); if ( tp_catcher_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 1 ) { if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE || tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance > tp_dispatch_coordinates->m_catcher_wheel_base_limit || tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW ) { catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } else { tp_catcher_task->m_step +=2; } } else { return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_carrier() fun error "); } } if ( tp_catcher_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴 { if ( tp_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z ) { //检测正常, 直接跳过即可 tp_catcher_task->m_step +=2; } else { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); tp_catcher_task->m_step++; } } if ( tp_catcher_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到搬运器的姿态 { float temp_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) && Common_data::approximate_difference(tp_catcher->m_actual_y, temp_y, DISPATCH_DEFAULT_DIFFERENCE)) { //检测正常, 直接跳过即可 tp_catcher_task->m_step +=2; } else { catcher_adjust_from_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } } if ( tp_catcher_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降 { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 9 )//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. { float temp_wheel_base = tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance; //修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_catcher_task->m_step +=2; } else { catcher_adjust_wheel_base(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } } if ( tp_catcher_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 11 )//机器手 夹车 { catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 13 )//机器手 z轴上升 { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 14 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 15 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //机器手去出口放车(例如:机器手移动到3号出口上方,然后下降到一楼放车,最后上升到最上方) Error_manager Dispatch_process::excute_robot_put_car_to_outlet(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Catcher * tp_catcher = NULL; Catcher_task * tp_catcher_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get(); tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl; // getchar(); if ( tp_catcher_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 1 )//检查姿态 { if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR && tp_catcher->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT && (Common_data::approximate_difference(tp_catcher->m_actual_b, 90, DISPATCH_DEFAULT_DIFFERENCE) || Common_data::approximate_difference(tp_catcher->m_actual_b, 270, DISPATCH_DEFAULT_DIFFERENCE) ) ) { //检测正常, 直接跳过即可 tp_catcher_task->m_step +=2; } else { return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_put_car_to_carrier() fun error "); } } if ( tp_catcher_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴 { if ( tp_catcher->m_actual_z*1.02 >= tp_dispatch_coordinates->m_catcher_4th_floor_z ) { //检测正常, 直接跳过即可 tp_catcher_task->m_step +=2; } else { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); tp_catcher_task->m_step++; } } if ( tp_catcher_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到地面的姿态 { if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) && Common_data::approximate_difference(tp_catcher->m_actual_y, dispatch_control_node.m_destination_coordinates.y, DISPATCH_DEFAULT_DIFFERENCE)) { //检测正常, 直接跳过即可 tp_catcher_task->m_step +=2; } else { catcher_adjust_to_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } } if ( tp_catcher_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降 { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 9 )//机器手 松开夹杆 { catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 11 )//机器手 z轴上升 { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 13 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //机器手的自由移动(例如:机器手移动到6号出入口的上方4楼处,给其他设备避让)(不进行抓车和放车的操作) Error_manager Dispatch_process::excute_robot_move(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Catcher * tp_catcher = NULL; Catcher_task * tp_catcher_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get(); tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get(); } if ( tp_catcher_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 1 )//检查姿态 { if ( tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance > tp_dispatch_coordinates->m_catcher_wheel_base_limit || tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW ) { catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } else { tp_catcher_task->m_step +=2; } } if ( tp_catcher_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴 { catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到地面的姿态 { catcher_adjust_to_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates); tp_catcher_task->m_step++; } if ( tp_catcher_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step); } if ( tp_catcher_task->m_step == 7 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //搬运器从机器手上接车(例如:搬运器移动到2号入口的上方,然后等待机器手把车放到中跑车上,小跑车保持松夹杆状态) Error_manager Dispatch_process::excute_carrier_receive_car_from_robot(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Carrier * tp_carrier = NULL; Carrier_task * tp_carrier_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get(); tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_carrier_task->m_step << std::endl; // getchar(); if ( tp_carrier_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 1 )//检查姿态 { //检查姿态 if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE && Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE)) { tp_carrier_task->m_step +=2; } else { carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates); tp_carrier_task->m_step++; } } else { return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error "); } } if ( tp_carrier_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上 { if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " m_destination_coordinates.z PARAMRTER ERROR "); } } } if ( tp_carrier_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 11 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2) { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); tp_carrier_task->m_step++; } else { tp_carrier_task->m_step +=2; } } if ( tp_carrier_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 14 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 15 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至电梯井) Error_manager Dispatch_process::excute_carrier_store_car_to_parkingspace(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Carrier * tp_carrier = NULL; Carrier_task * tp_carrier_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get(); tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_carrier_task->m_step << std::endl; // getchar(); if ( tp_carrier_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 1 )//检查姿态 { //检查姿态(注注注注注意了, 调试阶段是无车, 真实环境是有车的) if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR && tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE) { //检测正常, 直接跳过即可 tp_carrier_task->m_step +=2; } //注意了, 如果是去7号出口, 那么就直接跳过轮距修正和夹车. else if(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() == 1107) { tp_carrier_task->m_step = 7; } else { return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "Dispatch_process::excute_carrier_store_car_to_parkingspace() fun error "); } } if ( tp_carrier_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 3 )//修正轴距 { float temp_wheel_base = tp_carrier->m_actual_y1 - tp_carrier->m_actual_y2; if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_adjust_wheel_base(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 5 )//让小跑车 夹车 { carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 7 )//让中跑车回到电梯井 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " m_destination_coordinates.z PARAMRTER ERROR "); } } } if ( tp_carrier_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 9 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 11 )//电梯移动到对应的楼层 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 13 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2) { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); tp_carrier_task->m_step++; } else { tp_carrier_task->m_step +=2; } } if ( tp_carrier_task->m_step == 14 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 15 )//中跑车 x轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 16 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 17 )//小跑车 进入车位 { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 18 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 19 )//小跑车 松开夹杆 { carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 20 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 21 )//小跑车 回到中跑车 { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 22 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 23 )//让中跑车回到电梯井 { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " m_destination_coordinates.z PARAMRTER ERROR "); } } if ( tp_carrier_task->m_step == 24 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 25 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 26 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 27 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至56车位外面即可) Error_manager Dispatch_process::excute_carrier_store_car_to_parkingspace_ex(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Carrier * tp_carrier = NULL; Carrier_task * tp_carrier_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get(); tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl; // getchar(); if ( tp_carrier_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 1 )//检查姿态 { //检查姿态(注注注注注意了, 调试阶段是无车, 真实环境是有车的) if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR && tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE) { //检测正常, 直接跳过即可 tp_carrier_task->m_step +=2; } //注意了, 如果是去7号出口, 那么就直接跳过轮距修正和夹车. else if(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() == 1107) { tp_carrier_task->m_step = 7; } else { return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "Dispatch_process::excute_carrier_store_car_to_parkingspace() fun error "); } } if ( tp_carrier_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 3 )//修正轴距 { float temp_wheel_base = tp_carrier->m_actual_y1 - tp_carrier->m_actual_y2; if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_adjust_wheel_base(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 5 )//让小跑车 夹车 { carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 7 )//让中跑车回到电梯井 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR "); } } } if ( tp_carrier_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 9 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 11 )//电梯移动到对应的楼层 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 13 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2) { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); tp_carrier_task->m_step++; } else { tp_carrier_task->m_step +=2; } } if ( tp_carrier_task->m_step == 14 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 15 )//中跑车 x轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 16 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 17 )//小跑车 进入车位 { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 18 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 19 )//小跑车 松开夹杆 { carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 20 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 21 )//小跑车 回到中跑车 { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 22 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 23 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至电梯井) Error_manager Dispatch_process::excute_carrier_pickup_car_from_parkingspace(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Carrier * tp_carrier = NULL; Carrier_task * tp_carrier_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get(); tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl; // getchar(); if ( tp_carrier_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 1 )//检查姿态 { //检查姿态 if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE && Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE)) { tp_carrier_task->m_step +=2; } else { carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates); tp_carrier_task->m_step++; } } else { return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error "); } } if ( tp_carrier_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上 { if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR "); } } } if ( tp_carrier_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 11 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() != 2) { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); tp_carrier_task->m_step++; } else { tp_carrier_task->m_step +=2; } } if ( tp_carrier_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 14 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 15 )//小跑车 进入车位 { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 16 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 17 )//小跑车 夹车 { carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 18 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 19 )//小跑车 回到中跑车 { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 20 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 21 )//让中跑车回到电梯井 { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR "); } } if ( tp_carrier_task->m_step == 22 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 23 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { if ( tp_carrier->get_device_id() != 2 ) { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } else { tp_carrier_task->m_step +=2; } } if ( tp_carrier_task->m_step == 24 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 25 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至56车位外面即可) Error_manager Dispatch_process::excute_carrier_pickup_car_from_parkingspace_ex(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Carrier * tp_carrier = NULL; Carrier_task * tp_carrier_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get(); tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl; // getchar(); if ( tp_carrier_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 1 )//检查姿态 { //检查姿态 if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR ) { if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE && Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE)) { tp_carrier_task->m_step +=2; } else { carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates); tp_carrier_task->m_step++; } } else { return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error "); } } if ( tp_carrier_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上 { if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR "); } } } if ( tp_carrier_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 11 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() != 2) { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); tp_carrier_task->m_step++; } else { tp_carrier_task->m_step +=2; } } if ( tp_carrier_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 14 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 15 )//小跑车 进入车位 { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 16 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 17 )//小跑车 夹车 { carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 18 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 19 )//小跑车 回到中跑车 { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 20 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 21 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //搬运器把车交付给机器手(例如:搬运器移动到3号入口的上方,小跑车松夹杆,然后等待机器手把车从中跑车上取走) Error_manager Dispatch_process::excute_carrier_deliver_car_to_robot(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Carrier * tp_carrier = NULL; Carrier_task * tp_carrier_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get(); tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl; // getchar(); if ( tp_carrier_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 1 )//检查姿态 { //检查姿态 if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR && tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT) { //检测正常, 直接跳过 tp_carrier_task->m_step +=2; } else { return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR, "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error "); } } if ( tp_carrier_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上 { if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR "); } } } if ( tp_carrier_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 7 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 11 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2) { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); tp_carrier_task->m_step++; } else { tp_carrier_task->m_step +=2; } } if ( tp_carrier_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 14 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 15 )//小跑车 松开夹杆 { carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE); tp_carrier_task->m_step++; } if ( tp_carrier_task->m_step == 16 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 17 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //搬运器的自由移动(可以提前到2楼来准备接车,或者为了避让就退回至电梯井)(小跑车不进行取车和存车) Error_manager Dispatch_process::excute_carrier_move(Dispatch_control_node & dispatch_control_node) { Error_manager t_error; Carrier * tp_carrier = NULL; Carrier_task * tp_carrier_task = NULL; Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL "); } else { tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get(); tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get(); } // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl; // getchar(); if ( tp_carrier_task->m_step == 0 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 1 )//让小跑车回到中跑车上 { if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 2 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 3 )//让中跑车回到电梯井 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left); tp_carrier_task->m_step++; } else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 ) { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right); tp_carrier_task->m_step++; } else { return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR, " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR "); } } } if ( tp_carrier_task->m_step == 4 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 5 )//收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 6 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 7 )//电梯移动到对应的楼层 { if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 8 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 9 )//伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动 { if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK && tp_carrier->get_device_id() !=2) { carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT); tp_carrier_task->m_step++; } else { tp_carrier_task->m_step +=2; } } if ( tp_carrier_task->m_step == 10 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 11 )//中跑车 x轴移动 { if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) ) { tp_carrier_task->m_step +=2; } else { carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x); tp_carrier_task->m_step++; } } if ( tp_carrier_task->m_step == 12 ) { return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step); } if ( tp_carrier_task->m_step == 13 ) { return Error_code::SUCCESS; } return Error_code::SUCCESS; } //执行通道口动作 Error_manager Dispatch_process::excute_passageway_motion(Dispatch_control_node & dispatch_control_node) { return Error_code::SUCCESS; } //检查 任务单 是否完成任务, 里面不会调整短步骤 Error_manager Dispatch_process::check_task_status(std::shared_ptr p_task) { if ( p_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::check_passageway_task POINTER IS NULL "); } else { if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER ) { // dispatch_control_status = (Dispatch_control_status)(dispatch_control_status+1); // return Error_code::NODATA; //这里返回nodata 表示任务继续下一步 //注意了, 这里返回成功, 用作动作完成判断 return Error_code::SUCCESS; } else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR ) { // dispatch_control_status = DISPATCH_CONTROL_FAULT; return p_task->get_task_error_manager(); } else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING || p_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED || p_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED || p_task->get_task_statu() == Task_Base::Task_statu::TASK_STOP) { //继续等待任务, 直到状态改变 return Error_code::NODATA; } else { LOG(INFO) << "Dispatch_process::check_task_ex failed p_catcher_task->get_task_statu() = "<< p_task->get_task_statu() << " " << this; return Error_code::NODATA; } } return Error_code::SUCCESS; }//检查 任务单 是否完成任务, 里面会调整短步骤 Error_manager Dispatch_process::check_task_status(std::shared_ptr p_task, Dispatch_control_status & dispatch_control_status) { if ( p_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::check_passageway_task POINTER IS NULL "); } else { if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER ) { dispatch_control_status = (Dispatch_control_status)(dispatch_control_status+1); // return Error_code::NODATA; //这里返回nodata 表示任务继续下一步 //注意了, 这里返回成功, 用作动作完成判断 return Error_code::SUCCESS; } else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR ) { dispatch_control_status = DISPATCH_CONTROL_FAULT; return p_task->get_task_error_manager(); } else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING || p_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED || p_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED || p_task->get_task_statu() == Task_Base::Task_statu::TASK_STOP) { //继续等待任务, 直到状态改变 return Error_code::NODATA; } else { LOG(INFO) << "Dispatch_process::check_task_ex failed p_catcher_task->get_task_statu() = "<< p_task->get_task_statu() << " " << this; return Error_code::NODATA; } } return Error_code::SUCCESS; } //检查 任务单 是否完成任务, 里面会调整短步骤 Error_manager Dispatch_process::check_task_ex(std::shared_ptr p_task, int& step) { if ( p_task.get() == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, "Dispatch_process::check_passageway_task POINTER IS NULL "); } else { if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER ) { step++; return Error_code::NODATA; //这里返回nodata 表示任务继续下一步 } else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR ) { step = 0; return p_task->get_task_error_manager(); } else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING || p_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED || p_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED || p_task->get_task_statu() == Task_Base::Task_statu::TASK_STOP) { //继续等待任务, 直到状态改变 return Error_code::NODATA; } else { LOG(INFO) << "Dispatch_process::check_task_ex failed p_catcher_task->get_task_statu() = "<< p_task->get_task_statu() << " " << this; return Error_code::NODATA; } } return Error_code::SUCCESS; } //机器手调整到正常待机的姿态(调节夹杆和轴距) Error_manager Dispatch_process::catcher_adjust_to_ready(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step); tp_catcher_task->m_request_key = t_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //调整姿态 tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE; tp_catcher_task->m_request_d1 = tp_dispatch_coordinates->m_catcher_d1_min; tp_catcher_task->m_request_d2 = tp_dispatch_coordinates->m_catcher_d2_min; if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 270; } else { tp_catcher_task->m_request_b = 90; } std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl; return Error_code::SUCCESS; } //机器手 移动z Error_manager Dispatch_process::catcher_move_z(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target) { std::unique_lock t_lock2(tp_catcher_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step); tp_catcher_task->m_request_key = t_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 移动z tp_catcher_task->m_request_z = target; std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_z = " << tp_catcher_task->m_request_z << std::endl; return Error_code::SUCCESS; } //机器手调整到 准备从地面抓车前的姿态 Error_manager Dispatch_process::catcher_adjust_from_ground(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step); tp_catcher_task->m_request_key = t_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车. tp_catcher_task->m_request_x = m_car_measure_information.center_x; tp_catcher_task->m_request_y = m_car_measure_information.center_y; if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { //机器手如果旋转反向, 那就+180即可, 没有必要旋转归位. tp_catcher_task->m_request_b = 180 + m_car_measure_information.car_angle; } else { tp_catcher_task->m_request_b = m_car_measure_information.car_angle; } //限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升. if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit ) { tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit; } else { tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = m_wheel_base; } tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE; std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl; std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. Error_manager Dispatch_process::catcher_adjust_wheel_base(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step); tp_catcher_task->m_request_key = t_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //修正轴距 tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = m_wheel_base; std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl; return Error_code::SUCCESS; } //机器手 移动c轴 夹杆 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_catcher_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step); tp_catcher_task->m_request_key = t_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); tp_catcher_task->m_request_clamp_motion = target; std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //机器手调整到 准备把车放到搬运器的姿态 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_catcher_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step); tp_catcher_task->m_request_key = t_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上. tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x; tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); //存车需要反向, 取车不需要 if ( reverse_flag ) { if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 90; } else { tp_catcher_task->m_request_b = 270; } } else { if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 270; } else { tp_catcher_task->m_request_b = 90; } } std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl; return Error_code::SUCCESS; } //机器手调整到 准备把车放到地面的姿态 Error_manager Dispatch_process::catcher_adjust_to_ground(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step); tp_catcher_task->m_request_key = t_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上. tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x; tp_catcher_task->m_request_y = dispatch_control_node.m_destination_coordinates.y; if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 270; } else { tp_catcher_task->m_request_b = 90; } std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl; return Error_code::SUCCESS; } //机器手调整到 对接搬运器的姿态 Error_manager Dispatch_process::catcher_adjust_from_carrier(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step); tp_catcher_task->m_request_key = t_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上. tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x; tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 270; } else { tp_catcher_task->m_request_b = 90; } //限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升. if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit ) { tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit; } else { tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = m_wheel_base; } tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE; std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl; std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //搬运器调整到 正常待机的姿态(调节夹杆和轴距) Error_manager Dispatch_process::carrier_adjust_to_ready(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_carrier_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step); tp_carrier_task->m_request_key = t_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_clamp_motion = Carrier_task::Clamp_motion::E_CLAMP_LOOSE; tp_carrier_task->m_request_wheelbase = m_wheel_base; tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance; tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase; std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //搬运器 移动x Error_manager Dispatch_process::carrier_move_x(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target) { std::unique_lock t_lock2(tp_carrier_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step); tp_carrier_task->m_request_key = t_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_x = target; std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_x = " << tp_carrier_task->m_request_x << std::endl; return Error_code::SUCCESS; } //搬运器 移动y Error_manager Dispatch_process::carrier_move_y(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target) { std::unique_lock t_lock2(tp_carrier_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step); tp_carrier_task->m_request_key = t_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_y = target; tp_carrier_task->m_request_wheelbase = m_wheel_base; tp_carrier_task->m_request_y1 = tp_carrier_task->m_request_y + tp_dispatch_coordinates->m_carrier_y_y1_distance; tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase; std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y = " << tp_carrier_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl; return Error_code::SUCCESS; } //搬运器 移动z Error_manager Dispatch_process::carrier_move_z(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target) { std::unique_lock t_lock2(tp_carrier_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step); tp_carrier_task->m_request_key = t_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_z = target; std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_z = " << tp_carrier_task->m_request_z << std::endl; return Error_code::SUCCESS; } //搬运器 移动c轴 夹车杆 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_carrier_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step); tp_carrier_task->m_request_key = t_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_clamp_motion = target; std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //搬运器调整 水平的交接 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_carrier_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step); tp_carrier_task->m_request_key = t_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_joint_motion_x = target; std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_joint_motion_x = " << tp_carrier_task->m_request_joint_motion_x << std::endl; return Error_code::SUCCESS; } //搬运器 修正轴距 Error_manager Dispatch_process::carrier_adjust_wheel_base(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_carrier_task->m_lock); char t_key[50] = {0}; sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step); tp_carrier_task->m_request_key = t_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_wheelbase = m_wheel_base; tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance; tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase; std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl; return Error_code::SUCCESS; } //机器手 准备开始, 需要同步任务单和设备真实数据. Error_manager Dispatch_process::catcher_ready_to_start(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); tp_catcher_task->m_request_x = tp_catcher->m_actual_x; tp_catcher_task->m_request_y = tp_catcher->m_actual_y; tp_catcher_task->m_request_b = tp_catcher->m_actual_b; tp_catcher_task->m_request_z = tp_catcher->m_actual_z; tp_catcher_task->m_request_d1 = tp_catcher->m_actual_d1; tp_catcher_task->m_request_d2 = tp_catcher->m_actual_d2; tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase; tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)tp_catcher->m_actual_clamp_motion1; return Error_code::SUCCESS; } //机器手调整到正常待机的姿态(调节夹杆和轴距) Error_manager Dispatch_process::catcher_adjust_to_ready(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //调整姿态 tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE; tp_catcher_task->m_request_d1 = tp_dispatch_coordinates->m_catcher_d1_min; tp_catcher_task->m_request_d2 = tp_dispatch_coordinates->m_catcher_d2_min; if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 270; } else { tp_catcher_task->m_request_b = 90; } std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl; return Error_code::SUCCESS; } //机器手 移动x Error_manager Dispatch_process::catcher_move_x(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 移动x tp_catcher_task->m_request_x = target_x; std::cout << " huli test :::: " << " catcher_move_x = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_move_x = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; return Error_code::SUCCESS; } //机器手 移动y Error_manager Dispatch_process::catcher_move_y(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_y) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 移动y tp_catcher_task->m_request_y = target_y; std::cout << " huli test :::: " << " catcher_move_y = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_move_y = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; return Error_code::SUCCESS; } //机器手 移动z Error_manager Dispatch_process::catcher_move_z(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_z) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 移动z tp_catcher_task->m_request_z = target_z; std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_z = " << tp_catcher_task->m_request_z << std::endl; return Error_code::SUCCESS; } //机器手调整到 准备从地面抓车前的姿态 Error_manager Dispatch_process::catcher_adjust_from_ground(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车. tp_catcher_task->m_request_x = m_car_measure_information.center_x; tp_catcher_task->m_request_y = m_car_measure_information.center_y; if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { //机器手如果旋转反向, 那就+180即可, 没有必要旋转归位. tp_catcher_task->m_request_b = 180 + m_car_measure_information.car_angle; } else { tp_catcher_task->m_request_b = m_car_measure_information.car_angle; } //限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升. if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit ) { tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit; } else { tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = m_wheel_base; } tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE; std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl; std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距. Error_manager Dispatch_process::catcher_adjust_wheel_base(std::string dispatch_control_command_key, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //修正轴距 tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = m_wheel_base; std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl; return Error_code::SUCCESS; } //机器手 移动c轴 夹杆 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); tp_catcher_task->m_request_clamp_motion = target; std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //机器手调整到 准备把车放到搬运器的姿态 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上. tp_catcher_task->m_request_x = target_x; tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); //存车需要反向, 取车不需要 if ( reverse_flag ) { if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 90; } else { tp_catcher_task->m_request_b = 270; } } else { if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 270; } else { tp_catcher_task->m_request_b = 90; } } std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl; return Error_code::SUCCESS; } //机器手调整到 准备把车放到地面的姿态 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上. tp_catcher_task->m_request_x = target_x; tp_catcher_task->m_request_y = target_y; if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 270; } else { tp_catcher_task->m_request_b = 90; } std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl; return Error_code::SUCCESS; } //机器手调整到 对接搬运器的姿态 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_catcher_task->m_lock); tp_catcher_task->m_request_key = dispatch_control_command_key; tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED); //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上. tp_catcher_task->m_request_x = target_x; tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2); if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE ) { tp_catcher_task->m_request_b = 270; } else { tp_catcher_task->m_request_b = 90; } //限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升. if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit ) { tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit; } else { tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2; tp_catcher_task->m_request_wheelbase = m_wheel_base; } tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE; std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_request_key << std::endl; std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl; std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //搬运器 准备开始, 需要同步任务单和设备真实数据. Error_manager Dispatch_process::carrier_ready_to_start(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_carrier_task->m_lock); tp_carrier_task->m_request_key = dispatch_control_command_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_x = tp_carrier->m_actual_x; tp_carrier_task->m_request_y = tp_carrier->m_actual_y; tp_carrier_task->m_request_z = tp_carrier->m_actual_z; tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y1; tp_carrier_task->m_request_y2 = tp_carrier->m_actual_y2; tp_carrier_task->m_request_clamp_motion = (Carrier_task::Clamp_motion)tp_carrier->m_actual_clamp_motion1; tp_carrier_task->m_request_joint_motion_x = (Carrier_task::Joint_motion)tp_carrier->m_actual_joint_motion_x1; tp_carrier_task->m_request_joint_motion_y = Carrier_task::Joint_motion::E_JOINT_NO_ACTION; tp_carrier_task->m_request_space_id = 0; tp_carrier_task->m_request_floor_id = 0; tp_carrier_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase; return Error_code::SUCCESS; } //搬运器调整到 正常待机的姿态(调节夹杆和轴距) Error_manager Dispatch_process::carrier_adjust_to_ready(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_carrier_task->m_lock); tp_carrier_task->m_request_key = dispatch_control_command_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_clamp_motion = Carrier_task::Clamp_motion::E_CLAMP_LOOSE; tp_carrier_task->m_request_wheelbase = m_wheel_base; tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance; tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase; std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //搬运器 移动x Error_manager Dispatch_process::carrier_move_x(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_x) { std::unique_lock t_lock2(tp_carrier_task->m_lock); tp_carrier_task->m_request_key = dispatch_control_command_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_x = target_x; std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_x = " << tp_carrier_task->m_request_x << std::endl; return Error_code::SUCCESS; } //搬运器 移动y Error_manager Dispatch_process::carrier_move_y(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_y) { std::unique_lock t_lock2(tp_carrier_task->m_lock); tp_carrier_task->m_request_key = dispatch_control_command_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_y = target_y; tp_carrier_task->m_request_wheelbase = m_wheel_base; tp_carrier_task->m_request_y1 = tp_carrier_task->m_request_y + tp_dispatch_coordinates->m_carrier_y_y1_distance; tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase; std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y = " << tp_carrier_task->m_request_y << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl; return Error_code::SUCCESS; } //搬运器 移动z Error_manager Dispatch_process::carrier_move_z(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target_z) { std::unique_lock t_lock2(tp_carrier_task->m_lock); tp_carrier_task->m_request_key = dispatch_control_command_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_z = target_z; std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_z = " << tp_carrier_task->m_request_z << std::endl; return Error_code::SUCCESS; } //搬运器 移动c轴 夹车杆 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_carrier_task->m_lock); tp_carrier_task->m_request_key = dispatch_control_command_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_clamp_motion = target; std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl; return Error_code::SUCCESS; } //搬运器调整 水平的交接 Error_manager Dispatch_process::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) { std::unique_lock t_lock2(tp_carrier_task->m_lock); tp_carrier_task->m_request_key = dispatch_control_command_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_joint_motion_x = target; std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_joint_motion_x = " << tp_carrier_task->m_request_joint_motion_x << std::endl; return Error_code::SUCCESS; } //搬运器 修正轴距 Error_manager Dispatch_process::carrier_adjust_wheel_base(std::string dispatch_control_command_key, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates) { std::unique_lock t_lock2(tp_carrier_task->m_lock); tp_carrier_task->m_request_key = dispatch_control_command_key; tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED); tp_carrier_task->m_request_wheelbase = m_wheel_base; tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance; tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase; std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_request_key << std::endl; std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_respons_key << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl; std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl; return Error_code::SUCCESS; }