Restructuring error reporting in start_traj_mode and start_point_queue_mode

.gitignore

@@ -361,3 +361,4 @@ libmicroros_dx200_foxy/
 
 # M+ build output
 *.out
+/libmicroros_yrc1000_iron

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));
         }

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;
 }

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();

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;
     }

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