diff --git a/.gitignore b/.gitignore index 50a54284..4dbb8f52 100644 --- a/.gitignore +++ b/.gitignore @@ -361,3 +361,4 @@ libmicroros_dx200_foxy/ # M+ build output *.out +/libmicroros_yrc1000_iron diff --git a/src/ActionServer_FJT.c b/src/ActionServer_FJT.c index 544b0411..a6aef4b9 100644 --- a/src/ActionServer_FJT.c +++ b/src/ActionServer_FJT.c @@ -252,7 +252,7 @@ rcl_ret_t Ros_ActionServer_FJT_Goal_Received(rclc_action_goal_handle_t* goal_han } else if (!bMotionReady) { - motomanErrorCode = Ros_Controller_GetNotReadySubcode(); + motomanErrorCode = Ros_Controller_GetNotReadySubcode(false); rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)motomanErrorCode)); } diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c index 8e845b02..2cf420b1 100644 --- a/src/ControllerStatusIO.c +++ b/src/ControllerStatusIO.c @@ -383,7 +383,7 @@ BOOL Ros_Controller_IsMotionReady() BOOL bMotionReady; #ifndef DUMMY_SERVO_MODE - bMotionReady = Ros_Controller_GetNotReadySubcode() == MOTION_READY; + bMotionReady = Ros_Controller_GetNotReadySubcode(false) == MOTION_READY; #else bMotionReady = Ros_Controller_IsOperating(); #endif @@ -423,16 +423,8 @@ BOOL Ros_Controller_IsAnyFaultActive() } -MotionNotReadyCode Ros_Controller_GetNotReadySubcode() +MotionNotReadyCode Ros_Controller_GetNotReadySubcode(bool ignoreTractableProblems) { - // Check alarm - if(Ros_Controller_IsAlarm()) - return MOTION_NOT_READY_ALARM; - - // Check error - if(Ros_Controller_IsError()) - return MOTION_NOT_READY_ERROR; - // Check e-stop if(Ros_Controller_IsEStop()) return MOTION_NOT_READY_ESTOP; @@ -441,24 +433,20 @@ MotionNotReadyCode Ros_Controller_GetNotReadySubcode() if(!Ros_Controller_IsPlay()) return MOTION_NOT_READY_NOT_PLAY; - // Check if in continuous cycle mode (Here due to being checked before starting servor power) - if (!Ros_Controller_IsContinuousCycle()) - return MOTION_NOT_READY_NOT_CONT_CYCLE_MODE; - #ifndef DUMMY_SERVO_MODE // Check remote if(!Ros_Controller_IsRemote()) return MOTION_NOT_READY_NOT_REMOTE; - - // Check servo power - if(!Ros_Controller_IsServoOn()) - return MOTION_NOT_READY_SERVO_OFF; #endif // Check hold if(Ros_Controller_IsHold()) return MOTION_NOT_READY_HOLD; + // Check alarm + if (Ros_Controller_IsAlarm()) + return MOTION_NOT_READY_ALARM; + // Check PFL active if (Ros_Controller_IsPflActive()) return MOTION_NOT_READY_PFL_ACTIVE; @@ -467,16 +455,34 @@ MotionNotReadyCode Ros_Controller_GetNotReadySubcode() if (Ros_Controller_IsMpIncMoveErrorActive()) return MOTION_NOT_READY_INC_MOVE_ERROR; - // Check operating - if(!Ros_Controller_IsOperating()) - return MOTION_NOT_READY_NOT_STARTED; + // Check error + if (Ros_Controller_IsError()) + return MOTION_NOT_READY_ERROR; - if(!Ros_Controller_MasterTaskIsJobName(g_nodeConfigSettings.inform_job_name)) + if (Ros_Controller_IsOperating() && !Ros_Controller_MasterTaskIsJobName(g_nodeConfigSettings.inform_job_name)) return MOTION_NOT_READY_OTHER_PROGRAM_RUNNING; - // Check ready I/O signal (should confirm wait) - if(!Ros_Controller_IsWaitingRos()) - return MOTION_NOT_READY_WAITING_ROS; + if (!ignoreTractableProblems) + { + + // Check if in continuous cycle mode (Here due to being checked before starting servo power) + if (!Ros_Controller_IsContinuousCycle()) + return MOTION_NOT_READY_NOT_CONT_CYCLE_MODE; +#ifndef DUMMY_SERVO_MODE + + // Check servo power + if (!Ros_Controller_IsServoOn()) + return MOTION_NOT_READY_SERVO_OFF; +#endif + + // Check operating + if (!Ros_Controller_IsOperating()) + return MOTION_NOT_READY_NOT_STARTED; + + // Check ready I/O signal (should confirm wait) + if (!Ros_Controller_IsWaitingRos()) + return MOTION_NOT_READY_WAITING_ROS; + } return MOTION_READY; } diff --git a/src/ControllerStatusIO.h b/src/ControllerStatusIO.h index 255928d7..22865a2d 100644 --- a/src/ControllerStatusIO.h +++ b/src/ControllerStatusIO.h @@ -129,7 +129,7 @@ extern BOOL Ros_Controller_IsPflActive(); extern BOOL Ros_Controller_IsMpIncMoveErrorActive(); extern BOOL Ros_Controller_IsAnyFaultActive(); extern BOOL Ros_Controller_MasterTaskIsJobName(const char* const jobName); -extern MotionNotReadyCode Ros_Controller_GetNotReadySubcode(); +extern MotionNotReadyCode Ros_Controller_GetNotReadySubcode(bool ignoreTractableProblems); //reset internal flag indicating whether PFL became active during a move extern void Ros_Controller_Reset_PflDuringRosMove(); diff --git a/src/ErrorHandling.c b/src/ErrorHandling.c index 3897c0d7..bea05f39 100644 --- a/src/ErrorHandling.c +++ b/src/ErrorHandling.c @@ -28,6 +28,7 @@ const char* const Ros_ErrorHandling_ErrNo_ToString(int errNo) case 0x3050: return "Out of range (ABSO data)"; case 0x3400: return "Cannot operate MASTER JOB"; case 0x3410: return "The JOB name is already registered in another task"; + case 0x3450: return "Servo power cannot be turned on"; case 0x4040: return "Specified JOB not found"; case 0x5200: return "Over data range"; default: return "Unspecified reason"; @@ -73,6 +74,10 @@ const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCo return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_NOT_CONT_CYCLE_MODE_STR; case MOTION_NOT_READY_MAJOR_ALARM: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_MAJOR_ALARM_STR; + case MOTION_NOT_READY_ECO_MODE: + return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_ECO_MODE_STR; + case MOTION_NOT_READY_SERVO_ON_TIMEOUT: + return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_SERVO_ON_TIMEOUT_STR; default: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_UNSPECIFIED_STR; } diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index 366a7ded..b018f844 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -33,6 +33,8 @@ typedef enum MOTION_NOT_READY_OTHER_TRAJ_MODE_ACTIVE = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_OTHER_TRAJ_MODE_ACTIVE, MOTION_NOT_READY_NOT_CONT_CYCLE_MODE = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_NOT_CONT_CYCLE_MODE, MOTION_NOT_READY_MAJOR_ALARM = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_MAJOR_ALARM, + MOTION_NOT_READY_ECO_MODE = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_ECO_MODE, + MOTION_NOT_READY_SERVO_ON_TIMEOUT = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_SERVO_ON_TIMEOUT, } MotionNotReadyCode; typedef enum diff --git a/src/MotionControl.c b/src/MotionControl.c index aab86d1b..b23c5162 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1355,15 +1355,16 @@ static STATUS Ros_Controller_DisableEcoMode() // // NOTE: only attempts to start job if necessary, does not reset errors, alarms. // Does attempt to enable servo power (if not on) -// Does attempt to set the cycle mode to continuous (if not set) +// Does attempt to set the cycle mode to AUTO (if not set) //----------------------------------------------------------------------- -BOOL Ros_MotionControl_StartMotionMode(MOTION_MODE mode) +MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_runtime_c__String* responseMessage) { int ret; MP_STD_RSP_DATA rData; MP_START_JOB_SEND_DATA sStartData; - int checkCount; - int grpNo; + int checkCount, grpNo, alarmcode; + MotionNotReadyCode motion_readiness_code; + char output[MOTION_START_ERROR_MESSAGE_LENGTH] = { 0 }; Ros_Debug_BroadcastMsg("%s: enter", __func__); @@ -1371,63 +1372,66 @@ BOOL Ros_MotionControl_StartMotionMode(MOTION_MODE mode) Ros_MotionControl_ActiveMotionMode != mode) { Ros_Debug_BroadcastMsg("Another trajectory mode (%d) is already active.", mode); - return FALSE; + return MOTION_NOT_READY_OTHER_TRAJ_MODE_ACTIVE; } - // Update status Ros_Controller_IoStatusUpdate(); - // Check if already in the proper mode - if(Ros_Controller_IsMotionReady()) + if (Ros_Controller_IsMotionReady()) { Ros_Debug_BroadcastMsg("Already active"); - return TRUE; + return MOTION_READY; } + motion_readiness_code = Ros_Controller_GetNotReadySubcode(true); -#ifndef DUMMY_SERVO_MODE - // Check for condition that need operator manual intervention - if(!Ros_Controller_IsRemote()) - { - Ros_Debug_BroadcastMsg("Not remote, can't enable trajectory mode"); - return FALSE; - } -#endif - - if (Ros_Controller_IsAnyFaultActive()) + //Return with code if any of the problems are intractable + switch (motion_readiness_code) { - Ros_Debug_BroadcastMsg("Controller is in a fault state. Please call /reset_error"); - return FALSE; - } - - // Check if currently in operation, we don't want to interrupt current operation - if (Ros_Controller_IsOperating()) - { - if (!Ros_Controller_MasterTaskIsJobName(g_nodeConfigSettings.inform_job_name)) - { + case MOTION_NOT_READY_ESTOP: + case MOTION_NOT_READY_NOT_PLAY: + case MOTION_NOT_READY_NOT_REMOTE: + case MOTION_NOT_READY_HOLD: + return motion_readiness_code; + case MOTION_NOT_READY_ERROR: + //the responseMessage gets populated only in this case or for other MOTION_NOT_READY_ERROR + //returns because it has the most information about the error in this situtation. + //in other cases, the responseMessage is populated upstream/with only the MotionNotReadyCode + alarmcode = Ros_Controller_GetAlarmCode(); + snprintf(output, MOTION_START_ERROR_MESSAGE_LENGTH, "%s: '%s' (0x%04X)", + Ros_ErrorHandling_MotionNotReadyCode_ToString(MOTION_NOT_READY_ERROR), + Ros_ErrorHandling_ErrNo_ToString(alarmcode), + alarmcode); + Ros_Debug_BroadcastMsg(output); + rosidl_runtime_c__String__assign(responseMessage, output); + Ros_Debug_BroadcastMsg("Controller is in a fault state. Please call /reset_error"); + return motion_readiness_code; + case MOTION_NOT_READY_ALARM: + case MOTION_NOT_READY_PFL_ACTIVE: + case MOTION_NOT_READY_INC_MOVE_ERROR: + Ros_Debug_BroadcastMsg("Controller is in a fault state. Please call /reset_error"); + return motion_readiness_code; + case MOTION_NOT_READY_OTHER_PROGRAM_RUNNING: Ros_Debug_BroadcastMsg("%s: robot is running another job (expected: '%s')", __func__, g_nodeConfigSettings.inform_job_name); - goto updateStatus; - } - else - { - //the current call to StarTrajMode is likely intended to get the - //servos out of eco mode. In that case, servo power will be turned - //ON again below, but in order for things to work, we need to stop - //the currently running job first. It will be (re)started at the - //end of StartTrajMode. - MP_HOLD_SEND_DATA holdSendData; - MP_STD_RSP_DATA stdRspData; - - holdSendData.sHold = ON; - mpHold(&holdSendData, &stdRspData); - - Ros_Sleep(MOTION_START_CHECK_PERIOD); - - holdSendData.sHold = OFF; - mpHold(&holdSendData, &stdRspData); - - Ros_Sleep(MOTION_START_CHECK_PERIOD); - } + return MOTION_NOT_READY_OTHER_PROGRAM_RUNNING; + default: //Only here to get rid of warnings while compiling... + ; + } + if (Ros_Controller_IsOperating()) + { + //the current call to StarTrajMode is likely intended to get the + //servos out of eco mode. In that case, servo power will be turned + //ON again below, but in order for things to work, we need to stop + //the currently running job first. It will be (re)started at the + //end of StartTrajMode. + MP_HOLD_SEND_DATA holdSendData; + MP_STD_RSP_DATA stdRspData; + holdSendData.sHold = ON; + mpHold(&holdSendData, &stdRspData); + Ros_Sleep(MOTION_START_CHECK_PERIOD); + holdSendData.sHold = OFF; + mpHold(&holdSendData, &stdRspData); + Ros_Sleep(MOTION_START_CHECK_PERIOD); } // Check if in continous cycle mode @@ -1439,13 +1443,18 @@ BOOL Ros_MotionControl_StartMotionMode(MOTION_MODE mode) bzero(&rData, sizeof(rData)); sCycleData.sCycle = MP_CYCLE_MODE_AUTO; ret = mpSetCycle(&sCycleData, &rData); - if( (ret != 0) || (rData.err_no != 0) ) + if ((ret != 0) || (rData.err_no != 0)) { - Ros_Debug_BroadcastMsg( - "Can't set cycle mode to continuous because: '%s' (0x%04X)", - Ros_ErrorHandling_ErrNo_ToString(rData.err_no), rData.err_no); + snprintf(output, MOTION_START_ERROR_MESSAGE_LENGTH, + "%s: Can't set cycle mode to AUTO because: '%s' (0x%04X)", + Ros_ErrorHandling_MotionNotReadyCode_ToString(MOTION_NOT_READY_ERROR), + Ros_ErrorHandling_ErrNo_ToString(rData.err_no), + rData.err_no); + Ros_Debug_BroadcastMsg(output); + rosidl_runtime_c__String__assign(responseMessage, output); + mpSetAlarm(ALARM_OPERATION_FAIL, "Set job-cycle to AUTO", SUBCODE_OPERATION_SET_CYCLE); - return FALSE; + return MOTION_NOT_READY_ERROR; } Ros_Sleep(g_Ros_Controller.interpolPeriod); //give CIO time to potentially overwrite the cycle (Ladder scan time is smaller than the interpolPeriod) @@ -1455,21 +1464,20 @@ BOOL Ros_MotionControl_StartMotionMode(MOTION_MODE mode) { Ros_Debug_BroadcastMsg("Can't set cycle mode. Check CIOPRG.LST for OUT #40050 - #40052"); mpSetAlarm(ALARM_OPERATION_FAIL, "Set job-cycle to AUTO", SUBCODE_OPERATION_SET_CYCLE); - return false; + return MOTION_NOT_READY_NOT_CONT_CYCLE_MODE; } } #ifndef DUMMY_SERVO_MODE - // Servo On - if(Ros_Controller_IsServoOn() == FALSE) + if (!Ros_Controller_IsServoOn()) { // if servos are "off" due to eco mode, attempt to disable it if (Ros_Controller_IsEcoMode()) { if (Ros_Controller_DisableEcoMode() == NG) { - Ros_Debug_BroadcastMsg("%s: couldn't disable eco mode"); - goto updateStatus; + Ros_Debug_BroadcastMsg("Couldn't disable eco mode"); + return MOTION_NOT_READY_ECO_MODE; } } @@ -1480,10 +1488,10 @@ BOOL Ros_MotionControl_StartMotionMode(MOTION_MODE mode) bzero(&rData, sizeof(rData)); sServoData.sServoPower = 1; // ON ret = mpSetServoPower(&sServoData, &rData); - if( (ret == 0) && (rData.err_no ==0) ) + if ((ret == 0) && (rData.err_no == 0)) { // wait for the Servo On confirmation - for(checkCount=0; checkCountresult_code.value = MOTION_READY; rosidl_runtime_c__String__assign(&response->message, ""); - if (!Ros_MotionControl_StartMotionMode(MOTION_MODE_POINTQUEUE)) + MotionNotReadyCode motion_result_code = Ros_MotionControl_StartMotionMode(MOTION_MODE_POINTQUEUE, &response->message); + if (motion_result_code != MOTION_READY) { // update response - response->result_code.value = Ros_Controller_GetNotReadySubcode(); + response->result_code.value = motion_result_code; - if (response->result_code.value == MOTION_READY || Ros_MotionControl_IsMotionMode_Trajectory() || Ros_MotionControl_IsMotionMode_RawStreaming()) - { - //Motion is ready, but the StartPointQueueMode service failed - //because it's already in a different mode. - response->result_code.value = MOTION_NOT_READY_OTHER_TRAJ_MODE_ACTIVE; - rosidl_runtime_c__String__assign(&response->message, - "Another motion mode is already active. Please call 'stop_traj_mode' service and try again."); - } - else - { + //If it is a MOTION_NOT_READY_ERROR, then the string was already populated in the Ros_MotionControl_StartMotionMode function + if (motion_result_code != MOTION_NOT_READY_ERROR) { // map to human readable string rosidl_runtime_c__String__assign(&response->message, Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)response->result_code.value)); diff --git a/src/ServiceStartTrajMode.c b/src/ServiceStartTrajMode.c index bce5062a..5f81ae5a 100644 --- a/src/ServiceStartTrajMode.c +++ b/src/ServiceStartTrajMode.c @@ -53,21 +53,14 @@ void Ros_ServiceStartTrajMode_Trigger(const void* request_msg, void* response_ms response->result_code.value = MOTION_READY; rosidl_runtime_c__String__assign(&response->message, ""); - if (!Ros_MotionControl_StartMotionMode(MOTION_MODE_TRAJECTORY)) + MotionNotReadyCode motion_result_code = Ros_MotionControl_StartMotionMode(MOTION_MODE_TRAJECTORY, &response->message); + if (motion_result_code != MOTION_READY) { // update response - response->result_code.value = Ros_Controller_GetNotReadySubcode(); + response->result_code.value = motion_result_code; - if (response->result_code.value == MOTION_READY || Ros_MotionControl_IsMotionMode_PointQueue() || Ros_MotionControl_IsMotionMode_RawStreaming()) - { - //Motion is ready, but the StartTrajMode service failed - //because it's already in a different mode. - response->result_code.value = MOTION_NOT_READY_OTHER_TRAJ_MODE_ACTIVE; - rosidl_runtime_c__String__assign(&response->message, - "Another motion mode is already active. Please call 'stop_traj_mode' service and try again."); - } - else - { + //If it is a MOTION_NOT_READY_ERROR, then the string was already populated in the Ros_MotionControl_StartMotionMode function + if (motion_result_code != MOTION_NOT_READY_ERROR) { // map to human readable string rosidl_runtime_c__String__assign(&response->message, Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)response->result_code.value));